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 batch) { absl::flat_hash_set 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_; };