Filter rigid body geometries from a point cloud (RobotLocomotion#9280)

perception/BUILD.bazel

@@ -17,6 +17,7 @@ drake_cc_package_library(
         ":depth_image_to_point_cloud",
         ":point_cloud",
         ":point_cloud_flags",
+        ":rigid_body_point_cloud_filter",
         ":transform_point_cloud",
     ],
 )
@@ -75,6 +76,18 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_library(
+    name = "rigid_body_point_cloud_filter",
+    srcs = ["rigid_body_point_cloud_filter.cc"],
+    hdrs = ["rigid_body_point_cloud_filter.h"],
+    deps = [
+        ":point_cloud",
+        "//common:essential",
+        "//multibody:rigid_body_tree",
+        "//systems/framework",
+    ],
+)
+
 drake_cc_googletest(
     name = "point_cloud_flags_test",
     srcs = ["test/point_cloud_flags_test.cc"],
@@ -92,6 +105,17 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "rigid_body_point_cloud_filter_test",
+    srcs = ["test/rigid_body_point_cloud_filter_test.cc"],
+    deps = [
+        ":point_cloud",
+        ":rigid_body_point_cloud_filter",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//multibody:rigid_body_tree",
+    ],
+)
+
 drake_cc_googletest(
     name = "transform_point_cloud_test",
     srcs = ["test/transform_point_cloud_test.cc"],

perception/rigid_body_point_cloud_filter.cc

@@ -0,0 +1,72 @@
+#include "drake/perception/rigid_body_point_cloud_filter.h"
+
+#include <algorithm>
+#include <unordered_set>
+#include <vector>
+
+namespace drake {
+namespace perception {
+
+RigidBodyPointCloudFilter::RigidBodyPointCloudFilter(
+    RigidBodyTree<double>* tree, double collision_threshold)
+    : tree_(tree), collision_threshold_(collision_threshold) {
+  // Create input port for point cloud.
+  point_cloud_input_port_index_ = DeclareAbstractInputPort().get_index();
+
+  // Create input port for tree positions and velocities.
+  state_input_port_index_ =
+      DeclareInputPort(systems::kVectorValued,
+                       tree_->get_num_positions() + tree_->get_num_velocities())
+          .get_index();
+
+  // Create output port for filtered point cloud.
+  DeclareAbstractOutputPort(&RigidBodyPointCloudFilter::MakeOutputPointCloud,
+                            &RigidBodyPointCloudFilter::FilterPointCloud);
+}
+
+PointCloud RigidBodyPointCloudFilter::MakeOutputPointCloud() const {
+  PointCloud cloud(0);
+  return cloud;
+}
+
+void RigidBodyPointCloudFilter::FilterPointCloud(
+    const systems::Context<double>& context, PointCloud* output) const {
+  // 1. Create the list of points to be considered.
+  const PointCloud* input_cloud =
+      EvalInputValue<PointCloud>(context, point_cloud_input_port_index_);
+  DRAKE_ASSERT(input_cloud != nullptr);
+
+  std::vector<Eigen::Vector3d> points;
+  points.resize(input_cloud->size());
+  for (int i = 0; i < input_cloud->size(); i++) {
+    points[i] = input_cloud->xyz(i).cast<double>();
+  }
+
+  // 2. Extract the indices of the points in collision.
+  const Eigen::VectorXd q =
+      EvalEigenVectorInput(context, state_input_port_index_)
+          .head(tree_->get_num_positions());
+  const KinematicsCache<double> kinematics_cache = tree_->doKinematics(q);
+  std::vector<size_t> filtered_point_indices =
+      tree_->collidingPoints(kinematics_cache, points, collision_threshold_);
+
+  // 3. Create a new point cloud without the colliding points.
+  if (!filtered_point_indices.empty()) {
+    std::unordered_set<size_t> unique_indices(filtered_point_indices.begin(),
+        filtered_point_indices.end());
+    DRAKE_DEMAND(unique_indices.size() <= points.size());
+    output->resize(points.size() - unique_indices.size());
+    int k = 0;
+    for (size_t i = 0; i < points.size(); ++i) {
+      if (unique_indices.find(i) == unique_indices.end()) {
+        output->mutable_xyz(k) = points[i].cast<float>();
+        k++;
+      }
+    }
+  } else {
+    *output = *input_cloud;
+  }
+}
+
+}  // namespace perception
+}  // namespace drake

perception/rigid_body_point_cloud_filter.h

@@ -0,0 +1,73 @@
+#pragma once
+
+#include "drake/common/drake_copyable.h"
+#include "drake/multibody/rigid_body_tree.h"
+#include "drake/perception/point_cloud.h"
+#include "drake/systems/framework/context.h"
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace perception {
+
+/// Removes known geometries from point clouds.
+///
+/// Given a RigidBodyTree, the system takes a point cloud and the state of the
+/// RigidBodyTree as input, and produces a filtered point cloud as output from
+/// which all points expected to belong to the rigid bodies comprising the
+/// RigidBodyTree have been removed.
+///
+/// The system has two inpt ports and one output port. The first input port
+/// consumes a PointCloud and the second takes the state of the RigidBodyTree.
+/// The ouput port contains the filtered PointCloud.
+class RigidBodyPointCloudFilter final : public systems::LeafSystem<double> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RigidBodyPointCloudFilter)
+
+  /// Constructs the filter given a RigidBodyTree.
+  ///
+  /// @param[in] tree The RigidBodyTree containing the geometric configuration
+  /// of the world. Notice that calculating the filter's output updates the
+  /// `tree`'s collision model.
+  /// @param[in] collision_threshold The threshold for the collision
+  /// detection that determines which points to remove from the point cloud.
+  ///
+  /// The `tree` object must remain valid for the duration of this object.
+  RigidBodyPointCloudFilter(RigidBodyTree<double>* tree,
+                            double collision_threshold);
+
+  /// Returns the vector valued input port that contains a vector of `q, v`
+  /// corresponding to the positions and velocities associated with a
+  /// RigidBodyTree.
+  const systems::InputPort<double>& state_input_port() const {
+    return this->get_input_port(state_input_port_index_);
+  }
+
+  /// Returns the abstract valued input port that contains a PointCloud.
+  const systems::InputPort<double>& point_cloud_input_port() const {
+    return this->get_input_port(point_cloud_input_port_index_);
+  }
+
+  /// Returns the abstract valued output port that contains a PointCloud.
+  const systems::OutputPort<double>& point_cloud_output_port() const {
+    return LeafSystem<double>::get_output_port(0);
+  }
+
+ private:
+  // Returns an empty point cloud.
+  PointCloud MakeOutputPointCloud() const;
+
+  // Filters the point cloud by removing those geometries that are known
+  // from the RigidBodyTree `tree_`.
+  void FilterPointCloud(const systems::Context<double>& context,
+                        PointCloud* output) const;
+
+  RigidBodyTree<double>* tree_;
+
+  int point_cloud_input_port_index_{-1};
+  int state_input_port_index_{-1};
+
+  double collision_threshold_{};
+};
+
+}  // namespace perception
+}  // namespace drake

