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.master
parent
f179bd942c
commit
638aee7c2a
|
@ -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());
|
||||
|
|
|
@ -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());
|
||||
|
|
|
@ -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());
|
||||
|
|
|
@ -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()) {
|
||||
batch->colors.erase(batch->colors.begin() + index);
|
||||
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()) {
|
||||
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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue