Skip to content

Commit

Permalink
sensors: Move ConvertDepthImageToPointCloud to perception (RobotLocom…
Browse files Browse the repository at this point in the history 
…otion#9972)
  • Loading branch information
@jwnimmer-tri
jwnimmer-tri authored Nov 9, 2018
1 parent 0c41673 commit 72dc768
Show file tree
Hide file tree
Showing 8 changed files with 165 additions and 211 deletions.
38 changes: 36 additions & 2 deletions perception/depth_image_to_point_cloud.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,7 @@ void DepthImageToPointCloud::ConvertDepthImageToPointCloud(
input_depth_image_ptr->GetValue<systems::sensors::ImageDepth32F>();

Eigen::Matrix3Xf point_cloud;
systems::sensors::RgbdCamera::ConvertDepthImageToPointCloud(
input_image, camera_info_, &point_cloud);
Convert(input_image, camera_info_, &point_cloud);

const int kNumCols = point_cloud.cols();
output->resize(kNumCols);
Expand All @@ -41,5 +40,40 @@ void DepthImageToPointCloud::ConvertDepthImageToPointCloud(
}
}

// Note that if `depth_image` holds any pixels that have NaN, the converted
// points will also become NaN.
void DepthImageToPointCloud::Convert(
const systems::sensors::ImageDepth32F& depth_image,
const systems::sensors::CameraInfo& camera_info,
Eigen::Matrix3Xf* point_cloud) {
using InvalidDepth = systems::sensors::InvalidDepth;

if (depth_image.size() != point_cloud->cols()) {
point_cloud->resize(3, depth_image.size());
}

const int height = depth_image.height();
const int width = depth_image.width();
const float cx = camera_info.center_x();
const float cy = camera_info.center_y();
const float fx_inv = 1.f / camera_info.focal_x();
const float fy_inv = 1.f / camera_info.focal_y();

Eigen::Matrix3Xf& pc = *point_cloud;
pc = Eigen::Matrix3Xf::Constant(3, height * width, InvalidDepth::kTooFar);

for (int v = 0; v < height; ++v) {
for (int u = 0; u < width; ++u) {
float z = depth_image.at(u, v)[0];
if (z != InvalidDepth::kTooClose &&
z != InvalidDepth::kTooFar) {
pc(0, v * width + u) = z * (u - cx) * fx_inv;
pc(1, v * width + u) = z * (v - cy) * fy_inv;
pc(2, v * width + u) = z;
}
}
}
}

} // namespace perception
} // namespace drake
20 changes: 20 additions & 0 deletions perception/depth_image_to_point_cloud.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,26 @@ class DepthImageToPointCloud final : public systems::LeafSystem<double> {
return LeafSystem<double>::get_output_port(0);
}

/// Converts a depth image obtained from RgbdCamera to a point cloud. If a
/// pixel in the depth image has NaN depth value, all the `(x, y, z)` values
/// in the converted point will be NaN.
/// Similarly, if a pixel has either InvalidDepth::kTooFar or
/// InvalidDepth::kTooClose, the converted point will be
/// InvalidDepth::kTooFar. Note that this matches the convention used by the
/// Point Cloud Library (PCL).
///
/// @param[in] depth_image The input depth image obtained from RgbdCamera.
///
/// @param[in] camera_info The input camera info which is used for conversion.
///
/// @param[out] point_cloud The pointer of output point cloud.
// TODO(kunimatsu-tri) Use drake::perception::PointCloud instead of
// Eigen::Matrix3Xf and create new constants there instead of reusing
// InvalidDepth.
static void Convert(const systems::sensors::ImageDepth32F& depth_image,
const systems::sensors::CameraInfo& camera_info,
Eigen::Matrix3Xf* point_cloud);

private:
/// Returns an empty point cloud.
PointCloud MakeOutputPointCloud() const;
Expand Down
108 changes: 108 additions & 0 deletions perception/test/depth_image_to_point_cloud_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,114 @@ TEST_F(DepthImageToPointCloudTest, ConversionAndNanValueTest) {
}
}

// Note that we use ASSERTs instead of EXPECTs when we need to traverse many
// pixels in an image or many points in a point cloud. This is to prevent
// outputting massive amount of error messages and making debug hard when
// test failed.

