Random voxel filtering. (#1753)

This leads to slightly slower performance but better quality.
Also replaces the VoxelFilter class by free standing functions.

Signed-off-by: Wolfgang Hess <whess@lyft.com>

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -58,8 +58,8 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
                             options_.min_z(), options_.max_z());
   return sensor::RangeData{
       cropped.origin,
-      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns),
-      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)};
+      sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()),
+      sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
 }
 
 std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
@@ -226,8 +226,8 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
       non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
 
   const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
-      sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
-          .Filter(gravity_aligned_range_data.returns);
+      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
+                                  options_.adaptive_voxel_filter_options());
   if (filtered_gravity_aligned_point_cloud.empty()) {
     return nullptr;
   }

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -154,9 +154,8 @@ LocalTrajectoryBuilder3D::AddRangeData(
     accumulated_point_cloud_origin_data_.clear();
   }
 
-  synchronized_data.ranges =
-      sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
-          .Filter(synchronized_data.ranges);
+  synchronized_data.ranges = sensor::VoxelFilter(
+      synchronized_data.ranges, 0.5f * options_.voxel_filter_size());
   accumulated_point_cloud_origin_data_.emplace_back(
       std::move(synchronized_data));
   ++num_accumulated_;
@@ -237,10 +236,10 @@ LocalTrajectoryBuilder3D::AddRangeData(
   const auto voxel_filter_start = std::chrono::steady_clock::now();
   const sensor::RangeData filtered_range_data = {
       extrapolation_result.current_pose.translation().cast<float>(),
-      sensor::VoxelFilter(options_.voxel_filter_size())
-          .Filter(accumulated_range_data.returns),
-      sensor::VoxelFilter(options_.voxel_filter_size())
-          .Filter(accumulated_range_data.misses)};
+      sensor::VoxelFilter(accumulated_range_data.returns,
+                          options_.voxel_filter_size()),
+      sensor::VoxelFilter(accumulated_range_data.misses,
+                          options_.voxel_filter_size())};
   const auto voxel_filter_stop = std::chrono::steady_clock::now();
   const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
 
@@ -274,19 +273,18 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
 
   const auto scan_matcher_start = std::chrono::steady_clock::now();
 
-  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
-      options_.high_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud high_resolution_point_cloud_in_tracking =
-      adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns);
+      sensor::AdaptiveVoxelFilter(
+          filtered_range_data_in_tracking.returns,
+          options_.high_resolution_adaptive_voxel_filter_options());
   if (high_resolution_point_cloud_in_tracking.empty()) {
     LOG(WARNING) << "Dropped empty high resolution point cloud data.";
     return nullptr;
   }
