From d3473fca4a99f239980cfab58a88eb35fa59d2c0 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@lyft.com>
Date: Mon, 19 Oct 2020 12:14:50 +0200
Subject: [PATCH] Keep intensities in the voxel filter. (#1764)

Signed-off-by: Wolfgang Hess <whess@lyft.com>
---
 cartographer/sensor/internal/voxel_filter.cc  | 35 ++++++++++++++++---
 .../sensor/internal/voxel_filter_test.cc      | 23 ++++++++++++
 2 files changed, 54 insertions(+), 4 deletions(-)

diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc
index b1640ad..a6e5c0b 100644
--- a/cartographer/sensor/internal/voxel_filter.cc
+++ b/cartographer/sensor/internal/voxel_filter.cc
@@ -86,9 +86,9 @@ VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point,
 }
 
 template <class T, class PointFunction>
-std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
-                                     const float resolution,
-                                     PointFunction&& point_function) {
+std::vector<bool> RandomizedVoxelFilterIndices(
+    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>>
@@ -110,6 +110,15 @@ std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
   for (const auto& voxel_and_index : voxel_count_and_point_index) {
     points_used[voxel_and_index.second.second] = true;
   }
+  return points_used;
+}
+
+template <class T, class PointFunction>
+std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
+                                     const float resolution,
+                                     PointFunction&& point_function) {
+  const std::vector<bool> points_used =
+      RandomizedVoxelFilterIndices(point_cloud, resolution, point_function);
 
   std::vector<T> results;
   for (size_t i = 0; i < point_cloud.size(); i++) {
@@ -130,7 +139,25 @@ std::vector<RangefinderPoint> VoxelFilter(
 }
 
 PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
-  return PointCloud(VoxelFilter(point_cloud.points(), resolution));
+  const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
+      point_cloud.points(), resolution,
+      [](const RangefinderPoint& point) { return point.position; });
+
+  std::vector<RangefinderPoint> filtered_points;
+  for (size_t i = 0; i < point_cloud.size(); i++) {
+    if (points_used[i]) {
+      filtered_points.push_back(point_cloud[i]);
+    }
+  }
+  std::vector<float> filtered_intensities;
+  CHECK_LE(point_cloud.intensities().size(), point_cloud.points().size());
+  for (size_t i = 0; i < point_cloud.intensities().size(); i++) {
+    if (points_used[i]) {
+      filtered_intensities.push_back(point_cloud.intensities()[i]);
+    }
+  }
+  return PointCloud(std::move(filtered_points),
+                    std::move(filtered_intensities));
 }
 
 TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
diff --git a/cartographer/sensor/internal/voxel_filter_test.cc b/cartographer/sensor/internal/voxel_filter_test.cc
index db6cd3f..b84152f 100644
--- a/cartographer/sensor/internal/voxel_filter_test.cc
+++ b/cartographer/sensor/internal/voxel_filter_test.cc
@@ -25,6 +25,7 @@ namespace sensor {
 namespace {
 
 using ::testing::Contains;
+using ::testing::IsEmpty;
 
 TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) {
   const PointCloud point_cloud({{{0.f, 0.f, 0.f}},
@@ -33,11 +34,32 @@ TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) {
                                 {{0.f, 0.f, 0.1f}}});
   const PointCloud result = VoxelFilter(point_cloud, 0.3f);
   ASSERT_EQ(result.size(), 2);
+  EXPECT_THAT(result.intensities(), IsEmpty());
   EXPECT_THAT(point_cloud.points(), Contains(result[0]));
   EXPECT_THAT(point_cloud.points(), Contains(result[1]));
   EXPECT_THAT(result.points(), Contains(point_cloud[2]));
 }
 
+TEST(VoxelFilterTest, CorrectIntensities) {
+  std::vector<RangefinderPoint> points;
+  std::vector<float> intensities;
+  for (int i = 0; i < 100; ++i) {
+    const float value = 0.1f * i;
+    // We add points with intensity equal to the z coordinate, so we can later
+    // verify that the resulting intensities are corresponding to the filtered
+    // points.
+    points.push_back({{-100.f, 0.3f, value}});
+    intensities.push_back(value);
+  }
+  const PointCloud point_cloud(points, intensities);
+  const PointCloud result = VoxelFilter(point_cloud, 0.3f);
+
+  ASSERT_EQ(result.intensities().size(), result.points().size());
+  for (size_t i = 0; i < result.size(); ++i) {
+    ASSERT_NEAR(result[i].position.z(), result.intensities()[i], 1e-6);
+  }
+}
+
 TEST(VoxelFilterTest, HandlesLargeCoordinates) {
   const PointCloud point_cloud({{{100000.f, 0.f, 0.f}},
                                 {{100000.001f, -0.0001f, 0.0001f}},
@@ -45,6 +67,7 @@ TEST(VoxelFilterTest, HandlesLargeCoordinates) {
                                 {{-200000.f, 0.f, 0.f}}});
   const PointCloud result = VoxelFilter(point_cloud, 0.01f);
   EXPECT_EQ(result.size(), 2);
+  EXPECT_THAT(result.intensities(), IsEmpty());
   EXPECT_THAT(result.points(), Contains(point_cloud[3]));
 }