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