-  sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
-      options_.low_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud low_resolution_point_cloud_in_tracking =
-      low_resolution_adaptive_voxel_filter.Filter(
-          filtered_range_data_in_tracking.returns);
+      sensor::AdaptiveVoxelFilter(
+          filtered_range_data_in_tracking.returns,
+          options_.low_resolution_adaptive_voxel_filter_options());
   if (low_resolution_point_cloud_in_tracking.empty()) {
     LOG(WARNING) << "Dropped empty low resolution point cloud data.";
     return nullptr;

cartographer/sensor/internal/voxel_filter.cc

@@ -17,7 +17,10 @@
 #include "cartographer/sensor/internal/voxel_filter.h"
 
 #include <cmath>
+#include <random>
+#include <utility>
 
+#include "absl/container/flat_hash_map.h"
 #include "cartographer/common/math.h"
 
 namespace cartographer {
@@ -43,7 +46,7 @@ PointCloud AdaptivelyVoxelFiltered(
     // 'point_cloud' is already sparse enough.
     return point_cloud;
   }
-  PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);
+  PointCloud result = VoxelFilter(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;
@@ -54,15 +57,14 @@ 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 = VoxelFilter(low_length).Filter(point_cloud);
+    result = VoxelFilter(point_cloud, low_length);
     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 =
-            VoxelFilter(mid_length).Filter(point_cloud);
+        const PointCloud candidate = VoxelFilter(point_cloud, mid_length);
         if (candidate.size() >= options.min_num_points()) {
           low_length = mid_length;
           result = candidate;
@@ -76,58 +78,77 @@ PointCloud AdaptivelyVoxelFiltered(
   return result;
 }
 
-}  // namespace
+using VoxelKeyType = uint64_t;
 
-PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
-  PointCloud results;
-  for (const RangefinderPoint& point : point_cloud) {
-    auto it_inserted =
-        voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
-    if (it_inserted.second) {
-      results.push_back(point);
-    }
-  }
-  return results;
+VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point,
+                               const float resolution) {
+  const Eigen::Array3f index = point.array() / resolution;
+  const uint64_t x = common::RoundToInt(index.x());
+  const uint64_t y = common::RoundToInt(index.y());
+  const uint64_t z = common::RoundToInt(index.z());
+  return (x << 42) + (y << 21) + z;
 }
 
-TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
-  TimedPointCloud results;
-  for (const TimedRangefinderPoint& point : timed_point_cloud) {
-    auto it_inserted =
-        voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
-    if (it_inserted.second) {
-      results.push_back(point);
+template <class T, class PointFunction>
+std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
+                                     const float resolution,
+                                     PointFunction&& point_function) {
+  // According to https://en.wikipedia.org/wiki/Reservoir_sampling
+  std::minstd_rand0 generator;
+  absl::flat_hash_map<VoxelKeyType, std::pair<int, int>>
+      voxel_count_and_point_index;
+  for (size_t i = 0; i < point_cloud.size(); i++) {
+    auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex(
+        point_function(point_cloud[i]), resolution)];
+    voxel.first++;
+    if (voxel.first == 1) {
+      voxel.second = i;
+    } else {
+      std::uniform_int_distribution<> distribution(1, voxel.first);
+      if (distribution(generator) == voxel.first) {
+        voxel.second = i;
+      }
     }
   }
-  return results;
-}
+  std::vector<bool> points_used(point_cloud.size(), false);
+  for (const auto& voxel_and_index : voxel_count_and_point_index) {
+    points_used[voxel_and_index.second.second] = true;
+  }
 
-std::vector<TimedPointCloudOriginData::RangeMeasurement> VoxelFilter::Filter(
-    const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
-        range_measurements) {
-  std::vector<TimedPointCloudOriginData::RangeMeasurement> results;
-  for (const auto& range_measurement : range_measurements) {
-    auto it_inserted = voxel_set_.insert(
-        IndexToKey(GetCellIndex(range_measurement.point_time.position)));
-    if (it_inserted.second) {
-      results.push_back(range_measurement);
+  std::vector<T> results;
+  for (size_t i = 0; i < point_cloud.size(); i++) {
+    if (points_used[i]) {
+      results.push_back(point_cloud[i]);
     }
   }
   return results;
 }
 
-VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) {
-  KeyType k_0(static_cast<uint32>(index[0]));
-  KeyType k_1(static_cast<uint32>(index[1]));
-  KeyType k_2(static_cast<uint32>(index[2]));
-  return (k_0 << 2 * 32) | (k_1 << 1 * 32) | k_2;
+}  // namespace
+
+PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
+  return RandomizedVoxelFilter(
+      point_cloud, resolution,
+      [](const RangefinderPoint& point) { return point.position; });
 }
 
-Eigen::Array3i VoxelFilter::GetCellIndex(const Eigen::Vector3f& point) const {
-  Eigen::Array3f index = point.array() / resolution_;
-  return Eigen::Array3i(common::RoundToInt(index.x()),
-                        common::RoundToInt(index.y()),
-                        common::RoundToInt(index.z()));
+TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
+                            const float resolution) {
+  return RandomizedVoxelFilter(
+      timed_point_cloud, resolution,
+      [](const TimedRangefinderPoint& point) { return point.position; });
+}
+
+std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> VoxelFilter(
+    const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
+        range_measurements,
+    const float resolution) {
+  return RandomizedVoxelFilter(
+      range_measurements, resolution,
+      [](const sensor::TimedPointCloudOriginData::RangeMeasurement&
+             range_measurement) {
+        return range_measurement.point_time.position;
+      });
 }
 
 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
@@ -140,13 +161,11 @@ proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
   return options;
 }
 
-AdaptiveVoxelFilter::AdaptiveVoxelFilter(
-    const proto::AdaptiveVoxelFilterOptions& options)
-    : options_(options) {}
-
-PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const {
+PointCloud AdaptiveVoxelFilter(
+    const PointCloud& point_cloud,
+    const proto::AdaptiveVoxelFilterOptions& options) {
   return AdaptivelyVoxelFiltered(
-      options_, FilterByMaxRange(point_cloud, options_.max_range()));
+      options, FilterByMaxRange(point_cloud, options.max_range()));
 }
 
 }  // namespace sensor

cartographer/sensor/internal/voxel_filter.h

@@ -19,7 +19,6 @@
 
 #include <bitset>
 
