-
Notifications
You must be signed in to change notification settings - Fork 103
/
join.go
205 lines (190 loc) · 6.93 KB
/
join.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
// Package align defines the camera models that are used to align a color camera's output with a depth camera's output,
// in order to make point clouds.
package align
import (
"context"
"fmt"
"image"
"github.com/pkg/errors"
"go.opencensus.io/trace"
"go.uber.org/multierr"
"go.viam.com/rdk/components/camera"
"go.viam.com/rdk/gostream"
"go.viam.com/rdk/logging"
"go.viam.com/rdk/pointcloud"
"go.viam.com/rdk/resource"
"go.viam.com/rdk/rimage"
"go.viam.com/rdk/rimage/transform"
)
var joinModel = resource.DefaultModelFamily.WithModel("join_color_depth")
func init() {
resource.RegisterComponent(camera.API, joinModel,
resource.Registration[camera.Camera, *joinConfig]{
Constructor: func(ctx context.Context, deps resource.Dependencies,
conf resource.Config, logger logging.Logger,
) (camera.Camera, error) {
newConf, err := resource.NativeConfig[*joinConfig](conf)
if err != nil {
return nil, err
}
colorName := newConf.Color
color, err := camera.FromDependencies(deps, colorName)
if err != nil {
return nil, fmt.Errorf("no color camera (%s): %w", colorName, err)
}
depthName := newConf.Depth
depth, err := camera.FromDependencies(deps, depthName)
if err != nil {
return nil, fmt.Errorf("no depth camera (%s): %w", depthName, err)
}
src, err := newJoinColorDepth(ctx, color, depth, newConf, logger)
if err != nil {
return nil, err
}
return camera.FromVideoSource(conf.ResourceName(), src, logger), nil
},
})
}
// joinConfig is the attribute struct for aligning.
type joinConfig struct {
ImageType string `json:"output_image_type"`
Color string `json:"color_camera_name"`
Depth string `json:"depth_camera_name"`
CameraParameters *transform.PinholeCameraIntrinsics `json:"intrinsic_parameters,omitempty"`
Debug bool `json:"debug,omitempty"`
DistortionParameters *transform.BrownConrady `json:"distortion_parameters,omitempty"`
}
func (cfg *joinConfig) Validate(path string) ([]string, error) {
var deps []string
if cfg.Color == "" {
return nil, resource.NewConfigValidationFieldRequiredError(path, "color_camera_name")
}
deps = append(deps, cfg.Color)
if cfg.Depth == "" {
return nil, resource.NewConfigValidationFieldRequiredError(path, "depth_camera_name")
}
if cfg.CameraParameters != nil {
if cfg.CameraParameters.Height < 0 || cfg.CameraParameters.Width < 0 {
return nil, fmt.Errorf(
"got illegal negative dimensions for width_px and height_px (%d, %d) fields set in intrinsic_parameters"+
" for join_color_depth camera",
cfg.CameraParameters.Width, cfg.CameraParameters.Height)
}
}
deps = append(deps, cfg.Depth)
return deps, nil
}
// joinColorDepth takes a color and depth image source and aligns them together.
type joinColorDepth struct {
color, depth gostream.VideoStream
colorName, depthName string
underlyingCamera camera.VideoSource
projector transform.Projector
imageType camera.ImageType
debug bool
logger logging.Logger
}
// newJoinColorDepth creates a gostream.VideoSource that aligned color and depth channels.
func newJoinColorDepth(ctx context.Context, color, depth camera.VideoSource, conf *joinConfig, logger logging.Logger,
) (camera.VideoSource, error) {
imgType := camera.ImageType(conf.ImageType)
// get intrinsic parameters from config, or from the underlying camera
var camParams *transform.PinholeCameraIntrinsics
if conf.CameraParameters == nil {
if imgType == camera.DepthStream {
props, err := depth.Properties(ctx)
if err == nil {
camParams = props.IntrinsicParams
}
} else {
props, err := color.Properties(ctx)
if err == nil {
camParams = props.IntrinsicParams
}
}
} else {
camParams = conf.CameraParameters
}
err := camParams.CheckValid()
if err != nil {
if conf.CameraParameters != nil {
return nil, errors.Wrap(err, "error in the intrinsic_parameters field of the attributes")
}
return nil, errors.Wrap(err, "error in the intrinsic parameters of the underlying camera")
}
videoSrc := &joinColorDepth{
colorName: conf.Color,
depthName: conf.Depth,
color: gostream.NewEmbeddedVideoStream(color),
depth: gostream.NewEmbeddedVideoStream(depth),
projector: camParams,
imageType: imgType,
debug: conf.Debug,
logger: logger,
}
if conf.Color == conf.Depth { // store the underlying VideoSource for an Images call
videoSrc.underlyingCamera = color
}
cameraModel := camera.NewPinholeModelWithBrownConradyDistortion(conf.CameraParameters, conf.DistortionParameters)
return camera.NewVideoSourceFromReader(
ctx,
videoSrc,
&cameraModel,
imgType,
)
}
// Read returns the next image from either the color or depth camera..
// imageType parameter will determine which channel gets streamed.
func (jcd *joinColorDepth) Read(ctx context.Context) (image.Image, func(), error) {
ctx, span := trace.StartSpan(ctx, "align::joinColorDepth::Read")
defer span.End()
switch jcd.imageType {
case camera.ColorStream, camera.UnspecifiedStream:
return jcd.color.Next(ctx)
case camera.DepthStream:
return jcd.depth.Next(ctx)
default:
return nil, nil, camera.NewUnsupportedImageTypeError(jcd.imageType)
}
}
func (jcd *joinColorDepth) NextPointCloud(ctx context.Context) (pointcloud.PointCloud, error) {
ctx, span := trace.StartSpan(ctx, "align::joinColorDepth::NextPointCloud")
defer span.End()
if jcd.projector == nil {
return nil, transform.NewNoIntrinsicsError("no intrinsic_parameters in camera attributes")
}
if jcd.colorName == jcd.depthName {
return jcd.nextPointCloudFromImages(ctx)
}
col, dm := camera.SimultaneousColorDepthNext(ctx, jcd.color, jcd.depth)
if col == nil {
return nil, errors.Errorf("could not get color image from source camera %q for join_color_depth camera", jcd.colorName)
}
if dm == nil {
return nil, errors.Errorf("could not get depth image from source camera %q for join_color_depth camera", jcd.depthName)
}
return jcd.projector.RGBDToPointCloud(rimage.ConvertImage(col), dm)
}
func (jcd *joinColorDepth) nextPointCloudFromImages(ctx context.Context) (pointcloud.PointCloud, error) {
imgs, _, err := jcd.underlyingCamera.Images(ctx)
if err != nil {
return nil, errors.Wrapf(err, "could not call Images on underlying camera %q", jcd.colorName)
}
var col *rimage.Image
var dm *rimage.DepthMap
for _, img := range imgs {
if img.SourceName == "color" {
col = rimage.ConvertImage(img.Image)
}
if img.SourceName == "depth" {
dm, err = rimage.ConvertImageToDepthMap(ctx, img.Image)
if err != nil {
return nil, errors.Wrap(err, "image called 'depth' from Images not actually a depth map")
}
}
}
return jcd.projector.RGBDToPointCloud(col, dm)
}
func (jcd *joinColorDepth) Close(ctx context.Context) error {
return multierr.Combine(jcd.color.Close(ctx), jcd.depth.Close(ctx))
}