Drake
rgbd_camera.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <limits>
4 #include <memory>
5 #include <string>
6 
7 #include <Eigen/Dense>
8 
17 
18 namespace drake {
19 namespace systems {
20 namespace sensors {
64 class RgbdCamera : public LeafSystem<double> {
65  public:
67 
68 
69  static void ConvertDepthImageToPointCloud(const ImageDepth32F& depth_image,
83  const CameraInfo& camera_info,
84  Eigen::Matrix3Xf* point_cloud);
85 
88  class InvalidDepth {
89  public:
92  static constexpr float kTooFar{std::numeric_limits<float>::infinity()};
93 
101  static constexpr float kTooClose{0.f};
102  };
103 
105  class Label {
106  public:
109  static constexpr int16_t kNoBody{std::numeric_limits<int16_t>::max()};
111  static constexpr int16_t kFlatTerrain{
113  };
114 
146  RgbdCamera(const std::string& name,
148  const Eigen::Vector3d& position,
149  const Eigen::Vector3d& orientation,
150  double depth_range_near = 0.5,
151  double depth_range_far = 5.0,
152  double fov_y = M_PI_4,
153  bool show_window = true);
154 
182  RgbdCamera(const std::string& name,
183  const RigidBodyTree<double>& tree,
185  double depth_range_near = 0.5,
186  double depth_range_far = 5.0,
187  double fov_y = M_PI_4,
188  bool show_window = true);
189 
190  ~RgbdCamera();
191 
193  const CameraInfo& color_camera_info() const;
194 
196  const CameraInfo& depth_camera_info() const;
197 
199  const Eigen::Isometry3d& color_camera_optical_pose() const;
200 
202  const Eigen::Isometry3d& depth_camera_optical_pose() const;
203 
205  const RigidBodyFrame<double>& frame() const;
206 
208  const RigidBodyTree<double>& tree() const;
209 
214 
218 
221 
225 
228 
229  private:
230  void Init(const std::string& name);
231 
232  // These are the calculator methods for the four output ports.
233  void OutputColorImage(const Context<double>& context,
234  ImageRgba8U* color_image) const;
235  void OutputDepthImage(const Context<double>& context,
236  ImageDepth32F* depth_image) const;
237  void OutputLabelImage(const Context<double>& context,
238  ImageLabel16I* label_image) const;
239  void OutputPoseVector(const Context<double>& context,
240  rendering::PoseVector<double>* pose_vector) const;
241 
242  const OutputPort<double>* color_image_port_{};
243  const OutputPort<double>* depth_image_port_{};
244  const OutputPort<double>* label_image_port_{};
245  const OutputPort<double>* camera_base_pose_port_{};
246 
247  class Impl;
248  std::unique_ptr<Impl> impl_;
249 };
250 
255 class RgbdCameraDiscrete : public systems::Diagram<double> {
256  public:
258 
261  RgbdCameraDiscrete(std::unique_ptr<RgbdCamera> camera,
262  double period = 1. / 30);
263 
265  const RgbdCamera& camera() const { return *camera_; }
266 
268  RgbdCamera& mutable_camera() { return *camera_; }
269 
271  double period() const { return period_; }
272 
275  return get_input_port(input_port_state_);
276  }
277 
280  return get_output_port(output_port_color_image_);
281  }
282 
285  return get_output_port(output_port_depth_image_);
286  }
287 
290  return get_output_port(output_port_label_image_);
291  }
292 
295  return get_output_port(output_port_pose_);
296  }
297 
298  private:
299  RgbdCamera* const camera_{};
300  const double period_{};
301 
302  int input_port_state_{-1};
303  int output_port_color_image_{-1};
304  int output_port_depth_image_{-1};
305  int output_port_label_image_{-1};
306  int output_port_pose_{-1};
307 };
308 
309 } // namespace sensors
310 } // namespace systems
311 } // namespace drake
const CameraInfo & color_camera_info() const
Reterns the color sensor&#39;s info.
Definition: rgbd_camera.cc:791
const RgbdCamera & camera() const
Returns reference to RgbdCamera intsance.
Definition: rgbd_camera.h:265
static constexpr float kTooClose
The depth value when the min sensing range is violated because the object being sensed is too close...
Definition: rgbd_camera.h:101
const systems::OutputPort< double > & label_image_output_port() const
Definition: rgbd_camera.h:289
const Eigen::Isometry3d & color_camera_optical_pose() const
Returns X_BC.
Definition: rgbd_camera.cc:799
double position
Definition: system_identification_test.cc:205
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:830
const InputPortDescriptor< double > & state_input_port() const
Definition: rgbd_camera.h:274
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:255
const InputPortDescriptor< double > & get_input_port(int port_index) const
Returns the descriptor of the input port at index port_index.
Definition: system.h:815
const Eigen::Isometry3d & depth_camera_optical_pose() const
Returns X_BD.
Definition: rgbd_camera.cc:803
const OutputPort< double > & camera_base_pose_output_port() const
Returns the vector valued output port that contains a PoseVector.
Definition: rgbd_camera.cc:820
Set of constants used to represent invalid depth values.
Definition: rgbd_camera.h:88
static constexpr float kTooFar
The depth value when the max sensing range is exceeded.
Definition: rgbd_camera.h:92
RgbdCamera(const RgbdCamera &)=delete
const RigidBodyFrame< double > & frame() const
Returns the RigidBodyFrame to which this RgbdCamera is attached.
Definition: rgbd_camera.cc:807
double period() const
Returns update period for discrete camera.
Definition: rgbd_camera.h:271
~RgbdCamera()
Definition: rgbd_camera.cc:789
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:697
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:835
double depth_range_near
Definition: rgbd_camera_publish_lcm_example.cc:77
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:215
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:17
const RigidBodyTree< double > & tree() const
Returns the RigidBodyTree to which this RgbdCamera is attached.
Definition: rgbd_camera.cc:811
double depth_range_far
Definition: rgbd_camera_publish_lcm_example.cc:78
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:815
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:825
const systems::OutputPort< double > & camera_base_pose_output_port() const
Definition: rgbd_camera.h:294
const OutputPort< double > & get_output_port(int port_index) const
Returns the output port at index port_index.
Definition: system.h:826
#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:35
const systems::OutputPort< double > & depth_image_output_port() const
Definition: rgbd_camera.h:284
Set of labels used for label image.
Definition: rgbd_camera.h:105
const CameraInfo & depth_camera_info() const
Reterns the depth sensor&#39;s info.
Definition: rgbd_camera.cc:795
RgbdCamera & mutable_camera()
Returns reference to RgbdCamera intsance.
Definition: rgbd_camera.h:268
const systems::OutputPort< double > & color_image_output_port() const
Definition: rgbd_camera.h:279
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