class DepthImageToPointCloudConversionTest : public ::testing::Test {
public:
static constexpr float kFocal = 500.f;
static constexpr int kDepthWidth = 60;
static constexpr int kDepthHeight = 40;

DepthImageToPointCloudConversionTest() : camera_info_(
kDepthWidth, kDepthHeight,
kFocal, kFocal, kDepthWidth * 0.5, kDepthHeight * 0.5),
depth_image_(kDepthWidth, kDepthHeight, 1) {}

// kTooClose is treated as kTooFar. For the detail, refer to the document of
// RgbdCamera::ConvertDepthImageToPointCloud.
void VerifyTooFarTooClose() {
for (int v = 0; v < depth_image_.height(); ++v) {
for (int u = 0; u < depth_image_.width(); ++u) {
const int i = v * depth_image_.width() + u;
Eigen::Vector3f actual = actual_point_cloud_.col(i);
ASSERT_EQ(actual(0), systems::sensors::InvalidDepth::kTooFar);
ASSERT_EQ(actual(1), systems::sensors::InvalidDepth::kTooFar);
ASSERT_EQ(actual(2), systems::sensors::InvalidDepth::kTooFar);
}
}
}

protected:
// This must be called by all the test cases first.
void Init(float depth_value) {
std::fill(depth_image_.at(0, 0),
depth_image_.at(0, 0) + depth_image_.size(),
depth_value);
}

const systems::sensors::CameraInfo camera_info_;
systems::sensors::ImageDepth32F depth_image_;
Eigen::Matrix3Xf actual_point_cloud_;
};

// Verifies computed point cloud when pixel values in depth image are valid.
TEST_F(DepthImageToPointCloudConversionTest, ValidValueTest) {
constexpr float kDepthValue = 1.f;
Init(kDepthValue);

DepthImageToPointCloud::Convert(
depth_image_, camera_info_, &actual_point_cloud_);

// This tolerance was determined empirically using Drake's supported
// platforms.
constexpr float kDistanceTolerance = 1e-8;
for (int v = 0; v < depth_image_.height(); ++v) {
for (int u = 0; u < depth_image_.width(); ++u) {
const int i = v * depth_image_.width() + u;
Eigen::Vector3f actual = actual_point_cloud_.col(i);

const double expected_x =
kDepthValue * (u - kDepthWidth * 0.5) / kFocal;
ASSERT_NEAR(actual(0), expected_x, kDistanceTolerance);
const double expected_y =
kDepthValue * (v - kDepthHeight * 0.5) / kFocal;
ASSERT_NEAR(actual(1), expected_y, kDistanceTolerance);
ASSERT_NEAR(actual(2), kDepthValue, kDistanceTolerance);
}
}
}

// Verifies computed point cloud when pixel values in depth image are NaN.
TEST_F(DepthImageToPointCloudConversionTest, NanValueTest) {
Init(std::numeric_limits<float>::quiet_NaN());

DepthImageToPointCloud::Convert(
depth_image_, camera_info_, &actual_point_cloud_);

for (int v = 0; v < depth_image_.height(); ++v) {
for (int u = 0; u < depth_image_.width(); ++u) {
const int i = v * depth_image_.width() + u;
Eigen::Vector3f actual = actual_point_cloud_.col(i);
ASSERT_TRUE(std::isnan(actual(0)));
ASSERT_TRUE(std::isnan(actual(1)));
ASSERT_TRUE(std::isnan(actual(2)));
}
}
}

// Verifies computed point cloud when pixel values in depth image are kTooFar.
TEST_F(DepthImageToPointCloudConversionTest, TooFarTest) {
Init(systems::sensors::InvalidDepth::kTooFar);

DepthImageToPointCloud::Convert(
depth_image_, camera_info_, &actual_point_cloud_);

VerifyTooFarTooClose();
}

// Verifies computed point cloud when pixel values in depth image are kTooClose.
TEST_F(DepthImageToPointCloudConversionTest, TooCloseTest) {
Init(systems::sensors::InvalidDepth::kTooClose);

DepthImageToPointCloud::Convert(
depth_image_, camera_info_, &actual_point_cloud_);

VerifyTooFarTooClose();
}

} // namespace
} // namespace perception
} // namespace drake
31 changes: 0 additions & 31 deletions systems/sensors/dev/rgbd_camera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,37 +24,6 @@ using geometry::dev::QueryObject;
using geometry::dev::render::DepthCameraProperties;
using geometry::dev::SceneGraph;

// Note that if `depth_image` holds any pixels that have NaN, the converted
// points will also become NaN.
void RgbdCamera::ConvertDepthImageToPointCloud(const ImageDepth32F& depth_image,
const CameraInfo& camera_info,
Eigen::Matrix3Xf* point_cloud) {
if (depth_image.size() != point_cloud->cols()) {
point_cloud->resize(3, depth_image.size());
}

const int height = depth_image.height();
const int width = depth_image.width();
const float cx = camera_info.center_x();
const float cy = camera_info.center_y();
const float fx_inv = 1.f / camera_info.focal_x();
const float fy_inv = 1.f / camera_info.focal_y();

Eigen::Matrix3Xf& pc = *point_cloud;
pc = Eigen::Matrix3Xf::Constant(3, height * width, InvalidDepth::kTooFar);

for (int v = 0; v < height; ++v) {
for (int u = 0; u < width; ++u) {
float z = depth_image.at(u, v)[0];
if (z != InvalidDepth::kTooClose && z != InvalidDepth::kTooFar) {
pc(0, v * width + u) = z * (u - cx) * fx_inv;
pc(1, v * width + u) = z * (v - cy) * fy_inv;
pc(2, v * width + u) = z;
}
}
}
}