perception/test/rigid_body_point_cloud_filter_test.cc

@@ -0,0 +1,154 @@
+#include "drake/perception/rigid_body_point_cloud_filter.h"
+
+#include <cmath>
+#include <random>
+
+#include <gtest/gtest.h>
+
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/multibody/joints/fixed_joint.h"
+#include "drake/multibody/rigid_body_tree.h"
+
+namespace drake {
+namespace perception {
+namespace {
+
+const double kCollisionThreshold = 0.01;
+const Eigen::Vector3d kBoxPosition(0., 0., 0.);
+const Eigen::Vector3d kBoxSize(0.2, 0.2, 0.2);
+const Eigen::Vector3f kPoint1(0.3, 0., 0.);
+const Eigen::Vector3f kPoint2(0., -0.42, 0.);
+const Eigen::Vector3f kPoint3(0., 0., 0.35);
+const int kPointsPerFace = 10;
+const int kFaces = 6;
+
+class RigidBodyPointCloudFilterTest : public ::testing::Test {
+ public:
+  // Checks if the point cloud `cloud` contains the point `x`.
+  static bool ContainsPoint(const PointCloud& cloud, const Vector3<float>& x) {
+    for (int i = 0; i < cloud.size(); i++) {
+      if (cloud.xyz(i)(0) == x(0) && cloud.xyz(i)(1) == x(1) &&
+          cloud.xyz(i)(2) == x(2)) {
+        return true;
+      }
+    }
+    return false;
+  }
+
+ protected:
+  void SetUp() override {
+    tree_ = std::make_unique<RigidBodyTree<double>>();
+
+    AddBox(tree_.get(), kBoxPosition, kBoxSize);
+    tree_->compile();
+
+    filter_ = std::make_unique<RigidBodyPointCloudFilter>(tree_.get(),
+                                                          kCollisionThreshold);
+
+    cloud_input_ = std::make_unique<PointCloud>(MakePointCloudFromBox(
+        kBoxPosition.cast<float>(), kBoxSize.cast<float>()));
+
+    // Add three points to the point cloud. These points should remain after
+    // filtering.
+    const int size = cloud_input_->size();
+    cloud_input_->resize(size + 3);
+    cloud_input_->mutable_xyz(size) = kPoint1;
+    cloud_input_->mutable_xyz(size + 1) = kPoint2;
+    cloud_input_->mutable_xyz(size + 2) = kPoint3;
+
+    state_input_ = VectorX<double>::Zero(tree_->get_num_positions() +
+                                         tree_->get_num_velocities());
+
+    context_ = filter_->CreateDefaultContext();
+    context_->FixInputPort(
+        filter_->point_cloud_input_port().get_index(),
+        systems::AbstractValue::Make<PointCloud>(*cloud_input_.get()));
+    context_->FixInputPort(filter_->state_input_port().get_index(),
+                           state_input_);
+
+    output_ = filter_->point_cloud_output_port().Allocate();
+  }
+
+  std::unique_ptr<RigidBodyTree<double>> tree_;
+  std::unique_ptr<RigidBodyPointCloudFilter> filter_;
+  std::unique_ptr<systems::Context<double>> context_;
+  VectorX<double> state_input_;
+  std::unique_ptr<PointCloud> cloud_input_;
+  std::unique_ptr<systems::AbstractValue> output_;
+
+ private:
+  // Adds a RigidBody with a box shape to the RigidBodyTree at a fixed location.
+  void AddBox(RigidBodyTree<double>* tree, const Eigen::Vector3d& position,
+              const Eigen::Vector3d& size) {
+    auto body = std::make_unique<RigidBody<double>>();
+    body->set_name("box");
+    body->set_mass(1.0);
+    body->set_spatial_inertia(Matrix6<double>::Identity());
+
+    const DrakeShapes::Box shape(size);
+    const Eigen::Vector4d material(0.0, 0.0, 0.5, 1.0);
+    const DrakeShapes::VisualElement visual_element(
+        shape, Eigen::Isometry3d::Identity(), material);
+    body->AddVisualElement(visual_element);
+
+    Eigen::Isometry3d joint_transform;
+    {
+      const drake::math::RotationMatrix<double> kRotIdentity;
+      joint_transform.matrix() << kRotIdentity.matrix(), position, 0, 0, 0, 1;
+    }
+    auto joint = std::make_unique<FixedJoint>("box_joint", joint_transform);
+    body->add_joint(&tree->world(), std::move(joint));
+
+    RigidBody<double>* body_in_tree = tree->add_rigid_body(std::move(body));
+
+    drake::multibody::collision::Element collision(
+        shape, Isometry3<double>::Identity());
+    tree->addCollisionElement(collision, *body_in_tree, "default");
+  }
+
+  // Creates a point cloud from an axis aligned box.
+  PointCloud MakePointCloudFromBox(const Eigen::Vector3f& position,
+                                   const Eigen::Vector3f& size) {
+    PointCloud cloud(kFaces * kPointsPerFace);
+    for (int i = 0; i < kFaces; i++) {
+      int dimension = i / 2;
+      int sign = (i + 1) % 2 ? -1 : 1;
+      for (int j = 0; j < kPointsPerFace; j++) {
+        int index = i * kPointsPerFace + j;
+        cloud.mutable_xyz(index) = position;
+        cloud.mutable_xyz(index)(dimension) += sign * 0.5 * size(dimension);
+      }
+    }
+    return cloud;
+  }
+
+  // Finds the points in `cloud` that collide with some body in `tree`.
+  std::vector<size_t> CollidingPoints(const PointCloud& cloud,
+                                      RigidBodyTree<double>* tree) {
+    KinematicsCache<double> kinematics_cache = tree->doKinematics(state_input_);
+    std::vector<Eigen::Vector3d> points;
+    points.resize(cloud.size());
+    for (int i = 0; i < cloud.size(); i++) {
+      points[i] = cloud.xyz(i).cast<double>();
+    }
+    std::vector<size_t> indices =
+        tree->collidingPoints(kinematics_cache, points, kCollisionThreshold);
+    return indices;
+  }
+};
+
+TEST_F(RigidBodyPointCloudFilterTest, RemoveBoxTest) {
+  // Calculate the system's actual output.
+  filter_->point_cloud_output_port().Calc(*context_, output_.get());
+  PointCloud output_cloud = output_->GetValueOrThrow<PointCloud>();
+
+  EXPECT_GT(output_cloud.size(), 0);
+
+  EXPECT_TRUE(ContainsPoint(output_cloud, kPoint1));
+  EXPECT_TRUE(ContainsPoint(output_cloud, kPoint2));
+  EXPECT_TRUE(ContainsPoint(output_cloud, kPoint3));
+}
+
+}  // namespace
+}  // namespace perception
+}  // namespace drake