From 638aee7c2a6c7a70d2a7da39f4551b6cc6d8b318 Mon Sep 17 00:00:00 2001
From: Holger Rapp <HolgerRapp@gmx.net>
Date: Mon, 23 Oct 2017 10:56:55 +0200
Subject: [PATCH] Fix slow point deletion in RemovePoints. (#608)

This speeds up the asset writer significantly:

For the 3D sample bag:

~~~
/usr/bin/time roslaunch cartographer_ros assets_writer_backpack_3d.launch \
   bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag \
   pose_graph_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream

master:
2100.96user 26.79system 35:43.60elapsed 99%CPU (0avgtext+0avgdata 3657028maxresident)k
17682632inputs+32080outputs (0major+20021051minor)pagefaults 0swaps

this CL:
1937.46user 11.77system 32:22.49elapsed 100%CPU (0avgtext+0avgdata 3659412maxresident)k
1352inputs+32088outputs (0major+7042032minor)pagefaults 0swaps
~~~

So ~10%. For pipelines that do less this can be even 30% or more.
---
 .../fixed_ratio_sampling_points_processor.cc  |  4 +--
 ...in_max_range_filtering_points_processor.cc |  4 +--
 .../io/outlier_removing_points_processor.cc   |  4 +--
 cartographer/io/points_batch.cc               | 29 +++++++++++++++----
 cartographer/io/points_batch.h                |  3 +-
 5 files changed, 31 insertions(+), 13 deletions(-)

diff --git a/cartographer/io/fixed_ratio_sampling_points_processor.cc b/cartographer/io/fixed_ratio_sampling_points_processor.cc
index 858e789..e517a1d 100644
--- a/cartographer/io/fixed_ratio_sampling_points_processor.cc
+++ b/cartographer/io/fixed_ratio_sampling_points_processor.cc
@@ -42,10 +42,10 @@ FixedRatioSamplingPointsProcessor::FixedRatioSamplingPointsProcessor(
 
 void FixedRatioSamplingPointsProcessor::Process(
     std::unique_ptr<PointsBatch> batch) {
-  std::vector<int> to_remove;
+  std::unordered_set<int> to_remove;
   for (size_t i = 0; i < batch->points.size(); ++i) {
     if (!sampler_->Pulse()) {
-      to_remove.push_back(i);
+      to_remove.insert(i);
     }
   }
   RemovePoints(to_remove, batch.get());
diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc
index e2846a8..eb17090 100644
--- a/cartographer/io/min_max_range_filtering_points_processor.cc
+++ b/cartographer/io/min_max_range_filtering_points_processor.cc
@@ -38,11 +38,11 @@ MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor(
 
 void MinMaxRangeFiteringPointsProcessor::Process(
     std::unique_ptr<PointsBatch> batch) {
-  std::vector<int> to_remove;
+  std::unordered_set<int> to_remove;
   for (size_t i = 0; i < batch->points.size(); ++i) {
     const float range = (batch->points[i] - batch->origin).norm();
     if (!(min_range_ <= range && range <= max_range_)) {
-      to_remove.push_back(i);
+      to_remove.insert(i);
     }
   }
   RemovePoints(to_remove, batch.get());
diff --git a/cartographer/io/outlier_removing_points_processor.cc b/cartographer/io/outlier_removing_points_processor.cc
index 469b87f..11f0eaa 100644
--- a/cartographer/io/outlier_removing_points_processor.cc
+++ b/cartographer/io/outlier_removing_points_processor.cc
@@ -106,11 +106,11 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
 void OutlierRemovingPointsProcessor::ProcessInPhaseThree(
     std::unique_ptr<PointsBatch> batch) {
   constexpr double kMissPerHitLimit = 3;
-  std::vector<int> to_remove;
+  std::unordered_set<int> to_remove;
   for (size_t i = 0; i < batch->points.size(); ++i) {
     const auto voxel = voxels_.value(voxels_.GetCellIndex(batch->points[i]));
     if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) {
-      to_remove.push_back(i);
+      to_remove.insert(i);
     }
   }
   RemovePoints(to_remove, batch.get());
diff --git a/cartographer/io/points_batch.cc b/cartographer/io/points_batch.cc
index 19f1ec4..c6df9dd 100644
--- a/cartographer/io/points_batch.cc
+++ b/cartographer/io/points_batch.cc
@@ -19,17 +19,34 @@
 namespace cartographer {
 namespace io {
 
-void RemovePoints(std::vector<int> to_remove, PointsBatch* batch) {
-  std::sort(to_remove.begin(), to_remove.end(), std::greater<int>());
-  for (const int index : to_remove) {
-    batch->points.erase(batch->points.begin() + index);
+void RemovePoints(std::unordered_set<int> to_remove, PointsBatch* batch) {
+  const int new_num_points = batch->points.size() - to_remove.size();
+  std::vector<Eigen::Vector3f> points;
+  points.reserve(new_num_points);
+  std::vector<float> intensities;
+  if (!batch->intensities.empty()) {
+    intensities.reserve(new_num_points);
+  }
+  std::vector<FloatColor> colors;
+  if (!batch->colors.empty()) {
+    colors.reserve(new_num_points);
+  }
+
+  for (size_t i = 0; i < batch->points.size(); ++i) {
+    if (to_remove.count(i) == 1) {
+      continue;
+    }
+    points.push_back(batch->points[i]);
     if (!batch->colors.empty()) {
-      batch->colors.erase(batch->colors.begin() + index);
+      colors.push_back(batch->colors[i]);
     }
     if (!batch->intensities.empty()) {
-      batch->intensities.erase(batch->intensities.begin() + index);
+      intensities.push_back(batch->intensities[i]);
     }
   }
+  batch->points = std::move(points);
+  batch->intensities = std::move(intensities);
+  batch->colors = std::move(colors);
 }
 
 }  // namespace io
diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h
index 7a068bf..9060d2a 100644
--- a/cartographer/io/points_batch.h
+++ b/cartographer/io/points_batch.h
@@ -19,6 +19,7 @@
 
 #include <array>
 #include <cstdint>
+#include <unordered_set>
 #include <vector>
 
 #include "Eigen/Core"
@@ -65,7 +66,7 @@ struct PointsBatch {
 };
 
 // Removes the indices in 'to_remove' from 'batch'.
-void RemovePoints(std::vector<int> to_remove, PointsBatch* batch);
+void RemovePoints(std::unordered_set<int> to_remove, PointsBatch* batch);
 
 }  // namespace io
 }  // namespace cartographer