12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

MinMaxRangeFiteringPointsProcessor: use squaredNorm() instead of norm() (#1637) 

This should represent a considerable speedup.
master
Davide Faconti 2020-06-03 10:36:59 +02:00 committed by GitHub
parent 1659f0dc8b
commit 46063be28f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/io/min_max_range_filtering_points_processor.cc
View File

@ -34,14 +34,15 @@ MinMaxRangeFiteringPointsProcessor::FromDictionary(
MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor( MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor(
const double min_range, const double max_range, PointsProcessor* next) 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( void MinMaxRangeFiteringPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch) { std::unique_ptr<PointsBatch> batch) {
absl::flat_hash_set<int> to_remove; absl::flat_hash_set<int> to_remove;
for (size_t i = 0; i < batch->points.size(); ++i) { for (size_t i = 0; i < batch->points.size(); ++i) {
const float range = (batch->points[i].position - batch->origin).norm(); const float range = (batch->points[i].position - batch->origin).squaredNorm();
if (!(min_range_ <= range && range <= max_range_)) { if (!(min_range_squared_ <= range && range <= max_range_squared_)) {
to_remove.insert(i); to_remove.insert(i);
} }
} }

4
cartographer/io/min_max_range_filtering_points_processor.h
View File

@ -47,8 +47,8 @@ class MinMaxRangeFiteringPointsProcessor : public PointsProcessor {
FlushResult Flush() override; FlushResult Flush() override;
private: private:
const double min_range_; const double min_range_squared_;
const double max_range_; const double max_range_squared_;
PointsProcessor* const next_; PointsProcessor* const next_;
}; };