From bd7d7202bfdf4e889a602b2cc16af9e63583ac78 Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Thu, 15 Mar 2018 12:31:08 +0100
Subject: [PATCH] Filter RangeMeasurement (#995)

[RFC=0017](https://github.com/googlecartographer/rfcs/blob/master/text/0017-synchronize-points.md)
---
 cartographer/sensor/internal/voxel_filter.cc | 15 +++++++++++++++
 cartographer/sensor/internal/voxel_filter.h  |  6 ++++++
 2 files changed, 21 insertions(+)

diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc
index 17c9762..1960245 100644
--- a/cartographer/sensor/internal/voxel_filter.cc
+++ b/cartographer/sensor/internal/voxel_filter.cc
@@ -101,6 +101,21 @@ TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
   return results;
 }
 
+std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>
+VoxelFilter::Filter(
+    const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
+        range_measurements) {
+  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> results;
+  for (const auto& range_measurement : range_measurements) {
+    auto it_inserted = voxel_set_.insert(
+        IndexToKey(GetCellIndex(range_measurement.point_time.head<3>())));
+    if (it_inserted.second) {
+      results.push_back(range_measurement);
+    }
+  }
+  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]));
diff --git a/cartographer/sensor/internal/voxel_filter.h b/cartographer/sensor/internal/voxel_filter.h
index 1a24b99..9429ccd 100644
--- a/cartographer/sensor/internal/voxel_filter.h
+++ b/cartographer/sensor/internal/voxel_filter.h
@@ -23,6 +23,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h"
+#include "cartographer/sensor/timed_point_cloud_data.h"
 
 namespace cartographer {
 namespace sensor {
@@ -44,6 +45,11 @@ class VoxelFilter {
   // Same for TimedPointCloud.
   TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
 
+  // Same for RangeMeasurement.
+  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> Filter(
+      const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
+          range_measurements);
+
  private:
   using KeyType = std::bitset<3 * 32>;