-#include "absl/container/flat_hash_set.h"
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h"
@@ -28,55 +27,20 @@
 namespace cartographer {
 namespace sensor {
 
-// 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.
-class VoxelFilter {
- public:
-  // 'size' is the length of a voxel edge.
-  explicit VoxelFilter(float size) : resolution_(size) {}
-
-  VoxelFilter(const VoxelFilter&) = delete;
-  VoxelFilter& operator=(const VoxelFilter&) = delete;
-
-  // Returns a voxel filtered copy of 'point_cloud'.
-  PointCloud Filter(const PointCloud& point_cloud);
-
-  // Same for TimedPointCloud.
-  TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
-
-  // Same for RangeMeasurement.
-  std::vector<TimedPointCloudOriginData::RangeMeasurement> Filter(
-      const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
-          range_measurements);
-
- private:
-  using KeyType = std::bitset<3 * 32>;
-
-  static KeyType IndexToKey(const Eigen::Array3i& index);
-
-  Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const;
-
-  float resolution_;
-  absl::flat_hash_set<KeyType> voxel_set_;
-};
+PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution);
+TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
+                            const float resolution);
+std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> VoxelFilter(
+    const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
+        range_measurements,
+    const float resolution);
 
 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
     common::LuaParameterDictionary* const parameter_dictionary);
 
-class AdaptiveVoxelFilter {
- public:
-  explicit AdaptiveVoxelFilter(
-      const proto::AdaptiveVoxelFilterOptions& options);
-
-  AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete;
-  AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete;
-
-  PointCloud Filter(const PointCloud& point_cloud) const;
-
- private:
-  const proto::AdaptiveVoxelFilterOptions options_;
-};
+PointCloud AdaptiveVoxelFilter(
+    const PointCloud& point_cloud,
+    const proto::AdaptiveVoxelFilterOptions& options);
 
 }  // namespace sensor
 }  // namespace cartographer

cartographer/sensor/internal/voxel_filter_test.cc

@@ -24,33 +24,38 @@ namespace cartographer {
 namespace sensor {
 namespace {
 
-using ::testing::ContainerEq;
-
-TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
-  PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}},
-                            {Eigen::Vector3f{0.1f, -0.1f, 0.1f}},
-                            {Eigen::Vector3f{0.3f, -0.1f, 0.f}},
-                            {Eigen::Vector3f{0.f, 0.f, 0.1f}}};
-  EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud),
-              ContainerEq(PointCloud{point_cloud[0], point_cloud[2]}));
+using ::testing::Contains;
+
+TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) {
+  const PointCloud point_cloud({{{0.f, 0.f, 0.f}},
+                                {{0.1f, -0.1f, 0.1f}},
+                                {{0.3f, -0.1f, 0.f}},
+                                {{0.f, 0.f, 0.1f}}});
+  const PointCloud result = VoxelFilter(point_cloud, 0.3f);
+  ASSERT_EQ(result.size(), 2);
+  EXPECT_THAT(point_cloud, Contains(result[0]));
+  EXPECT_THAT(point_cloud, Contains(result[1]));
+  EXPECT_THAT(result, Contains(point_cloud[2]));
 }
 
 TEST(VoxelFilterTest, HandlesLargeCoordinates) {
-  PointCloud point_cloud = {{Eigen::Vector3f{100000.f, 0.f, 0.f}},
-                            {Eigen::Vector3f{100000.001f, -0.0001f, 0.0001f}},
-                            {Eigen::Vector3f{100000.003f, -0.0001f, 0.f}},
-                            {Eigen::Vector3f{-200000.f, 0.f, 0.f}}};
-  EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud),
-              ContainerEq(PointCloud{point_cloud[0], point_cloud[3]}));
+  const PointCloud point_cloud({{{100000.f, 0.f, 0.f}},
+                                {{100000.001f, -0.0001f, 0.0001f}},
+                                {{100000.003f, -0.0001f, 0.f}},
+                                {{-200000.f, 0.f, 0.f}}});
+  const PointCloud result = VoxelFilter(point_cloud, 0.01f);
+  EXPECT_EQ(result.size(), 2);
+  EXPECT_THAT(result, Contains(point_cloud[3]));
 }
 
 TEST(VoxelFilterTest, IgnoresTime) {
   TimedPointCloud timed_point_cloud;
   for (int i = 0; i < 100; ++i) {
-    timed_point_cloud.push_back({Eigen::Vector3f{-100.f, 0.3f, 0.4f}, 1.f * i});
+    timed_point_cloud.push_back({{-100.f, 0.3f, 0.4f}, 1.f * i});
   }
-  EXPECT_THAT(VoxelFilter(0.3f).Filter(timed_point_cloud),
-              ContainerEq(TimedPointCloud{timed_point_cloud[0]}));
+  const TimedPointCloud result = VoxelFilter(timed_point_cloud, 0.3f);
+  ASSERT_EQ(result.size(), 1);
+  EXPECT_THAT(timed_point_cloud, Contains(result[0]));
 }
 
 }  // namespace