Always use the 3D voxel filter. (#61)

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -182,7 +182,7 @@ void ConstraintBuilder::ComputeConstraint(
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_index);
   const sensor::PointCloud filtered_point_cloud =
-      sensor::ToPointCloud(adaptive_voxel_filter_.Filter(*point_cloud));
+      adaptive_voxel_filter_.Filter(sensor::ToPointCloud(*point_cloud));
 
   // The 'constraint_transform' (i <- j) is computed from:
   // - a 'filtered_point_cloud' in j,

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -72,8 +72,8 @@ class SparsePoseGraphTest : public ::testing::Test {
               sampling_ratio = 1.,
               max_constraint_distance = 6.,
               adaptive_voxel_filter = {
-                max_length = 1e-5,
+                max_length = 1e-2,
-                min_num_points = 1,
+                min_num_points = 1000,
                 max_range = 50.,
               },
               min_score = 0.5,

cartographer/sensor/voxel_filter.cc

@@ -25,11 +25,10 @@ namespace sensor {
 
 namespace {
 
-template <typename PointCloudType>
+PointCloud FilterByMaxRange(const PointCloud& point_cloud,
-PointCloudType FilterByMaxRange(const PointCloudType& point_cloud,
                             const float max_range) {
-  PointCloudType result;
+  PointCloud result;
-  for (const auto& point : point_cloud) {
+  for (const Eigen::Vector3f& point : point_cloud) {
     if (point.norm() <= max_range) {
       result.push_back(point);
     }
@@ -37,15 +36,14 @@ PointCloudType FilterByMaxRange(const PointCloudType& point_cloud,
   return result;
 }
 
-template <typename PointCloudType>
+PointCloud AdaptivelyVoxelFiltered(
-PointCloudType AdaptivelyVoxelFiltered(
     const proto::AdaptiveVoxelFilterOptions& options,
-    const PointCloudType& point_cloud) {
+    const PointCloud& point_cloud) {
   if (point_cloud.size() <= options.min_num_points()) {
     // 'point_cloud' is already sparse enough.
     return point_cloud;
   }
-  PointCloudType result = VoxelFiltered(point_cloud, options.max_length());
+  PointCloud result = VoxelFiltered(point_cloud, options.max_length());
   if (result.size() >= options.min_num_points()) {
     // Filtering with 'max_length' resulted in a sufficiently dense point cloud.
     return result;
@@ -63,7 +61,7 @@ PointCloudType AdaptivelyVoxelFiltered(
       // edge length is at most 10% off.
       while ((high_length - low_length) / low_length > 1e-1f) {
         const float mid_length = (low_length + high_length) / 2.f;
-        const PointCloudType candidate = VoxelFiltered(point_cloud, mid_length);
+        const PointCloud candidate = VoxelFiltered(point_cloud, mid_length);
         if (candidate.size() >= options.min_num_points()) {
           low_length = mid_length;
           result = candidate;
@@ -79,37 +77,16 @@ PointCloudType AdaptivelyVoxelFiltered(
 
 }  // namespace
 
-PointCloud2D VoxelFiltered(const PointCloud2D& point_cloud, const float size) {
+PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
   VoxelFilter voxel_filter(size);
   voxel_filter.InsertPointCloud(point_cloud);
   return voxel_filter.point_cloud();
 }
 
-PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
+VoxelFilter::VoxelFilter(const float size)
-  VoxelFilter3D voxel_filter(size);
-  voxel_filter.InsertPointCloud(point_cloud);
-  return voxel_filter.point_cloud();
-}
-
-VoxelFilter::VoxelFilter(const float size) : size_(size) {}
-
-void VoxelFilter::InsertPointCloud(const PointCloud2D& point_cloud) {
-  for (const Eigen::Vector2f& point : point_cloud) {
-    if (voxels_
-            .emplace(common::RoundToInt64(point.x() / size_),
-                     common::RoundToInt64(point.y() / size_))
-            .second) {
-      point_cloud_.push_back(point);
-    }
-  }
-}
-
-const PointCloud2D& VoxelFilter::point_cloud() const { return point_cloud_; }
-
-VoxelFilter3D::VoxelFilter3D(const float size)
     : voxels_(size, Eigen::Vector3f::Zero()) {}
 
-void VoxelFilter3D::InsertPointCloud(const PointCloud& point_cloud) {
+void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) {
   for (const Eigen::Vector3f& point : point_cloud) {
     auto* const value = voxels_.mutable_value(voxels_.GetCellIndex(point));
     if (*value == 0) {
@@ -119,7 +96,7 @@ void VoxelFilter3D::InsertPointCloud(const PointCloud& point_cloud) {
   }
 }
 
-const PointCloud& VoxelFilter3D::point_cloud() const { return point_cloud_; }
+const PointCloud& VoxelFilter::point_cloud() const { return point_cloud_; }
 
 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
@@ -135,12 +112,6 @@ AdaptiveVoxelFilter::AdaptiveVoxelFilter(
     const proto::AdaptiveVoxelFilterOptions& options)
     : options_(options) {}
 
-PointCloud2D AdaptiveVoxelFilter::Filter(
-    const PointCloud2D& point_cloud) const {
-  return AdaptivelyVoxelFiltered(
-      options_, FilterByMaxRange(point_cloud, options_.max_range()));
-}
-
 PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const {
   return AdaptivelyVoxelFiltered(
       options_, FilterByMaxRange(point_cloud, options_.max_range()));

cartographer/sensor/voxel_filter.h

@@ -17,10 +17,6 @@
 #ifndef CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_
 #define CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_
 
-#include <tuple>
-#include <unordered_set>
-#include <utility>
-
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/mapping_3d/hybrid_grid.h"
 #include "cartographer/sensor/point_cloud.h"
@@ -29,10 +25,6 @@
 namespace cartographer {
 namespace sensor {
 
-// Returns a voxel filtered copy of 'point_cloud' where 'size' is the length
-// a voxel edge.
-PointCloud2D VoxelFiltered(const PointCloud2D& point_cloud, float size);
-
 // Returns a voxel filtered copy of 'point_cloud' where 'size' is the length
 // a voxel edge.
 PointCloud VoxelFiltered(const PointCloud& point_cloud, float size);
@@ -48,35 +40,6 @@ class VoxelFilter {
   VoxelFilter(const VoxelFilter&) = delete;
   VoxelFilter& operator=(const VoxelFilter&) = delete;
 
-  // Inserts a point cloud into the voxel filter.
-  void InsertPointCloud(const PointCloud2D& point_cloud);
-
-  // Returns the filtered point cloud representing the occupied voxels.
-  const PointCloud2D& point_cloud() const;
-
- private:
-  struct IntegerPairHash {
-    size_t operator()(const std::pair<int64, int64>& x) const {
-      const uint64 first = x.first;
-      const uint64 second = x.second;
-      return first ^ (first + 0x9e3779b9u + (second << 6) + (second >> 2));
-    }
-  };
-
-  const float size_;
-  std::unordered_set<std::pair<int64, int64>, IntegerPairHash> voxels_;
-  PointCloud2D point_cloud_;
-};
-
-// The same as VoxelFilter but for 3D PointClouds.
-class VoxelFilter3D {
- public:
-  // 'size' is the length of a voxel edge.
-  explicit VoxelFilter3D(float size);
-
-  VoxelFilter3D(const VoxelFilter3D&) = delete;
-  VoxelFilter3D& operator=(const VoxelFilter3D&) = delete;
-
   // Inserts a point cloud into the voxel filter.
   void InsertPointCloud(const PointCloud& point_cloud);
 
@@ -99,7 +62,6 @@ class AdaptiveVoxelFilter {
   AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete;
   AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete;
 
-  PointCloud2D Filter(const PointCloud2D& point_cloud) const;
   PointCloud Filter(const PointCloud& point_cloud) const;
 
  private:

cartographer/sensor/voxel_filter_test.cc

@@ -27,13 +27,6 @@ namespace {
 using ::testing::ContainerEq;
 
 TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
-  PointCloud2D point_cloud = {
-      {0.f, 0.f}, {0.1f, -0.1f}, {0.3f, -0.1f}, {0.f, 0.f}};
-  EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f),
-              ContainerEq(PointCloud2D{point_cloud[0], point_cloud[2]}));
-}
-
-TEST(VoxelFilter3DTest, ReturnsTheFirstPointInEachVoxel) {
   PointCloud point_cloud = {{0.f, 0.f, 0.f},
                             {0.1f, -0.1f, 0.1f},
                             {0.3f, -0.1f, 0.f},