From 2abe1c72a6d66e94ed6d1b068fef6ec03bedfe1f Mon Sep 17 00:00:00 2001
From: Michael Grupp <grupp@magazino.eu>
Date: Sun, 16 Dec 2018 00:06:09 +0100
Subject: [PATCH] Configurable miss_per_hit_limit in outlier removing
 processor. (#1476)

Was hardcoded to 3 before, but it makes sense to make this a parameter.
---
 .../io/outlier_removing_points_processor.cc     | 17 +++++++++++++----
 .../io/outlier_removing_points_processor.h      |  4 +++-
 2 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/cartographer/io/outlier_removing_points_processor.cc b/cartographer/io/outlier_removing_points_processor.cc
index 92c9787..585c36f 100644
--- a/cartographer/io/outlier_removing_points_processor.cc
+++ b/cartographer/io/outlier_removing_points_processor.cc
@@ -27,13 +27,23 @@ std::unique_ptr<OutlierRemovingPointsProcessor>
 OutlierRemovingPointsProcessor::FromDictionary(
     common::LuaParameterDictionary* const dictionary,
     PointsProcessor* const next) {
+  const double miss_per_hit_limit = [&]() {
+    if (!dictionary->HasKey("miss_per_hit_limit")) {
+      LOG(INFO) << "Using default value of 3 for miss_per_hit_limit.";
+      return 3.;
+    } else {
+      return dictionary->GetDouble("miss_per_hit_limit");
+    }
+  }();
   return absl::make_unique<OutlierRemovingPointsProcessor>(
-      dictionary->GetDouble("voxel_size"), next);
+      dictionary->GetDouble("voxel_size"), miss_per_hit_limit, next);
 }
 
 OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor(
-    const double voxel_size, PointsProcessor* next)
+    const double voxel_size, const double miss_per_hit_limit,
+    PointsProcessor* next)
     : voxel_size_(voxel_size),
+      miss_per_hit_limit_(miss_per_hit_limit),
       next_(next),
       state_(State::kPhase1),
       voxels_(voxel_size_) {
@@ -106,12 +116,11 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
 
 void OutlierRemovingPointsProcessor::ProcessInPhaseThree(
     std::unique_ptr<PointsBatch> batch) {
-  constexpr double kMissPerHitLimit = 3;
   absl::flat_hash_set<int> to_remove;
   for (size_t i = 0; i < batch->points.size(); ++i) {
     const VoxelData voxel =
         voxels_.value(voxels_.GetCellIndex(batch->points[i].position));
-    if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) {
+    if (!(voxel.rays < miss_per_hit_limit_ * voxel.hits)) {
       to_remove.insert(i);
     }
   }
diff --git a/cartographer/io/outlier_removing_points_processor.h b/cartographer/io/outlier_removing_points_processor.h
index fefc828..4853c78 100644
--- a/cartographer/io/outlier_removing_points_processor.h
+++ b/cartographer/io/outlier_removing_points_processor.h
@@ -31,7 +31,8 @@ class OutlierRemovingPointsProcessor : public PointsProcessor {
   constexpr static const char* kConfigurationFileActionName =
       "voxel_filter_and_remove_moving_objects";
 
-  OutlierRemovingPointsProcessor(double voxel_size, PointsProcessor* next);
+  OutlierRemovingPointsProcessor(double voxel_size, double miss_per_hit_limit,
+                                 PointsProcessor* next);
 
   static std::unique_ptr<OutlierRemovingPointsProcessor> FromDictionary(
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
@@ -76,6 +77,7 @@ class OutlierRemovingPointsProcessor : public PointsProcessor {
   void ProcessInPhaseThree(std::unique_ptr<PointsBatch> batch);
 
   const double voxel_size_;
+  const double miss_per_hit_limit_;
   PointsProcessor* const next_;
   State state_;
   mapping::HybridGridBase<VoxelData> voxels_;