RgbdCamera::RgbdCamera(const std::string& name, const Eigen::Vector3d& p_WB_W,
const Eigen::Vector3d& rpy_WB_W,
const DepthCameraProperties& properties,
Expand Down
23 changes: 1 addition & 22 deletions systems/sensors/dev/rgbd_camera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,30 +72,9 @@ class RgbdCamera final : public LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RgbdCamera)

// TODO(SeanCurtis-TRI): Move this into a generic utility/system. After all,
// it just needs to take an input image and produce a point cloud.
/** Converts a depth image obtained from RgbdCamera to a point cloud. If a
pixel in the depth image has NaN depth value, all the `(x, y, z)` values
in the converted point will be NaN.
Similarly, if a pixel has either InvalidDepth::kTooFar or
InvalidDepth::kTooClose, the converted point will be
InvalidDepth::kTooFar. Note that this matches the convention used by the
Point Cloud Library (PCL).
@param[in] depth_image The input depth image obtained from RgbdCamera.
@param[in] camera_info The input camera info which is used for conversion.
@param[out] point_cloud The pointer of output point cloud. */
// TODO(kunimatsu-tri) Use drake::perception::PointCloud instead of
// Eigen::Matrix3Xf and create new constants there instead of reusing
// InvalidDepth.
// TODO(kunimatsu-tri) Move this to drake::perception.
static void ConvertDepthImageToPointCloud(
const ImageDepth32F& depth_image,
const CameraInfo& camera_info, Eigen::Matrix3Xf* point_cloud);

/** Constructs an %RgbdCamera that is fixed to the world frame with the given
pose and given camera properties.
@param name The name of the RgbdCamera. This can be any value, but
typically should be unique among all sensors attached
to a particular model instance.
Expand Down
32 changes: 0 additions & 32 deletions systems/sensors/rgbd_camera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,38 +17,6 @@ namespace drake {
namespace systems {
namespace sensors {

// Note that if `depth_image` holds any pixels that have NaN, the converted
// points will also become NaN.
void RgbdCamera::ConvertDepthImageToPointCloud(const ImageDepth32F& depth_image,
const CameraInfo& camera_info,
Eigen::Matrix3Xf* point_cloud) {
if (depth_image.size() != point_cloud->cols()) {
point_cloud->resize(3, depth_image.size());
}

const int height = depth_image.height();
const int width = depth_image.width();
const float cx = camera_info.center_x();
const float cy = camera_info.center_y();
const float fx_inv = 1.f / camera_info.focal_x();
const float fy_inv = 1.f / camera_info.focal_y();

Eigen::Matrix3Xf& pc = *point_cloud;
pc = Eigen::Matrix3Xf::Constant(3, height * width, InvalidDepth::kTooFar);

for (int v = 0; v < height; ++v) {
for (int u = 0; u < width; ++u) {
float z = depth_image.at(u, v)[0];
if (z != InvalidDepth::kTooClose &&
z != InvalidDepth::kTooFar) {
pc(0, v * width + u) = z * (u - cx) * fx_inv;
pc(1, v * width + u) = z * (v - cy) * fy_inv;
pc(2, v * width + u) = z;
}
}
}
}

RgbdCamera::RgbdCamera(const std::string& name,
const RigidBodyTree<double>& tree,
const Eigen::Vector3d& position,
Expand Down
21 changes: 0 additions & 21 deletions systems/sensors/rgbd_camera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,27 +65,6 @@ class RgbdCamera final : public LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RgbdCamera)

/// Converts a depth image obtained from RgbdCamera to a point cloud. If a
/// pixel in the depth image has NaN depth value, all the `(x, y, z)` values
/// in the converted point will be NaN.
/// Similarly, if a pixel has either InvalidDepth::kTooFar or
/// InvalidDepth::kTooClose, the converted point will be
/// InvalidDepth::kTooFar. Note that this matches the convention used by the
/// Point Cloud Library (PCL).
///
/// @param[in] depth_image The input depth image obtained from RgbdCamera.
///
/// @param[in] camera_info The input camera info which is used for conversion.
///
/// @param[out] point_cloud The pointer of output point cloud.
// TODO(kunimatsu-tri) Use drake::perception::PointCloud instead of
// Eigen::Matrix3Xf and create new constants there instead of reusing
// InvalidDepth.
// TODO(kunimatsu-tri) Move this to drake::perception.
static void ConvertDepthImageToPointCloud(const ImageDepth32F& depth_image,
const CameraInfo& camera_info,
Eigen::Matrix3Xf* point_cloud);

/// A constructor that uses the default RgbdRendererVTK renderer and
/// defines `B` using Euler angles.
/// The pose of %RgbdCamera will be fixed to the world coordinate system
Expand Down
Loading

0 comments on commit 72dc768

Please sign in to comment.