Drake
rgbd_camera.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 
6 #include <Eigen/Dense>
7 
17 
18 namespace drake {
19 namespace systems {
20 namespace sensors {
21 /// An RGB-D camera system that provides RGB, depth and label images using
22 /// visual elements of RigidBodyTree.
23 /// RgbdCamera uses [VTK](https://github.com/Kitware/VTK) as the rendering
24 /// backend.
25 /// Its image resolution is fixed at VGA (640 x 480 pixels) for all three
26 /// images. The default depth sensing range is from 0.5 m to 5.0 m.
27 ///
28 /// Let `W` be the world coordinate system. In addition to `W`, there are three
29 /// more coordinate systems that are associated with an RgbdCamera. They are
30 /// defined as follows:
31 ///
32 /// * `B` - the camera's base coordinate system: X-forward, Y-left, and Z-up.
33 ///
34 /// * `C` - the camera's color sensor's optical coordinate system: `X-right`,
35 /// `Y-down` and `Z-forward`.
36 ///
37 /// * `D` - the camera's depth sensor's optical coordinate system: `X-right`,
38 /// `Y-down` and `Z-forward`.
39 ///
40 /// The origins of `C` and `D` (i.e., `Co` and `Do`, respectively) are offset
41 /// from `B`'s origin (`Bo`) by 0 m in `B`'s X-axis, +0.02 m in `B`'s Y-axis,
42 /// and 0 m in `B`'s Z axis. Since `C` and `D` are coincident, the depth image
43 /// is a "registered depth image" for the RGB image. No disparity between the
44 /// RGB and depth images are modeled. For more details about the poses of `C`
45 /// and `D`, see the class documentation of CameraInfo.
46 ///
47 /// Output image format:
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 for %RgbdCamera that defines `B` using Euler angles.
90  /// The pose of %RgbdCamera will be fixed to the world coordinate system
91  /// throughout the simulation.
92  ///
93  /// @param name The name of the RgbdCamera. This can be any value, but
94  /// should typically be unique among all sensors attached to a particular
95  /// model instance.
96  ///
97  /// @param tree The RigidBodyTree containing the geometric description of the
98  /// world. The life span of this parameter must exceed that of this class's
99  /// instance. The maximum number of bodies in the `tree` must be less than
100  /// 1536 based on the number of the colors used for the label image, otherwise
101  /// an exception will be thrown.
102  ///
103  /// @param position The x-y-z position of `B` in `W`. This defines the
104  /// translation component of `X_WB`.
105  ///
106  /// @param orientation The roll-pitch-yaw orientation of `B` in `W`. This
107  /// defines the orientation component of `X_WB`.
108  ///
109  /// @param z_near The minimum depth distance RgbdCamera can measure.
110  /// The default is 0.5 meters.
111  ///
112  /// @param z_far The maximum depth distance RgbdCamera can measure.
113  /// The default is 5 meters.
114  ///
115  /// @param fov_y The RgbdCamera's vertical field of view.
116  /// The default is PI / 4.
117  ///
118  /// @param show_window A flag for showing a visible window. If this is false,
119  /// offscreen rendering is executed. This is useful for debugging purposes.
120  /// The default is true.
121  ///
122  /// @throws std::logic_error When the number of rigid bodies in the scene
123  /// exceeds the maximum limit 1535.
124  RgbdCamera(const std::string& name,
126  const Eigen::Vector3d& position,
127  const Eigen::Vector3d& orientation,
128  double z_near = 0.5,
129  double z_far = 5.0,
130  double fov_y = M_PI_4,
131  bool show_window = true);
132 
133  /// A constructor for %RgbdCamera that defines `B` using a RigidBodyFrame.
134  /// The pose of %RgbdCamera is fixed to a user-defined frame and will be
135  /// updated during the simulation.
136  ///
137  /// @param name The name of the RgbdCamera. This can be any value, but
138  /// should typically be unique among all sensors attached to a particular
139  /// model instance.
140  ///
141  /// @param tree The RigidBodyTree containing the geometric description of the
142  /// world. The life span of this parameter must exceed that of this class's
143  /// instance. The maximum number of bodies in the `tree` must be less than
144  /// 1536 based on the number of the colors used for the label image, otherwise
145  /// an exception will be thrown.
146  ///
147  /// @param frame The frame in @tree to which this camera is attached.
148  ///
149  /// @param z_near The minimum depth distance RgbdCamera can measure.
150  /// The default is 0.5 meters.
151  ///
152  /// @param z_far The maximum depth distance RgbdCamera can measure.
153  /// The default is 5 meters.
154  ///
155  /// @param fov_y The RgbdCamera's vertical field of view.
156  /// The default is PI / 4.
157  ///
158  /// @param show_window A flag for showing a visible window. If this is false,
159  /// offscreen rendering is executed. This is useful for debugging purposes.
160  /// The default is true.
161  ///
162  /// @throws std::logic_error When the number of rigid bodies in the scene
163  /// exceeds the maximum limit 1535.
164  RgbdCamera(const std::string& name,
165  const RigidBodyTree<double>& tree,
167  double z_near = 0.5,
168  double z_far = 5.0,
169  double fov_y = M_PI_4,
170  bool show_window = true);
171 
172  ~RgbdCamera() = default;
173 
174  /// Reterns the color sensor's info.
175  const CameraInfo& color_camera_info() const { return color_camera_info_; }
176 
177  /// Reterns the depth sensor's info.
178  const CameraInfo& depth_camera_info() const { return depth_camera_info_; }
179 
180  /// Returns `X_BC`.
181  const Eigen::Isometry3d& color_camera_optical_pose() const { return X_BC_; }
182 
183  /// Returns `X_BD`.
184  const Eigen::Isometry3d& depth_camera_optical_pose() const { return X_BD_; }
185 
186  /// Returns the RigidBodyFrame to which this RgbdCamera is attached.
187  ///
188  /// @throws std::logic_error When RgbdCamera is instantiated as a fixed
189  /// camera.
190  const RigidBodyFrame<double>& frame() const {
191  if (camera_fixed_)
192  throw std::logic_error(
193  "RgbdCamera::frame(): "
194  "A fixed camera does not have an associated frame.");
195  return frame_;
196  }
197 
198  /// Returns the RigidBodyTree to which this RgbdCamera is attached.
199  const RigidBodyTree<double>& tree() const { return tree_; }
200 
201  /// Returns a descriptor of the vector valued input port that takes a vector
202  /// of `q, v` corresponding to the positions and velocities associated with
203  /// the RigidBodyTree.
205 
206  /// Returns the abstract valued output port that contains a RGBA image of the
207  /// type ImageRgba8U.
209 
210  /// Returns the abstract valued output port that contains an ImageDepth32F.
212 
213  /// Returns the abstract valued output port that contains an label image of
214  /// the type ImageLabel16I.
216 
217  /// Returns the vector valued output port that contains a PoseVector.
219 
220  private:
221  void Init(const std::string& name);
222 
223  // These are the calculator methods for the four output ports.
224  void OutputColorImage(const Context<double>& context,
225  ImageRgba8U* color_image) const;
226  void OutputDepthImage(const Context<double>& context,
227  ImageDepth32F* depth_image) const;
228  void OutputLabelImage(const Context<double>& context,
229  ImageLabel16I* label_image) const;
230  void OutputPoseVector(const Context<double>& context,
231  rendering::PoseVector<double>* pose_vector) const;
232 
233  // TODO(sherm1) This should be the calculator for a cache entry containing
234  // the VTK update that must be valid before outputting any image info. For
235  // now it has to be repeated before each image output port calculation.
236  void UpdateModelPoses(const BasicVector<double>& input_vector) const;
237 
238  const InputPortDescriptor<double>* state_input_port_{};
239  const OutputPort<double>* color_image_port_{};
240  const OutputPort<double>* depth_image_port_{};
241  const OutputPort<double>* label_image_port_{};
242  const OutputPort<double>* camera_base_pose_port_{};
243 
244  const RigidBodyTree<double>& tree_;
245  const RigidBodyFrame<double> frame_;
246 
247  const bool camera_fixed_;
248  const CameraInfo color_camera_info_;
249  const CameraInfo depth_camera_info_;
250  const Eigen::Isometry3d X_WB_initial_;
251  // The color sensor's origin (`Co`) is offset by 0.02 m on the Y axis of
252  // the RgbdCamera's base coordinate system (`B`).
253  // TODO(kunimatsu-tri) Add support for arbitrary relative pose.
254  // TODO(kunimatsu-tri) Change the X_BD_ to be different from X_BC_ when
255  // it's needed.
256  const Eigen::Isometry3d X_BC_{Eigen::Translation3d(0., 0.02, 0.) *
257  (Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()) *
258  Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()))};
259  // The depth sensor's origin (`Do`) is offset by 0.02 m on the Y axis of
260  // the RgbdCamera's base coordinate system (`B`).
261  const Eigen::Isometry3d X_BD_{Eigen::Translation3d(0., 0.02, 0.) *
262  (Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()) *
263  Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()))};
264 
265  std::unique_ptr<RgbdRenderer> renderer_;
266 };
267 
268 /**
269  * Wraps a continuous RgbdCamera with zero order holds to have it function as
270  * a discrete sensor.
271  */
272 class RgbdCameraDiscrete final : public systems::Diagram<double> {
273  public:
275 
276  /// Constructs a diagram containing a (non-registered) RgbdCamera that will
277  /// update at a given rate.
278  RgbdCameraDiscrete(std::unique_ptr<RgbdCamera> camera,
279  double period = 1. / 30);
280 
281  /// Returns reference to RgbdCamera intsance.
282  const RgbdCamera& camera() const { return *camera_; }
283 
284  /// Returns reference to RgbdCamera intsance.
285  RgbdCamera& mutable_camera() { return *camera_; }
286 
287  /// Returns update period for discrete camera.
288  double period() const { return period_; }
289 
290  /// @see RgbdCamera::state_input_port().
292  return get_input_port(input_port_state_);
293  }
294 
295  /// @see RgbdCamera::color_image_output_port().
297  return get_output_port(output_port_color_image_);
298  }
299 
300  /// @see RgbdCamera::depth_image_output_port().
302  return get_output_port(output_port_depth_image_);
303  }
304 
305  /// @see RgbdCamera::label_image_output_port().
307  return get_output_port(output_port_label_image_);
308  }
309 
310  /// @see RgbdCamera::camera_base_pose_output_port().
312  return get_output_port(output_port_pose_);
313  }
314 
315  private:
316  RgbdCamera* const camera_{};
317  const double period_{};
318 
319  int input_port_state_{-1};
320  int output_port_color_image_{-1};
321  int output_port_depth_image_{-1};
322  int output_port_label_image_{-1};
323  int output_port_pose_{-1};
324 };
325 
326 } // namespace sensors
327 } // namespace systems
328 } // namespace drake
const RgbdCamera & camera() const
Returns reference to RgbdCamera intsance.
Definition: rgbd_camera.h:282
const systems::OutputPort< double > & label_image_output_port() const
Definition: rgbd_camera.h:306
Definition: automotive_demo.cc:88
const OutputPort< double > & depth_image_output_port() const
Returns the abstract valued output port that contains an ImageDepth32F.
Definition: rgbd_camera.cc:154
const InputPortDescriptor< double > & state_input_port() const
Definition: rgbd_camera.h:291
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:175
double fov_y
Definition: rgbd_camera_publish_lcm_example.cc:76
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:272
const CameraInfo & depth_camera_info() const
Reterns the depth sensor&#39;s info.
Definition: rgbd_camera.h:178
const InputPortDescriptor< double > & get_input_port(int port_index) const
Returns the descriptor of the input port at index port_index.
Definition: system.h:972
const OutputPort< double > & camera_base_pose_output_port() const
Returns the vector valued output port that contains a PoseVector.
Definition: rgbd_camera.cc:144
const RigidBodyFrame< double > & frame() const
Returns the RigidBodyFrame to which this RgbdCamera is attached.
Definition: rgbd_camera.h:190
RgbdCamera(const RgbdCamera &)=delete
double period() const
Returns update period for discrete camera.
Definition: rgbd_camera.h:288
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:159
const Eigen::Isometry3d & depth_camera_optical_pose() const
Returns X_BD.
Definition: rgbd_camera.h:184
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:30
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
Diagram is a System composed of one or more constituent Systems, arranged in a directed graph where t...
Definition: diagram.h:31
Simple data format for Image.
Definition: image.h:18
const RigidBodyTree< double > & tree() const
Returns the RigidBodyTree to which this RgbdCamera is attached.
Definition: rgbd_camera.h:199
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:139
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:149
const Eigen::Isometry3d & color_camera_optical_pose() const
Returns X_BC.
Definition: rgbd_camera.h:181
const systems::OutputPort< double > & camera_base_pose_output_port() const
Definition: rgbd_camera.h:311
const OutputPort< double > & get_output_port(int port_index) const
Returns the output port at index port_index.
Definition: system.h:983
#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:301
RgbdCamera & mutable_camera()
Returns reference to RgbdCamera intsance.
Definition: rgbd_camera.h:285
const systems::OutputPort< double > & color_image_output_port() const
Definition: rgbd_camera.h:296
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
An RGB-D camera system that provides RGB, depth and label images using visual elements of RigidBodyTr...
Definition: rgbd_camera.h:64