Skip to content

Commit

Permalink
Merge pull request RobotLocomotion#6176 from nikos-tri/depth-sensor-mods
Browse files Browse the repository at this point in the history 
Extracted reusable functionality from DoCalcOutput, renamed methods related to pitch and yaw
  • Loading branch information
@sherm1
sherm1 authored May 26, 2017
2 parents 8f2abb8 + a6bbb8a commit d7f3c01
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 52 deletions.
70 changes: 34 additions & 36 deletions drake/systems/sensors/depth_sensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,7 @@ DepthSensor::DepthSensor(const std::string& name,
const RigidBodyTree<double>& tree,
const RigidBodyFrame<double>& frame,
const DepthSensorSpecification& specification)
: name_(name),
tree_(tree),
frame_(frame),
specification_(specification) {
: name_(name), tree_(tree), frame_(frame), specification_(specification) {
DRAKE_DEMAND(specification_.min_yaw() <= specification_.max_yaw() &&
"min_yaw must be less than or equal to max_yaw.");
DRAKE_DEMAND(specification_.min_pitch() <= specification_.max_pitch() &&
Expand All @@ -62,10 +59,11 @@ DepthSensor::DepthSensor(const std::string& name,
DeclareInputPort(kVectorValued,
tree.get_num_positions() + tree.get_num_velocities())
.get_index();
depth_output_port_index_ = DeclareVectorOutputPort(
DepthSensorOutput<double>(specification_)).get_index();
pose_output_port_index_ = DeclareVectorOutputPort(
PoseVector<double>()).get_index();
depth_output_port_index_ =
DeclareVectorOutputPort(DepthSensorOutput<double>(specification_))
.get_index();
pose_output_port_index_ =
DeclareVectorOutputPort(PoseVector<double>()).get_index();
PrecomputeRaycastEndpoints();
}

Expand All @@ -80,23 +78,23 @@ void DepthSensor::PrecomputeRaycastEndpoints() {
// (3) pitch = PI and and pitch = -PI for a given yaw
// (3) yaw > 2 * PI
//
for (int i = 0; i < get_num_pixel_rows(); ++i) {
for (int i = 0; i < get_num_pitch(); ++i) {
double pitch =
specification_.min_pitch() + i * specification_.pitch_increment();

// If this is the top-most row, set the pitch equal to max_pitch_. This is
// necessary to account for small inaccuracies due to floating point
// arithmetic.
if (i == get_num_pixel_rows() - 1) pitch = specification_.max_pitch();
if (i == get_num_pitch() - 1) pitch = specification_.max_pitch();

for (int j = 0; j < get_num_pixel_cols(); ++j) {
for (int j = 0; j < get_num_yaw(); ++j) {
double yaw =
specification_.min_yaw() + j * specification_.yaw_increment();

// If this is the right-most column, set the yaw equal to max_yaw_.
// This is necessary to account for small inaccuracies due to floating
// point arithmetic.
if (j == get_num_pixel_cols() - 1) yaw = specification_.max_yaw();
if (j == get_num_yaw() - 1) yaw = specification_.max_yaw();

// Compute the location of the raycast end point assuming a max sensing
// range of one and no occlusions. This is done using the same equations
Expand All @@ -109,7 +107,7 @@ void DepthSensor::PrecomputeRaycastEndpoints() {
// the range cast end point exceeds the maximum range of the sensor. This
// is so we can detect when an object is sensed at precisely the maximum
// range of the sensor.
raycast_endpoints_.col(i * get_num_pixel_cols() + j) =
raycast_endpoints_.col(i * get_num_yaw() + j) =
1.1 * specification_.max_range() * Vector3<double>(x, y, z);
}
}
Expand Down Expand Up @@ -162,29 +160,30 @@ void DepthSensor::DoCalcOutput(const systems::Context<double>& context,
const_cast<RigidBodyTree<double>&>(tree_).collisionRaycast(
kinematics_cache, origin, raycast_endpoints_world, distances);

// Applies the min / max range of the sensor. Any measurement that is less
// than the minimum or greater than the maximum is set to an invalid value.
// This is so users of this sensor can distinguish between an object at the
// maximum sensing distance and not detecting any object within the sensing
// range.
for (int i = 0; i < distances.size(); ++i) {
if (distances[i] < 0) {
// Infinity distance measurements show up as -1.
if (distances[i] == -1) {
distances[i] = DepthSensorOutput<double>::GetTooFarDistance();
} else {
drake::log()->warn("Measured distance was < 0 and != -1: " +
std::to_string(distances[i]));
distances[i] = DepthSensorOutput<double>::GetErrorDistance();
}
} else if (distances[i] > specification_.max_range()) {
distances[i] = DepthSensorOutput<double>::GetTooFarDistance();
} else if (distances[i] < specification_.min_range()) {
distances[i] = DepthSensorOutput<double>::GetTooCloseDistance();
ApplyLimits(&distances);

UpdateOutputs(distances, kinematics_cache, output);
}

void DepthSensor::ApplyLimits(VectorX<double>* distances) const {
VectorX<double>& d = *distances;

for (int i = 0; i < d.size(); ++i) {
// Infinity distance measurements show up as -1.
DRAKE_DEMAND((d[i] >= 0) || (d[i] == -1));
if (d[i] == -1) {
d[i] = DepthSensorOutput<double>::GetTooFarDistance();
} else if (d[i] > specification_.max_range()) {
d[i] = DepthSensorOutput<double>::GetTooFarDistance();
} else if (d[i] < specification_.min_range()) {
d[i] = DepthSensorOutput<double>::GetTooCloseDistance();
}
}
}

// Evaluates the output port containing the depth measurements.
void DepthSensor::UpdateOutputs(const VectorX<double>& distances,
const KinematicsCache<double>& kinematics_cache,
SystemOutput<double>* output) const {
BasicVector<double>* data_output =
output->GetMutableVectorData(depth_output_port_index_);
DRAKE_ASSERT(data_output != nullptr);
Expand All @@ -193,9 +192,8 @@ void DepthSensor::DoCalcOutput(const systems::Context<double>& context,
// Evaluates the output port containing X_WS.
const drake::Isometry3<double> X_WS =
tree_.CalcFramePoseInWorldFrame(kinematics_cache, frame_);
PoseVector<double>* pose_output =
dynamic_cast<PoseVector<double>*>(
output->GetMutableVectorData(pose_output_port_index_));
PoseVector<double>* pose_output = dynamic_cast<PoseVector<double>*>(
output->GetMutableVectorData(pose_output_port_index_));
DRAKE_ASSERT(pose_output != nullptr);
pose_output->set_translation(
Eigen::Translation<double, 3>(X_WS.translation()));
Expand Down
45 changes: 29 additions & 16 deletions drake/systems/sensors/depth_sensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,11 @@ namespace sensors {

/// A simple model of an ideal depth sensor. Example real-world depth sensors
/// include Lidar, IR, sonar, etc. The depth measurements are taken at evenly
/// spaced pixel rows (pitch angles) and columns (yaw angles). Ray casting is
/// used to obtain the depth measurements and all pitch / yaw combinations in a
/// single resulting depth image are obtained at a single effective point in
/// time. Thus, this sensor does *not* model aliasing effects due to the time
/// spent scanning vertically and horizontally.
/// spaced pitch angles and yaw angles. Ray casting is used to obtain the depth
/// measurements and all pitch / yaw combinations in a single resulting depth
/// image are obtained at a single effective point in time. Thus, this sensor
/// does *not* model aliasing effects due to the time spent scanning vertically
/// and horizontally.
///
/// There are two frames associated with this sensor: its base frame and its
/// optical frame. This sensor's specification and configuration are defined in
Expand Down Expand Up @@ -59,14 +59,14 @@ namespace sensors {
///
/// This system has two output ports. The first contains the sensed values
/// stored in a DepthSensorOutput object. For each pitch, there is a fixed
/// number of yaw values as specified by num_pixel_cols(). Each of these vector
/// number of yaw values as specified by num_yaw(). Each of these vector
/// of yaw values that share the same pitch are contiguous in the output vector.
/// In other words, here is pseudocode describing this sensor's output vector:
///
/// <pre>
/// for i in 0 to num_pixel_rows():
/// for j in 0 to num_pixel_cols():
/// output_vector[i * num_pixel_cols() + j] ==
/// for i in 0 to num_pitch():
/// for j in 0 to num_yaw():
/// output_vector[i * num_yaw() + j] ==
/// [depth value when yaw = min_yaw() + j * yaw_increment() and
/// pitch = min_pitch() + i * pitch_increment()]
/// </pre>
Expand All @@ -92,6 +92,7 @@ namespace sensors {
/// @see DepthSensorOutput
/// @see DepthSensorSpecification
///

class DepthSensor : public systems::LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DepthSensor)
Expand Down Expand Up @@ -125,17 +126,17 @@ class DepthSensor : public systems::LeafSystem<double> {
const DepthSensorSpecification& get_specification() { return specification_; }

/// Returns the number of pixel rows in the resulting depth sensor output.
/// This is equal to parameter `num_pitch_values` that's passed into the
/// constructor.
int get_num_pixel_rows() const { return specification_.num_pitch_values(); }
/// This is equal to parameter DepthSensorSpecification::num_pitch_values
/// that's passed into the constructor.
int get_num_pitch() const { return specification_.num_pitch_values(); }

/// Returns the number of pixel columns in the resulting depth sensor output.
/// This is equal to parameter `num_yaw_values` that's passed into the
/// constructor.
int get_num_pixel_cols() const { return specification_.num_yaw_values(); }
/// This is equal to parameter DepthSensorSpecification::num_yaw_values
/// that's passed into the constructor.
int get_num_yaw() const { return specification_.num_yaw_values(); }

/// Returns the size of the system's output, which equals the product of
/// num_pixel_rows() and num_pixel_cols().
/// num_pitch() and num_yaw().
int get_num_depth_readings() const {
return specification_.num_depth_readings();
}
Expand Down Expand Up @@ -170,6 +171,18 @@ class DepthSensor : public systems::LeafSystem<double> {
// in the sensor's base frame.
void PrecomputeRaycastEndpoints();

// Applies the min / max range of the sensor. Any measurement that is less
// than the minimum or greater than the maximum is set to an invalid value.
// This is so users of this sensor can distinguish between an object at the
// maximum sensing distance and not detecting any object within the sensing
// range.
void ApplyLimits(VectorX<double>* dists) const;

// Evaluates the output port containing the depth measurements.
void UpdateOutputs(const VectorX<double>& distances,
const KinematicsCache<double>& kinematics_cache,
SystemOutput<double>* output) const;

const std::string name_;
const RigidBodyTree<double>& tree_;
const RigidBodyFrame<double> frame_;
Expand Down

0 comments on commit d7f3c01

Please sign in to comment.