Drake
rgbd_camera.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 #include <utility>
6 
7 #include <Eigen/Dense>
8 
18 
19 namespace drake {
20 namespace systems {
21 namespace sensors {
22 /// An RGB-D camera system that provides RGB, depth and label images using
23 /// visual elements of RigidBodyTree.
24 /// RgbdCamera uses [VTK](https://github.com/Kitware/VTK) as the rendering
25 /// backend.
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 for %RgbdCamera that 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  /// The default is 0.5 meters.
112  ///
113  /// @param z_far The maximum depth distance RgbdCamera can measure.
114  /// The default is 5 meters.
115  ///
116  /// @param fov_y The RgbdCamera's vertical field of view.
117  /// The default is PI / 4.
118  ///
119  /// @param show_window A flag for showing a visible window. If this is false,
120  /// offscreen rendering is executed. This is useful for debugging purposes.
121  /// The default is true.
122  ///
123  /// @throws std::logic_error When the number of rigid bodies in the scene
124  /// exceeds the maximum limit 1535.
125  RgbdCamera(const std::string& name,
127  const Eigen::Vector3d& position,
128  const Eigen::Vector3d& orientation,
129  double z_near = 0.5,
130  double z_far = 5.0,
131  double fov_y = M_PI_4,
132  bool show_window = RenderingConfig::kDefaultShowWindow,
135 
136  /// A constructor for %RgbdCamera that defines `B` using a RigidBodyFrame.
137  /// The pose of %RgbdCamera is fixed to a user-defined frame and will be
138  /// updated during the simulation.
139  ///
140  /// @param name The name of the RgbdCamera. This can be any value, but
141  /// should typically be unique among all sensors attached to a particular
142  /// model instance.
143  ///
144  /// @param tree The RigidBodyTree containing the geometric description of the
145  /// world. The life span of this parameter must exceed that of this class's
146  /// instance. The maximum number of bodies in the `tree` must be less than
147  /// 1536 based on the number of the colors used for the label image, otherwise
148  /// an exception will be thrown.
149  ///
150  /// @param frame The frame in @tree to which this camera is attached.
151  ///
152  /// @param z_near The minimum depth distance RgbdCamera can measure.
153  /// The default is 0.5 meters.
154  ///
155  /// @param z_far The maximum depth distance RgbdCamera can measure.
156  /// The default is 5 meters.
157  ///
158  /// @param fov_y The RgbdCamera's vertical field of view.
159  /// The default is PI / 4.
160  ///
161  /// @param show_window A flag for showing a visible window. If this is false,
162  /// offscreen rendering is executed. This is useful for debugging purposes.
163  /// The default is true.
164  ///
165  /// @throws std::logic_error When the number of rigid bodies in the scene
166  /// exceeds the maximum limit 1535.
167  RgbdCamera(const std::string& name,
168  const RigidBodyTree<double>& tree,
170  double z_near = 0.5,
171  double z_far = 5.0,
172  double fov_y = M_PI_4,
173  bool show_window = RenderingConfig::kDefaultShowWindow,
176 
177  ~RgbdCamera() = default;
178 
179  /// Sets and initializes RgbdRenderer. The viewpoint of renderer will be
180  /// appropriately handled inside this function.
181  /// Note that if any visual element is registered with renderer before
182  /// this method is called, its behavior will not be guaranteed.
183  // TODO(kunimatsu-tri) Initialize the internal state of renderer when this
184  // method is called.
185  void ResetRenderer(std::unique_ptr<RgbdRenderer> renderer) {
186  renderer_ = std::move(renderer);
187  InitRenderer();
188  // This is needed only for camera_fixed_ is true since UpdateViewpoint()
189  // will be called while rendering related output ports are evaluated
190  // if it is false.
191  if (camera_fixed_) {
192  renderer_->UpdateViewpoint(X_WB_initial_ * X_BC_);
193  }
194  }
195 
196  /// Reterns mutable renderer.
197  RgbdRenderer& mutable_renderer() { return *renderer_; }
198 
199  /// Reterns the color sensor's info.
200  const CameraInfo& color_camera_info() const { return color_camera_info_; }
201 
202  /// Reterns the depth sensor's info.
203  const CameraInfo& depth_camera_info() const { return depth_camera_info_; }
204 
205  /// Returns `X_BC`.
206  const Eigen::Isometry3d& color_camera_optical_pose() const { return X_BC_; }
207 
208  /// Returns `X_BD`.
209  const Eigen::Isometry3d& depth_camera_optical_pose() const { return X_BD_; }
210 
211  /// Returns the RigidBodyFrame to which this RgbdCamera is attached.
212  ///
213  /// @throws std::logic_error When RgbdCamera is instantiated as a fixed
214  /// camera.
215  const RigidBodyFrame<double>& frame() const {
216  if (camera_fixed_)
217  throw std::logic_error(
218  "RgbdCamera::frame(): "
219  "A fixed camera does not have an associated frame.");
220  return frame_;
221  }
222 
223  /// Returns the RigidBodyTree to which this RgbdCamera is attached.
224  const RigidBodyTree<double>& tree() const { return tree_; }
225 
226  /// Returns a descriptor of the vector valued input port that takes a vector
227  /// of `q, v` corresponding to the positions and velocities associated with
228  /// the RigidBodyTree.
230 
231  /// Returns the abstract valued output port that contains a RGBA image of the
232  /// type ImageRgba8U.
234 
235  /// Returns the abstract valued output port that contains an ImageDepth32F.
237 
238  /// Returns the abstract valued output port that contains an label image of
239  /// the type ImageLabel16I.
241 
242  /// Returns the vector valued output port that contains a PoseVector.
244 
245  private:
246  void InitPorts(const std::string& name);
247 
248  void InitRenderer();
249 
250  // These are the calculator methods for the four output ports.
251  void OutputColorImage(const Context<double>& context,
252  ImageRgba8U* color_image) const;
253  void OutputDepthImage(const Context<double>& context,
254  ImageDepth32F* depth_image) const;
255  void OutputLabelImage(const Context<double>& context,
256  ImageLabel16I* label_image) const;
257  void OutputPoseVector(const Context<double>& context,
258  rendering::PoseVector<double>* pose_vector) const;
259 
260  // TODO(sherm1) This should be the calculator for a cache entry containing
261  // the VTK update that must be valid before outputting any image info. For
262  // now it has to be repeated before each image output port calculation.
263  void UpdateModelPoses(const BasicVector<double>& input_vector) const;
264 
265  const InputPortDescriptor<double>* state_input_port_{};
266  const OutputPort<double>* color_image_port_{};
267  const OutputPort<double>* depth_image_port_{};
268  const OutputPort<double>* label_image_port_{};
269  const OutputPort<double>* camera_base_pose_port_{};
270 
271  const RigidBodyTree<double>& tree_;
272  const RigidBodyFrame<double> frame_;
273 
274  const bool camera_fixed_;
275  const CameraInfo color_camera_info_;
276  const CameraInfo depth_camera_info_;
277  const Eigen::Isometry3d X_WB_initial_;
278  // The color sensor's origin (`Co`) is offset by 0.02 m on the Y axis of
279  // the RgbdCamera's base coordinate system (`B`).
280  // TODO(kunimatsu-tri) Add support for arbitrary relative pose.
281  // TODO(kunimatsu-tri) Change the X_BD_ to be different from X_BC_ when
282  // it's needed.
283  const Eigen::Isometry3d X_BC_{Eigen::Translation3d(0., 0.02, 0.) *
284  (Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()) *
285  Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()))};
286  // The depth sensor's origin (`Do`) is offset by 0.02 m on the Y axis of
287  // the RgbdCamera's base coordinate system (`B`).
288  const Eigen::Isometry3d X_BD_{Eigen::Translation3d(0., 0.02, 0.) *
289  (Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()) *
290  Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()))};
291 
292  std::unique_ptr<RgbdRenderer> renderer_;
293 };
294 
295 /**
296  * Wraps a continuous RgbdCamera with zero order holds to have it function as
297  * a discrete sensor.
298  */
299 class RgbdCameraDiscrete final : public systems::Diagram<double> {
300  public:
302 
303  static constexpr double kDefaultPeriod = 1. / 30;
304 
305  /// Constructs a diagram containing a (non-registered) RgbdCamera that will
306  /// update at a given rate.
307  /// @param period
308  /// Update period (sec).
309  /// @param render_label_image
310  /// If true, renders label image (which requires additional overhead). If
311  /// false, `label_image_output_port` will raise an error if called.
312  RgbdCameraDiscrete(std::unique_ptr<RgbdCamera> camera,
313  double period = kDefaultPeriod,
314  bool render_label_image = true);
315 
316  /// Returns reference to RgbdCamera intsance.
317  const RgbdCamera& camera() const { return *camera_; }
318 
319  /// Returns reference to RgbdCamera intsance.
320  RgbdCamera& mutable_camera() { return *camera_; }
321 
322  /// Returns update period for discrete camera.
323  double period() const { return period_; }
324 
325  /// @see RgbdCamera::state_input_port().
327  return get_input_port(input_port_state_);
328  }
329 
330  /// @see RgbdCamera::color_image_output_port().
332  return get_output_port(output_port_color_image_);
333  }
334 
335  /// @see RgbdCamera::depth_image_output_port().
337  return get_output_port(output_port_depth_image_);
338  }
339 
340  /// @see RgbdCamera::label_image_output_port().
342  return get_output_port(output_port_label_image_);
343  }
344 
345  /// @see RgbdCamera::camera_base_pose_output_port().
347  return get_output_port(output_port_pose_);
348  }
349 
350  private:
351  RgbdCamera* const camera_{};
352  const double period_{};
353 
354  int input_port_state_{-1};
355  int output_port_color_image_{-1};
356  int output_port_depth_image_{-1};
357  int output_port_label_image_{-1};
358  int output_port_pose_{-1};
359 };
360 
361 } // namespace sensors
362 } // namespace systems
363 } // namespace drake
const RgbdCamera & camera() const
Returns reference to RgbdCamera intsance.
Definition: rgbd_camera.h:317
static constexpr bool kDefaultShowWindow
Default value for show_window.
Definition: rgbd_renderer.h:45
const systems::OutputPort< double > & label_image_output_port() const
Definition: rgbd_camera.h:341
static constexpr int kDefaultHeight
Default value for height.
Definition: rgbd_renderer.h:29
Definition: automotive_demo.cc:90
const OutputPort< double > & depth_image_output_port() const
Returns the abstract valued output port that contains an ImageDepth32F.
Definition: rgbd_camera.cc:151
const InputPortDescriptor< double > & state_input_port() const
Definition: rgbd_camera.h:326
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:200
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
Wraps a continuous RgbdCamera with zero order holds to have it function as a discrete sensor...
Definition: rgbd_camera.h:299
const CameraInfo & depth_camera_info() const
Reterns the depth sensor&#39;s info.
Definition: rgbd_camera.h:203
RgbdRenderer & mutable_renderer()
Reterns mutable renderer.
Definition: rgbd_camera.h:197
const InputPortDescriptor< double > & get_input_port(int port_index) const
Returns the typed input port at index port_index.
Definition: system.h:967
const OutputPort< double > & camera_base_pose_output_port() const
Returns the vector valued output port that contains a PoseVector.
Definition: rgbd_camera.cc:141
const RigidBodyFrame< double > & frame() const
Returns the RigidBodyFrame to which this RgbdCamera is attached.
Definition: rgbd_camera.h:215
RgbdCamera(const RgbdCamera &)=delete
double period() const
Returns update period for discrete camera.
Definition: rgbd_camera.h:323
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:156
const Eigen::Isometry3d & depth_camera_optical_pose() const
Returns X_BD.
Definition: rgbd_camera.h:209
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:85
Diagram is a System composed of one or more constituent Systems, arranged in a directed graph where t...
Definition: diagram.h:34
void ResetRenderer(std::unique_ptr< RgbdRenderer > renderer)
Sets and initializes RgbdRenderer.
Definition: rgbd_camera.h:185
const RigidBodyTree< double > & tree() const
Returns the RigidBodyTree to which this RgbdCamera is attached.
Definition: rgbd_camera.h:224
vtkNew< vtkRenderer > renderer
Definition: rgbd_renderer_vtk.cc:70
const InputPortDescriptor< double > & state_input_port() const
Returns a descriptor of the vector valued input port that takes a vector of q, v corresponding to the...
Definition: rgbd_camera.cc:136
static constexpr int kDefaultWidth
Default value for width.
Definition: rgbd_renderer.h:25
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:146
const Eigen::Isometry3d & color_camera_optical_pose() const
Returns X_BC.
Definition: rgbd_camera.h:206
const systems::OutputPort< double > & camera_base_pose_output_port() const
Definition: rgbd_camera.h:346
Abstract interface of RGB-D renderers, which render RGB, depth and label images using VisualElement...
Definition: rgbd_renderer.h:72
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:974
#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:336
RgbdCamera & mutable_camera()
Returns reference to RgbdCamera intsance.
Definition: rgbd_camera.h:320
const systems::OutputPort< double > & color_image_output_port() const
Definition: rgbd_camera.h:331
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