Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/io/points_batch.h"
00018
00019 namespace cartographer {
00020 namespace io {
00021
00022 void RemovePoints(absl::flat_hash_set<int> to_remove, PointsBatch* batch) {
00023 const int new_num_points = batch->points.size() - to_remove.size();
00024 sensor::PointCloud points;
00025 points.reserve(new_num_points);
00026 std::vector<float> intensities;
00027 if (!batch->intensities.empty()) {
00028 intensities.reserve(new_num_points);
00029 }
00030 std::vector<FloatColor> colors;
00031 if (!batch->colors.empty()) {
00032 colors.reserve(new_num_points);
00033 }
00034
00035 for (size_t i = 0; i < batch->points.size(); ++i) {
00036 if (to_remove.count(i) == 1) {
00037 continue;
00038 }
00039 points.push_back(batch->points[i]);
00040 if (!batch->colors.empty()) {
00041 colors.push_back(batch->colors[i]);
00042 }
00043 if (!batch->intensities.empty()) {
00044 intensities.push_back(batch->intensities[i]);
00045 }
00046 }
00047 batch->points = std::move(points);
00048 batch->intensities = std::move(intensities);
00049 batch->colors = std::move(colors);
00050 }
00051
00052 }
00053 }