Drake
depth_sensor.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 #include <vector>
6 
17 
18 namespace drake {
19 namespace systems {
20 namespace sensors {
21 
22 /// A simple model of an ideal depth sensor. Example real-world depth sensors
23 /// include Lidar, IR, sonar, etc. The depth measurements are taken at evenly
24 /// spaced pitch angles and yaw angles. Ray casting is used to obtain the depth
25 /// measurements and all pitch / yaw combinations in a single resulting depth
26 /// image are obtained at a single effective point in time. Thus, this sensor
27 /// does *not* model aliasing effects due to the time spent scanning vertically
28 /// and horizontally. For the ray casting, DepthSensor uses collision models of
29 /// RigidBodyTree.
30 ///
31 /// There are two frames associated with this sensor: its base frame and its
32 /// optical frame. This sensor's specification and configuration are defined in
33 /// terms of this sensor's base frame. The base frame's origin defines the point
34 /// from which this sensor's ray casts originate. Its +X, +Y, and +Z axes are
35 /// pointing towards this sensor's "forward," "left", and "up" directions,
36 /// respectively.
37 ///
38 /// This sensor's optical frame's origin is the same as the base frame's origin.
39 /// Its +X axis defines where a particular ray cast is pointing. There are two
40 /// angles associated with a depth measurement, as described below. Both angles
41 /// are defined in terms of the sensor's base frame. Together they define a
42 /// transform between the base frame and optical frame.
43 ///
44 /// 1. yaw - The horizontal scan angle that rotates about this sensor's base
45 /// frame's +Z axis using the right-hand rule. When the pitch is
46 /// zero, a yaw of zero results in this sensor measuring down the
47 /// base frame's +X axis.
48 /// 2. pitch - The vertical scan angle that rotates about this sensor's base
49 /// frames's -Y axis using using the right-hand rule. In other
50 /// words, when the pitch increases from zero to PI / 2, this
51 /// sensor's optical frame is tilted to point upward. When the yaw
52 /// is zero, a pitch of zero results in this sensor measuring down
53 /// the base frame's +X axis.
54 ///
55 /// To summarize, the location from which this sensor's rays emanate is
56 /// (0, 0, 0) in the base frame. When both the yaw and pitch are zero, this
57 /// sensor's base and optical frames are identical. The yaw causes the optical
58 /// frame to rotate about the base frame's +Z axis, while the pitch causes it to
59 /// rotate about this sensor's -Y axis.
60 ///
61 /// This system has two output ports. The first contains the sensed values
62 /// stored in a DepthSensorOutput object. For each pitch, there is a fixed
63 /// number of yaw values as specified by num_yaw(). Each of these vector
64 /// of yaw values that share the same pitch are contiguous in the output vector.
65 /// In other words, here is pseudocode describing this sensor's output vector:
66 ///
67 /// <pre>
68 /// for i in 0 to num_pitch():
69 /// for j in 0 to num_yaw():
70 /// output_vector[i * num_yaw() + j] ==
71 /// [depth value when yaw = min_yaw() + j * yaw_increment() and
72 /// pitch = min_pitch() + i * pitch_increment()]
73 /// </pre>
74 ///
75 /// If nothing is detected between min_range and max_range, an invalid value of
76 /// DepthSensor::kTooFar is provided. Is something is detected but the
77 /// distance is less than the sensor's minimum sensing range, a value of
78 /// DepthSensor::kTooClose is provided.
79 ///
80 /// Sensing errors can be expressed using a distance value of
81 /// DepthSensorOutput::GetErrorDistance(), and detected using
82 /// DepthSensorOutput::IsError() or DepthSensorOutput::IsValid(). It is not
83 /// expected to occur in this system's output because it models an ideal sensor
84 /// in which sensing errors do not occur. Sensing errors are expected to be used
85 /// by non-ideal depth sensors.
86 ///
87 /// The second output port contains a PoseVector, which is `X_WS`, i.e., the
88 /// transform from this sensor's frame to the world frame. It is useful for
89 /// converting this sensor's point cloud into the world frame.
90 ///
91 /// @ingroup sensor_systems
92 ///
93 /// @see DepthSensorOutput
94 /// @see DepthSensorSpecification
95 ///
96 
97 class DepthSensor : public systems::LeafSystem<double> {
98  public:
100 
101  /// A %DepthSensor constructor.
102  ///
103  /// @param[in] name The name of the depth sensor. This can be any value, but
104  /// should typically be unique among all sensors attached to a particular
105  /// model instance.
106  ///
107  /// @param[in] tree The RigidBodyTree containing the geometric configuration
108  /// of the world. This parameter is aliased by a class member variable. Thus,
109  /// its lifespan must exceed that of this class' instance.
110  ///
111  /// @param[in] frame The frame to which this depth sensor is attached.
112  ///
113  /// @param[in] specification The specifications of this sensor.
114  ///
115  DepthSensor(const std::string& name, const RigidBodyTree<double>& tree,
116  const RigidBodyFrame<double>& frame,
117  const DepthSensorSpecification& specification);
118 
119  /// Returns the RigidBodyTree that this sensor is sensing.
120  const RigidBodyTree<double>& get_tree() const { return tree_; }
121 
122  /// Returns this sensor's frame, which specifies its location and orientation
123  /// in the RigidBodyTree.
124  const RigidBodyFrame<double>& get_frame() const { return frame_; }
125 
126  /// Returns this sensor's specification.
127  const DepthSensorSpecification& get_specification() { return specification_; }
128 
129  /// Returns the number of pixel rows in the resulting depth sensor output.
130  /// This is equal to parameter DepthSensorSpecification::num_pitch_values
131  /// that's passed into the constructor.
132  int get_num_pitch() const { return specification_.num_pitch_values(); }
133 
134  /// Returns the number of pixel columns in the resulting depth sensor output.
135  /// This is equal to parameter DepthSensorSpecification::num_yaw_values
136  /// that's passed into the constructor.
137  int get_num_yaw() const { return specification_.num_yaw_values(); }
138 
139  /// Returns the size of the system's output, which equals the product of
140  /// num_pitch() and num_yaw().
142  return specification_.num_depth_readings();
143  }
144 
145  /// Returns a descriptor of the input port containing the generalized state of
146  /// the RigidBodyTree.
148  const;
149 
150  /// Returns the state output port, which contains the sensor's
151  /// sensed values.
153 
154  /// Returns the `X_WS` output port, which contains the
155  /// transform from this sensor's frame to the world frame.
157 
158  friend std::ostream& operator<<(std::ostream& out,
159  const DepthSensor& depth_sensor);
160 
161  private:
162  // These are calculators for the depth data and sensor pose outputs.
163  void CalcDepthOutput(const Context<double>& context,
164  DepthSensorOutput<double>* data_output) const;
165 
166  void CalcPoseOutput(const Context<double>& context,
167  rendering::PoseVector<double>* pose_output) const;
168 
169 
170  // The depth sensor will cast a ray with its start point at (0,0,0) in the
171  // sensor's base frame (as defined by get_frame()). Its end, also in the
172  // sensor's base frame, is computed by this method and stored in
173  // raycast_endpoints_ at the time of construction. raycast_endpoints_ is only
174  // computed once at the time of construction since the end points are constant
175  // in the sensor's base frame.
176  void PrecomputeRaycastEndpoints();
177 
178  // Applies the min / max range of the sensor. Any measurement that is less
179  // than the minimum or greater than the maximum is set to an invalid value.
180  // This is so users of this sensor can distinguish between an object at the
181  // maximum sensing distance and not detecting any object within the sensing
182  // range.
183  void ApplyLimits(VectorX<double>* dists) const;
184 
185  const std::string name_;
186  const RigidBodyTree<double>& tree_;
187  const RigidBodyFrame<double> frame_;
188  const DepthSensorSpecification specification_;
189  int input_port_index_{};
190  int depth_output_port_index_{};
191  int pose_output_port_index_{};
192 
193  // A cache of where a depth measurement ray endpoint would be if the maximum
194  // range were achieved. This is cached to avoid repeated allocation and
195  // computation.
196  Eigen::Matrix3Xd raycast_endpoints_;
197 };
198 
199 } // namespace sensors
200 } // namespace systems
201 } // namespace drake
int get_num_pitch() const
Returns the number of pixel rows in the resulting depth sensor output.
Definition: depth_sensor.h:132
const InputPortDescriptor< double > & get_rigid_body_tree_state_input_port() const
Returns a descriptor of the input port containing the generalized state of the RigidBodyTree.
Definition: depth_sensor.cc:119
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
const DepthSensorSpecification & get_specification()
Returns this sensor&#39;s specification.
Definition: depth_sensor.h:127
Definition: automotive_demo.cc:88
int num_yaw_values() const
Definition: depth_sensor_specification.h:66
friend std::ostream & operator<<(std::ostream &out, const DepthSensor &depth_sensor)
Definition: depth_sensor.cc:206
const OutputPort< double > & get_sensor_state_output_port() const
Returns the state output port, which contains the sensor&#39;s sensed values.
Definition: depth_sensor.cc:123
DepthSensor(const DepthSensor &)=delete
int num_depth_readings() const
Definition: depth_sensor_specification.cc:41
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
const RigidBodyFrame< double > & get_frame() const
Returns this sensor&#39;s frame, which specifies its location and orientation in the RigidBodyTree.
Definition: depth_sensor.h:124
int get_num_yaw() const
Returns the number of pixel columns in the resulting depth sensor output.
Definition: depth_sensor.h:137
const OutputPort< double > & get_pose_output_port() const
Returns the X_WS output port, which contains the transform from this sensor&#39;s frame to the world fram...
Definition: depth_sensor.cc:127
Holds a DepthSensor&#39;s specification.
Definition: depth_sensor_specification.h:16
const RigidBodyTree< double > & get_tree() const
Returns the RigidBodyTree that this sensor is sensing.
Definition: depth_sensor.h:120
int num_pitch_values() const
Definition: depth_sensor_specification.h:67
A simple model of an ideal depth sensor.
Definition: depth_sensor.h:97
Specializes BasicVector with specific getters and setters that are useful for consumers of DepthSenso...
Definition: depth_sensor_output.h:18
int get_num_depth_readings() const
Returns the size of the system&#39;s output, which equals the product of num_pitch() and num_yaw()...
Definition: depth_sensor.h:141
#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
Provides careful macros to selectively enable or disable the special member functions for copy-constr...