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.

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());

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());

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());

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

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