Drake
rgbd_camera.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <map>
4 #include <memory>
5 #include <string>
6 #include <utility>
7 #include <vector>
8 
9 #include <Eigen/Dense>
10 
20 
21 namespace drake {
22 namespace systems {
23 namespace sensors {
24 /// An RGB-D camera system that provides RGB, depth and label images using
25 /// visual elements of RigidBodyTree.
26 /// Its image resolution is fixed at VGA (640 x 480 pixels) for all three
27 /// images. The default depth sensing range is from 0.5 m to 5.0 m.
28 ///
29 /// Let `W` be the world coordinate system. In addition to `W`, there are three
30 /// more coordinate systems that are associated with an RgbdCamera. They are
31 /// defined as follows:
32 ///
33 /// * `B` - the camera's base coordinate system: X-forward, Y-left, and Z-up.
34 ///
35 /// * `C` - the camera's color sensor's optical coordinate system: `X-right`,
36 /// `Y-down` and `Z-forward`.
37 ///
38 /// * `D` - the camera's depth sensor's optical coordinate system: `X-right`,
39 /// `Y-down` and `Z-forward`.
40 ///
41 /// The origins of `C` and `D` (i.e., `Co` and `Do`, respectively) are offset
42 /// from `B`'s origin (`Bo`) by 0 m in `B`'s X-axis, +0.02 m in `B`'s Y-axis,
43 /// and 0 m in `B`'s Z axis. Since `C` and `D` are coincident, the depth image
44 /// is a "registered depth image" for the RGB image. No disparity between the
45 /// RGB and depth images are modeled. For more details about the poses of `C`
46 /// and `D`, see the class documentation of CameraInfo.
47 ///
48 /// Output image format:
49 /// - The RGB image has four channels in the following order: red, green
50 /// blue, alpha. Each channel is represented by a uint8_t.
51 ///
52 /// - The depth image has a depth channel represented by a float. The value
53 /// stored in the depth channel holds *the Z value in `D`.* Note that this
54 /// is different from the range data used by laser range finders (like that
55 /// provided by DepthSensor) in which the depth value represents the
56 /// distance from the sensor origin to the object's surface.
57 ///
58 /// - The label image has single channel represented by a int16_t. The value
59 /// stored in the channel holds a model ID which corresponds to an object
60 /// in the scene. For the pixels corresponding to no body, namely the sky
61 /// and the flat terrain, we assign Label::kNoBody and Label::kFlatTerrain,
62 /// respectively.
63 ///
64 /// @ingroup sensor_systems
65 class RgbdCamera final : public LeafSystem<double> {
66  public:
68 
69  /// Converts a depth image obtained from RgbdCamera to a point cloud. If a
70  /// pixel in the depth image has NaN depth value, all the `(x, y, z)` values
71  /// in the converted point will be NaN.
72  /// Similarly, if a pixel has either InvalidDepth::kTooFar or
73  /// InvalidDepth::kTooClose, the converted point will be
74  /// InvalidDepth::kTooFar. Note that this matches the convention used by the
75  /// Point Cloud Library (PCL).
76  ///
77  /// @param[in] depth_image The input depth image obtained from RgbdCamera.
78  ///
79  /// @param[in] camera_info The input camera info which is used for conversion.
80  ///
81  /// @param[out] point_cloud The pointer of output point cloud.
82  // TODO(kunimatsu-tri) Use drake::perception::PointCloud instead of
83  // Eigen::Matrix3Xf and create new constants there instead of reusing
84  // InvalidDepth.
85  // TODO(kunimatsu-tri) Move this to drake::perception.
86  static void ConvertDepthImageToPointCloud(const ImageDepth32F& depth_image,
87  const CameraInfo& camera_info,
88  Eigen::Matrix3Xf* point_cloud);
89 
90  /// A constructor that uses the default RgbdRendererVTK renderer and
91  /// defines `B` using Euler angles.
92  /// The pose of %RgbdCamera will be fixed to the world coordinate system
93  /// throughout the simulation.
94  ///
95  /// @param name The name of the RgbdCamera. This can be any value, but
96  /// should typically be unique among all sensors attached to a particular
97  /// model instance.
98  ///
99  /// @param tree The RigidBodyTree containing the geometric description of the
100  /// world. The life span of this parameter must exceed that of this class's
101  /// instance. The maximum number of bodies in the `tree` must be less than
102  /// 1536 based on the number of the colors used for the label image, otherwise
103  /// an exception will be thrown.
104  ///
105  /// @param position The x-y-z position of `B` in `W`. This defines the
106  /// translation component of `X_WB`.
107  ///
108  /// @param orientation The roll-pitch-yaw orientation of `B` in `W`. This
109  /// defines the orientation component of `X_WB`.
110  ///
111  /// @param z_near The minimum depth distance RgbdCamera can measure.
112  /// The default is 0.5 meters.
113  ///
114  /// @param z_far The maximum depth distance RgbdCamera can measure.
115  /// The default is 5 meters.
116  ///
117  /// @param fov_y The RgbdCamera's vertical field of view.
118  /// The default is PI / 4.
119  ///
120  /// @param show_window A flag for showing a visible window. If this is false,
121  /// offscreen rendering is executed. This is useful for debugging purposes.
122  /// The default is true.
123  ///
124  /// @param flat_terrain A flag to add a flat terrain at z = 0 in the world
125  /// coordinate system. The default is true.
126  ///
127  /// @throws std::logic_error When the number of rigid bodies in the scene
128  /// exceeds the maximum limit 1535.
129  // TODO(kunimatsu-tri) Use the information from RigidBodyTree for rendering
130  // the flat terrain.
131  RgbdCamera(const std::string& name,
133  const Eigen::Vector3d& position,
134  const Eigen::Vector3d& orientation,
135  double z_near = 0.5,
136  double z_far = 5.0,
137  double fov_y = M_PI_4,
138  bool show_window = RenderingConfig::kDefaultShowWindow,
141  bool flat_terrain = true);
142 
143  /// A constructor that uses the default RgbdRendererVTK renderer and
144  /// defines `B` using a RigidBodyFrame.
145  /// The pose of %RgbdCamera is fixed to a user-defined frame and will be
146  /// updated during the simulation.
147  ///
148  /// @param name The name of the RgbdCamera. This can be any value, but
149  /// should typically be unique among all sensors attached to a particular
150  /// model instance.
151  ///
152  /// @param tree The RigidBodyTree containing the geometric description of the
153  /// world. The life span of this parameter must exceed that of this class's
154  /// instance. The maximum number of bodies in the `tree` must be less than
155  /// 1536 based on the number of the colors used for the label image, otherwise
156  /// an exception will be thrown.
157  ///
158  /// @param frame The frame in @tree to which this camera is attached.
159  ///
160  /// @param z_near The minimum depth distance RgbdCamera can measure.
161  /// The default is 0.5 meters.
162  ///
163  /// @param z_far The maximum depth distance RgbdCamera can measure.
164  /// The default is 5 meters.
165  ///
166  /// @param fov_y The RgbdCamera's vertical field of view.
167  /// The default is PI / 4.
168  ///
169  /// @param show_window A flag for showing a visible window. If this is false,
170  /// offscreen rendering is executed. This is useful for debugging purposes.
171  /// The default is true.
172  ///
173  /// @param flat_terrain A flag to add a flat terrain at z = 0 in the world
174  /// coordinate system. The default is true.
175  ///
176  /// @throws std::logic_error When the number of rigid bodies in the scene
177  /// exceeds the maximum limit 1535.
178  // TODO(kunimatsu-tri) Use the information from RigidBodyTree for rendering
179  // the flat terrain.
180  RgbdCamera(const std::string& name,
181  const RigidBodyTree<double>& tree,
183  double z_near = 0.5,
184  double z_far = 5.0,
185  double fov_y = M_PI_4,
186  bool show_window = RenderingConfig::kDefaultShowWindow,
189  bool flat_terrain = true);
190 
191  /// A constructor that takes a renderer and defines `B` using Euler angles.
192  /// The pose of %RgbdCamera will be fixed to the world coordinate system
193  /// throughout the simulation.
194  ///
195  /// @param name The name of the RgbdCamera. This can be any value, but
196  /// should typically be unique among all sensors attached to a particular
197  /// model instance.
198  ///
199  /// @param tree The RigidBodyTree containing the geometric description of the
200  /// world. The life span of this parameter must exceed that of this class's
201  /// instance. The maximum number of bodies in the `tree` must be less than
202  /// 1536 based on the number of the colors used for the label image, otherwise
203  /// an exception will be thrown.
204  ///
205  /// @param position The x-y-z position of `B` in `W`. This defines the
206  /// translation component of `X_WB`.
207  ///
208  /// @param orientation The roll-pitch-yaw orientation of `B` in `W`. This
209  /// defines the orientation component of `X_WB`.
210  ///
211  /// @param renderer The rendering backend to render images for the camera.
212  /// Camera configuration parameters are obtained from the RenderingConfig
213  /// settings in the renderer. The renderer will be owned by this camera.
214  ///
215  /// @throws std::logic_error When the number of rigid bodies in the scene
216  /// exceeds the maximum limit 1535.
217  RgbdCamera(const std::string& name,
218  const RigidBodyTree<double>& tree,
219  const Eigen::Vector3d& position,
220  const Eigen::Vector3d& orientation,
221  std::unique_ptr<RgbdRenderer> renderer,
222  bool flat_terrain = true);
223 
224  /// A constructor that takes a renderer and defines `B` using a
225  /// RigidBodyFrame.
226  /// The pose of %RgbdCamera is fixed to a user-defined frame and will be
227  /// updated during the simulation.
228  ///
229  /// @param name The name of the RgbdCamera. This can be any value, but
230  /// should typically be unique among all sensors attached to a particular
231  /// model instance.
232  ///
233  /// @param tree The RigidBodyTree containing the geometric description of the
234  /// world. The life span of this parameter must exceed that of this class's
235  /// instance. The maximum number of bodies in the `tree` must be less than
236  /// 1536 based on the number of the colors used for the label image, otherwise
237  /// an exception will be thrown.
238  ///
239  /// @param frame The frame in @tree to which this camera is attached.
240  ///
241  /// @param renderer The rendering backend to render images for the camera.
242  /// Camera configuration parameters are obtained from the RenderingConfig
243  /// settings in the renderer. The renderer will be owned by this camera.
244  ///
245  /// @throws std::logic_error When the number of rigid bodies in the scene
246  /// exceeds the maximum limit 1535.
247  RgbdCamera(const std::string& name,
248  const RigidBodyTree<double>& tree,
249  const RigidBodyFrame<double>& frame,
250  std::unique_ptr<RgbdRenderer> renderer,
251  bool flat_terrain = true);
252 
253  ~RgbdCamera() = default;
254 
255  /// Sets and initializes RgbdRenderer. The viewpoint of renderer will be
256  /// appropriately handled inside this function.
257  /// Note that if any visual element is registered with renderer before
258  /// this method is called, its behavior will not be guaranteed.
260  "`ResetRenderer` will be removed after 2018/10/01; please update your "
261  "code to specify the `RgbdRenderer` instance at construction time.")
262  void ResetRenderer(std::unique_ptr<RgbdRenderer> renderer) {
263  renderer_ = std::move(renderer);
264  InitRenderer();
265  // This is needed only for camera_fixed_ is true since UpdateViewpoint()
266  // will be called while rendering related output ports are evaluated
267  // if it is false.
268  if (camera_fixed_) {
269  renderer_->UpdateViewpoint(X_WB_initial_ * X_BC_);
270  }
271  }
272 
273  /// Reterns mutable renderer.
274  RgbdRenderer& mutable_renderer() { return *renderer_; }
275 
276  /// Reterns the color sensor's info.
277  const CameraInfo& color_camera_info() const { return color_camera_info_; }
278 
279  /// Reterns the depth sensor's info.
280  const CameraInfo& depth_camera_info() const { return depth_camera_info_; }
281 
282  /// Returns `X_BC`.
283  const Eigen::Isometry3d& color_camera_optical_pose() const { return X_BC_; }
284 
285  /// Returns `X_BD`.
286  const Eigen::Isometry3d& depth_camera_optical_pose() const { return X_BD_; }
287 
288  /// Returns the RigidBodyFrame to which this RgbdCamera is attached.
289  ///
290  /// @throws std::logic_error When RgbdCamera is instantiated as a fixed
291  /// camera.
292  const RigidBodyFrame<double>& frame() const {
293  if (camera_fixed_)
294  throw std::logic_error(
295  "RgbdCamera::frame(): "
296  "A fixed camera does not have an associated frame.");
297  return frame_;
298  }
299 
300  /// Returns the RigidBodyTree to which this RgbdCamera is attached.
301  const RigidBodyTree<double>& tree() const { return tree_; }
302 
303  /// Returns the vector valued input port that takes a vector of `q, v`
304  /// corresponding to the positions and velocities associated with the
305  /// RigidBodyTree.
306  const InputPort<double>& state_input_port() const;
307 
308  /// Returns the abstract valued output port that contains a RGBA image of the
309  /// type ImageRgba8U.
311 
312  /// Returns the abstract valued output port that contains an ImageDepth32F.
314 
315  /// Returns the abstract valued output port that contains an label image of
316  /// the type ImageLabel16I.
318 
319  /// Returns the vector valued output port that contains a PoseVector.
321 
322  private:
323  void InitPorts(const std::string& name);
324 
325  void InitRenderer();
326 
327  // These are the calculator methods for the four output ports.
328  void OutputColorImage(const Context<double>& context,
329  ImageRgba8U* color_image) const;
330  void OutputDepthImage(const Context<double>& context,
331  ImageDepth32F* depth_image) const;
332  void OutputLabelImage(const Context<double>& context,
333  ImageLabel16I* label_image) const;
334  void OutputPoseVector(const Context<double>& context,
335  rendering::PoseVector<double>* pose_vector) const;
336 
337  // TODO(sherm1) This should be the calculator for a cache entry containing
338  // the VTK update that must be valid before outputting any image info. For
339  // now it has to be repeated before each image output port calculation.
340  void UpdateModelPoses(const BasicVector<double>& input_vector) const;
341 
342  const InputPort<double>* state_input_port_{};
343  const OutputPort<double>* color_image_port_{};
344  const OutputPort<double>* depth_image_port_{};
345  const OutputPort<double>* label_image_port_{};
346  const OutputPort<double>* camera_base_pose_port_{};
347 
348  const RigidBodyTree<double>& tree_;
349  const RigidBodyFrame<double> frame_;
351  std::map<int, std::vector<VisualIndex>> body_visual_indices_map_;
352  const bool camera_fixed_;
353  const bool flat_terrain_;
354  const CameraInfo color_camera_info_;
355  const CameraInfo depth_camera_info_;
356  const Eigen::Isometry3d X_WB_initial_;
357  // The color sensor's origin (`Co`) is offset by 0.02 m on the Y axis of
358  // the RgbdCamera's base coordinate system (`B`).
359  // TODO(kunimatsu-tri) Add support for arbitrary relative pose.
360  // TODO(kunimatsu-tri) Change the X_BD_ to be different from X_BC_ when
361  // it's needed.
362  const Eigen::Isometry3d X_BC_{Eigen::Translation3d(0., 0.02, 0.) *
363  (Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()) *
364  Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()))};
365  // The depth sensor's origin (`Do`) is offset by 0.02 m on the Y axis of
366  // the RgbdCamera's base coordinate system (`B`).
367  const Eigen::Isometry3d X_BD_{Eigen::Translation3d(0., 0.02, 0.) *
368  (Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()) *
369  Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()))};
370 
371  std::unique_ptr<RgbdRenderer> renderer_;
372 };
373 
374 /**
375  * Wraps a continuous RgbdCamera with zero order holds to have it function as
376  * a discrete sensor.
377  */
378 class RgbdCameraDiscrete final : public systems::Diagram<double> {
379  public:
381 
382  static constexpr double kDefaultPeriod = 1. / 30;
383 
384  /// Constructs a diagram containing a (non-registered) RgbdCamera that will
385  /// update at a given rate.
386  /// @param period
387  /// Update period (sec).
388  /// @param render_label_image
389  /// If true, renders label image (which requires additional overhead). If
390  /// false, `label_image_output_port` will raise an error if called.
391  RgbdCameraDiscrete(std::unique_ptr<RgbdCamera> camera,
392  double period = kDefaultPeriod,
393  bool render_label_image = true);
394 
395  /// Returns reference to RgbdCamera intsance.
396  const RgbdCamera& camera() const { return *camera_; }
397 
398  /// Returns reference to RgbdCamera intsance.
399  RgbdCamera& mutable_camera() { return *camera_; }
400 
401  /// Returns update period for discrete camera.
402  double period() const { return period_; }
403 
404  /// @see RgbdCamera::state_input_port().
406  return get_input_port(input_port_state_);
407  }
408 
409  /// @see RgbdCamera::color_image_output_port().
411  return get_output_port(output_port_color_image_);
412  }
413 
414  /// @see RgbdCamera::depth_image_output_port().
416  return get_output_port(output_port_depth_image_);
417  }
418 
419  /// @see RgbdCamera::label_image_output_port().
421  return get_output_port(output_port_label_image_);
422  }
423 
424  /// @see RgbdCamera::camera_base_pose_output_port().
426  return get_output_port(output_port_pose_);
427  }
428 
429  private:
430  RgbdCamera* const camera_{};
431  const double period_{};
432 
433  int input_port_state_{-1};
434  int output_port_color_image_{-1};
435  int output_port_depth_image_{-1};
436  int output_port_label_image_{-1};
437  int output_port_pose_{-1};
438 };
439 
440 } // namespace sensors
441 } // namespace systems
442 } // namespace drake
const RgbdCamera & camera() const
Returns reference to RgbdCamera intsance.
Definition: rgbd_camera.h:396
static constexpr bool kDefaultShowWindow
Default value for show_window.
Definition: rgbd_renderer.h:53
const systems::OutputPort< double > & label_image_output_port() const
Definition: rgbd_camera.h:420
static constexpr int kDefaultHeight
Default value for height.
Definition: rgbd_renderer.h:37
Definition: automotive_demo.cc:105
const OutputPort< double > & depth_image_output_port() const
Returns the abstract valued output port that contains an ImageDepth32F.
Definition: rgbd_camera.cc:200
const InputPort< double > & state_input_port() const
Returns the vector valued input port that takes a vector of q, v corresponding to the positions and v...
Definition: rgbd_camera.cc:185
const double position
Definition: robot_plan_interpolator_test.cc:65
const CameraInfo & color_camera_info() const
Reterns the color sensor&#39;s info.
Definition: rgbd_camera.h:277
DRAKE_DEPRECATED("`ResetRenderer` will be removed after 2018/10/01; please update your ""code to specify the `RgbdRenderer` instance at construction time.") void ResetRenderer(std
Sets and initializes RgbdRenderer.
Definition: rgbd_camera.h:259
double fov_y
Definition: rgbd_camera_publish_lcm_example.cc:80
Simple data structure for camera information that includes the image size and camera intrinsic parame...
Definition: camera_info.h:58
STL namespace.
Wraps a continuous RgbdCamera with zero order holds to have it function as a discrete sensor...
Definition: rgbd_camera.h:378
const InputPort< double > & get_input_port(int port_index) const
Returns the typed input port at index port_index.
Definition: system.h:1083
const CameraInfo & depth_camera_info() const
Reterns the depth sensor&#39;s info.
Definition: rgbd_camera.h:280
RgbdRenderer & mutable_renderer()
Reterns mutable renderer.
Definition: rgbd_camera.h:274
const OutputPort< double > & camera_base_pose_output_port() const
Returns the vector valued output port that contains a PoseVector.
Definition: rgbd_camera.cc:190
const RigidBodyFrame< double > & frame() const
Returns the RigidBodyFrame to which this RgbdCamera is attached.
Definition: rgbd_camera.h:292
RgbdCamera(const RgbdCamera &)=delete
double period() const
Returns update period for discrete camera.
Definition: rgbd_camera.h:402
const OutputPort< double > & label_image_output_port() const
Returns the abstract valued output port that contains an label image of the type ImageLabel16I.
Definition: rgbd_camera.cc:205
const Eigen::Isometry3d & depth_camera_optical_pose() const
Returns X_BD.
Definition: rgbd_camera.h:286
const InputPort< double > & state_input_port() const
Definition: rgbd_camera.h:405
TypeSafeIndex< class VisualTag > VisualIndex
Represents indices for visual elements.
Definition: rgbd_renderer.h:102
static void ConvertDepthImageToPointCloud(const ImageDepth32F &depth_image, const CameraInfo &camera_info, Eigen::Matrix3Xf *point_cloud)
Converts a depth image obtained from RgbdCamera to a point cloud.
Definition: rgbd_camera.cc:22
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:84
Diagram is a System composed of one or more constituent Systems, arranged in a directed graph where t...
Definition: diagram.h:45
vtkNew< vtkRenderer > renderer
Definition: rgbd_renderer_ospray.cc:75
const RigidBodyTree< double > & tree() const
Returns the RigidBodyTree to which this RgbdCamera is attached.
Definition: rgbd_camera.h:301
static constexpr int kDefaultWidth
Default value for width.
Definition: rgbd_renderer.h:33
const OutputPort< double > & color_image_output_port() const
Returns the abstract valued output port that contains a RGBA image of the type ImageRgba8U.
Definition: rgbd_camera.cc:195
const Eigen::Isometry3d & color_camera_optical_pose() const
Returns X_BC.
Definition: rgbd_camera.h:283
const systems::OutputPort< double > & camera_base_pose_output_port() const
Definition: rgbd_camera.h:425
Abstract interface of RGB-D renderers, which render RGB, depth and label images using VisualElement...
Definition: rgbd_renderer.h:80
int width
Definition: rgbd_camera_test.cc:51
const OutputPort< double > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:1090
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
const systems::OutputPort< double > & depth_image_output_port() const
Definition: rgbd_camera.h:415
RgbdCamera & mutable_camera()
Returns reference to RgbdCamera intsance.
Definition: rgbd_camera.h:399
A type-safe non-negative index class.
Definition: type_safe_index.h:155
const systems::OutputPort< double > & color_image_output_port() const
Definition: rgbd_camera.h:410
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
int height
Definition: rgbd_camera_test.cc:52
An RGB-D camera system that provides RGB, depth and label images using visual elements of RigidBodyTr...
Definition: rgbd_camera.h:65