VoxelFilter for TimedPointCloud, Filter method. (#710)

This makes the VoxelFilter ready for TimedPointCloud and
per-point unwarping.

cartographer/internal/mapping_2d/local_trajectory_builder.cc

@@ -46,8 +46,8 @@ LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter(
                             options_.min_z(), options_.max_z());
   return sensor::RangeData{
       cropped.origin,
-      sensor::VoxelFiltered(cropped.returns, options_.voxel_filter_size()),
-      sensor::VoxelFiltered(cropped.misses, options_.voxel_filter_size())};
+      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns),
+      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)};
 }
 
 std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(

cartographer/internal/mapping_3d/local_trajectory_builder.cc

@@ -102,10 +102,10 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
     num_accumulated_ = 0;
     const sensor::RangeData filtered_range_data = {
         accumulated_range_data_.origin,
-        sensor::VoxelFiltered(accumulated_range_data_.returns,
-                              options_.voxel_filter_size()),
-        sensor::VoxelFiltered(accumulated_range_data_.misses,
-                              options_.voxel_filter_size())};
+        sensor::VoxelFilter(options_.voxel_filter_size())
+            .Filter(accumulated_range_data_.returns),
+        sensor::VoxelFilter(options_.voxel_filter_size())
+            .Filter(accumulated_range_data_.misses)};
     return AddAccumulatedRangeData(
         time, sensor::TransformRangeData(filtered_range_data,
                                          tracking_delta.inverse()));

cartographer/sensor/voxel_filter.cc

@@ -43,7 +43,7 @@ PointCloud AdaptivelyVoxelFiltered(
     // 'point_cloud' is already sparse enough.
     return point_cloud;
   }
-  PointCloud result = VoxelFiltered(point_cloud, options.max_length());
+  PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);
   if (result.size() >= options.min_num_points()) {
     // Filtering with 'max_length' resulted in a sufficiently dense point cloud.
     return result;
@@ -54,14 +54,15 @@ PointCloud AdaptivelyVoxelFiltered(
   for (float high_length = options.max_length();
        high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
     float low_length = high_length / 2.f;
-    result = VoxelFiltered(point_cloud, low_length);
+    result = VoxelFilter(low_length).Filter(point_cloud);
     if (result.size() >= options.min_num_points()) {
       // Binary search to find the right amount of filtering. 'low_length' gave
       // a sufficiently dense 'result', 'high_length' did not. We stop when the
       // 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 PointCloud candidate = VoxelFiltered(point_cloud, mid_length);
+        const PointCloud candidate =
+            VoxelFilter(mid_length).Filter(point_cloud);
         if (candidate.size() >= options.min_num_points()) {
           low_length = mid_length;
           result = candidate;
@@ -75,27 +76,33 @@ PointCloud AdaptivelyVoxelFiltered(
   return result;
 }
 
-}  // namespace
-
-PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
-  VoxelFilter voxel_filter(size);
-  voxel_filter.InsertPointCloud(point_cloud);
-  return voxel_filter.point_cloud();
-}
-
-VoxelFilter::VoxelFilter(const float size) : voxels_(size) {}
-
-void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) {
-  for (const Eigen::Vector3f& point : point_cloud) {
-    auto* const value = voxels_.mutable_value(voxels_.GetCellIndex(point));
+template <typename PointCloudType>
+PointCloudType FilterPointCloudUsingVoxels(
+    const PointCloudType& point_cloud,
+    mapping_3d::HybridGridBase<uint8>* voxels) {
+  PointCloudType results;
+  for (const auto& point : point_cloud) {
+    auto* const value =
+        voxels->mutable_value(voxels->GetCellIndex(point.template head<3>()));
     if (*value == 0) {
-      point_cloud_.push_back(point);
+      results.push_back(point);
       *value = 1;
     }
   }
+  return results;
 }
 
-const PointCloud& VoxelFilter::point_cloud() const { return point_cloud_; }
+}  // namespace
+
+VoxelFilter::VoxelFilter(const float size) : voxels_(size) {}
+
+PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
+  return FilterPointCloudUsingVoxels(point_cloud, &voxels_);
+}
+
+TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
+  return FilterPointCloudUsingVoxels(timed_point_cloud, &voxels_);
+}
 
 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {

cartographer/sensor/voxel_filter.h

@@ -25,10 +25,6 @@
 namespace cartographer {
 namespace sensor {
 
-// Returns a voxel filtered copy of 'point_cloud' where 'size' is the length
-// a voxel edge.
-PointCloud VoxelFiltered(const PointCloud& point_cloud, float size);
-
 // Voxel filter for point clouds. For each voxel, the assembled point cloud
 // contains the first point that fell into it from any of the inserted point
 // clouds.
@@ -40,15 +36,14 @@ class VoxelFilter {
   VoxelFilter(const VoxelFilter&) = delete;
   VoxelFilter& operator=(const VoxelFilter&) = delete;
 
-  // Inserts a point cloud into the voxel filter.
-  void InsertPointCloud(const PointCloud& point_cloud);
+  // Returns a voxel filtered copy of 'point_cloud'.
+  PointCloud Filter(const PointCloud& point_cloud);
 
-  // Returns the filtered point cloud representing the occupied voxels.
-  const PointCloud& point_cloud() const;
+  // Same for TimedPointCloud.
+  TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
 
  private:
   mapping_3d::HybridGridBase<uint8> voxels_;
-  PointCloud point_cloud_;
 };
 
 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(

cartographer/sensor/voxel_filter_test.cc

@@ -31,7 +31,7 @@ TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
                             {0.1f, -0.1f, 0.1f},
                             {0.3f, -0.1f, 0.f},
                             {0.f, 0.f, 0.1f}};
-  EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f),
+  EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud),
               ContainerEq(PointCloud{point_cloud[0], point_cloud[2]}));
 }