From 46063be28f31e8e4951a8342b144346f3cc3899c Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Wed, 3 Jun 2020 10:36:59 +0200
Subject: [PATCH] MinMaxRangeFiteringPointsProcessor: use squaredNorm() instead
 of norm() (#1637)

This should represent a considerable speedup.
---
 .../io/min_max_range_filtering_points_processor.cc         | 7 ++++---
 cartographer/io/min_max_range_filtering_points_processor.h | 4 ++--
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc
index c487d6d..8cc56e7 100644
--- a/cartographer/io/min_max_range_filtering_points_processor.cc
+++ b/cartographer/io/min_max_range_filtering_points_processor.cc
@@ -34,14 +34,15 @@ MinMaxRangeFiteringPointsProcessor::FromDictionary(
 
 MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor(
     const double min_range, const double max_range, PointsProcessor* next)
-    : min_range_(min_range), max_range_(max_range), next_(next) {}
+    : min_range_squared_(min_range*min_range), max_range_squared_(max_range*max_range),
+      next_(next) {}
 
 void MinMaxRangeFiteringPointsProcessor::Process(
     std::unique_ptr<PointsBatch> batch) {
   absl::flat_hash_set<int> to_remove;
   for (size_t i = 0; i < batch->points.size(); ++i) {
-    const float range = (batch->points[i].position - batch->origin).norm();
-    if (!(min_range_ <= range && range <= max_range_)) {
+    const float range = (batch->points[i].position - batch->origin).squaredNorm();
+    if (!(min_range_squared_ <= range && range <= max_range_squared_)) {
       to_remove.insert(i);
     }
   }
diff --git a/cartographer/io/min_max_range_filtering_points_processor.h b/cartographer/io/min_max_range_filtering_points_processor.h
index 2219cb6..5bc727c 100644
--- a/cartographer/io/min_max_range_filtering_points_processor.h
+++ b/cartographer/io/min_max_range_filtering_points_processor.h
@@ -47,8 +47,8 @@ class MinMaxRangeFiteringPointsProcessor : public PointsProcessor {
   FlushResult Flush() override;
 
  private:
-  const double min_range_;
-  const double max_range_;
+  const double min_range_squared_;
+  const double max_range_squared_;
   PointsProcessor* const next_;
 };