From 363a337e67776d9762f29d762326bd2c53fb41fb Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Wed, 28 Feb 2018 11:12:18 +0100
Subject: [PATCH] Re-implement VoxelFilter with unordered_set (#938)

Use hashing to filter voxels.
Handles arbitrary grid coordinates up to sizeof(int).

FIXES=#937
---
 cartographer/sensor/voxel_filter.cc      | 46 +++++++++++++++---------
 cartographer/sensor/voxel_filter.h       | 15 ++++++--
 cartographer/sensor/voxel_filter_test.cc | 18 ++++++++++
 3 files changed, 59 insertions(+), 20 deletions(-)

diff --git a/cartographer/sensor/voxel_filter.cc b/cartographer/sensor/voxel_filter.cc
index fe496a0..ad28646 100644
--- a/cartographer/sensor/voxel_filter.cc
+++ b/cartographer/sensor/voxel_filter.cc
@@ -76,31 +76,43 @@ PointCloud AdaptivelyVoxelFiltered(
   return result;
 }
 
-template <typename PointCloudType>
-PointCloudType FilterPointCloudUsingVoxels(
-    const PointCloudType& point_cloud, mapping::HybridGridBase<uint8>* voxels) {
-  PointCloudType results;
-  for (const typename PointCloudType::value_type& point : point_cloud) {
-    auto* const value =
-        voxels->mutable_value(voxels->GetCellIndex(point.template head<3>()));
-    if (*value == 0) {
+}  // namespace
+
+PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
+  PointCloud results;
+  for (const Eigen::Vector3f& point : point_cloud) {
+    auto it_inserted = voxel_set_.insert(IndexToKey(GetCellIndex(point)));
+    if (it_inserted.second) {
       results.push_back(point);
-      *value = 1;
     }
   }
   return results;
 }
 
-}  // 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) {
+  TimedPointCloud results;
+  for (const Eigen::Vector4f& point : timed_point_cloud) {
+    auto it_inserted =
+        voxel_set_.insert(IndexToKey(GetCellIndex(point.head<3>())));
+    if (it_inserted.second) {
+      results.push_back(point);
+    }
+  }
+  return results;
 }
 
-TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
-  return FilterPointCloudUsingVoxels(timed_point_cloud, &voxels_);
+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;
+}
+
+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()));
 }
 
 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
diff --git a/cartographer/sensor/voxel_filter.h b/cartographer/sensor/voxel_filter.h
index 0470b0b..ed08f5b 100644
--- a/cartographer/sensor/voxel_filter.h
+++ b/cartographer/sensor/voxel_filter.h
@@ -17,8 +17,10 @@
 #ifndef CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_
 #define CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_
 
+#include <bitset>
+#include <unordered_set>
+
 #include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/mapping/3d/hybrid_grid.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h"
 
@@ -31,7 +33,7 @@ namespace sensor {
 class VoxelFilter {
  public:
   // 'size' is the length of a voxel edge.
-  explicit VoxelFilter(float size);
+  explicit VoxelFilter(float size) : resolution_(size){};
 
   VoxelFilter(const VoxelFilter&) = delete;
   VoxelFilter& operator=(const VoxelFilter&) = delete;
@@ -43,7 +45,14 @@ class VoxelFilter {
   TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
 
  private:
-  mapping::HybridGridBase<uint8> voxels_;
+  using KeyType = std::bitset<3 * 32>;
+
+  static KeyType IndexToKey(const Eigen::Array3i& index);
+
+  Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const;
+
+  float resolution_;
+  std::unordered_set<KeyType> voxel_set_;
 };
 
 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
diff --git a/cartographer/sensor/voxel_filter_test.cc b/cartographer/sensor/voxel_filter_test.cc
index 8187eea..b0f39c7 100644
--- a/cartographer/sensor/voxel_filter_test.cc
+++ b/cartographer/sensor/voxel_filter_test.cc
@@ -35,6 +35,24 @@ TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
               ContainerEq(PointCloud{point_cloud[0], point_cloud[2]}));
 }
 
+TEST(VoxelFilterTest, HandlesLargeCoordinates) {
+  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}};
+  EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud),
+              ContainerEq(PointCloud{point_cloud[0], point_cloud[3]}));
+}
+
+TEST(VoxelFilterTest, IgnoresTime) {
+  TimedPointCloud timed_point_cloud;
+  for (int i = 0; i < 100; ++i) {
+    timed_point_cloud.emplace_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]}));
+}
+
 }  // namespace
 }  // namespace sensor
 }  // namespace cartographer