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