diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index dcdb1d1..ad3dfb7 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include "Eigen/Eigenvalues" #include "absl/memory/memory.h" @@ -61,7 +63,6 @@ ConstraintBuilder2D::ConstraintBuilder2D( thread_pool_(thread_pool), finish_node_task_(absl::make_unique()), when_done_task_(absl::make_unique()), - sampler_(options.sampling_ratio()), ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} ConstraintBuilder2D::~ConstraintBuilder2D() { @@ -81,7 +82,12 @@ void ConstraintBuilder2D::MaybeAddConstraint( options_.max_constraint_distance()) { return; } - if (!sampler_.Pulse()) return; + if (!per_submap_sampler_ + .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id), + std::forward_as_tuple(options_.sampling_ratio())) + .first->second.Pulse()) { + return; + } absl::MutexLock locker(&mutex_); if (when_done_) { @@ -305,6 +311,7 @@ void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) { << "DeleteScanMatcher was called while WhenDone was scheduled."; } submap_scan_matchers_.erase(submap_id); + per_submap_sampler_.erase(submap_id); kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); } diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.h b/cartographer/mapping/internal/constraints/constraint_builder_2d.h index 272f7dc..6667fdb 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "Eigen/Core" @@ -160,8 +161,8 @@ class ConstraintBuilder2D { // Map of dispatched or constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); + std::map per_submap_sampler_; - common::FixedRatioSampler sampler_; scan_matching::CeresScanMatcher2D ceres_scan_matcher_; // Histogram of scan matcher scores. diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index 878aaa0..ca9ff71 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include "Eigen/Eigenvalues" #include "absl/memory/memory.h" @@ -63,7 +65,6 @@ ConstraintBuilder3D::ConstraintBuilder3D( thread_pool_(thread_pool), finish_node_task_(absl::make_unique()), when_done_task_(absl::make_unique()), - sampler_(options.sampling_ratio()), ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ConstraintBuilder3D::~ConstraintBuilder3D() { @@ -84,7 +85,12 @@ void ConstraintBuilder3D::MaybeAddConstraint( .norm() > options_.max_constraint_distance()) { return; } - if (!sampler_.Pulse()) return; + if (!per_submap_sampler_ + .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id), + std::forward_as_tuple(options_.sampling_ratio())) + .first->second.Pulse()) { + return; + } absl::MutexLock locker(&mutex_); if (when_done_) { @@ -336,6 +342,7 @@ void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) { << "DeleteScanMatcher was called while WhenDone was scheduled."; } submap_scan_matchers_.erase(submap_id); + per_submap_sampler_.erase(submap_id); kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); } diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.h b/cartographer/mapping/internal/constraints/constraint_builder_3d.h index fb0edc8..247a9da 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "Eigen/Core" @@ -169,8 +170,8 @@ class ConstraintBuilder3D { // Map of dispatched or constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); + std::map per_submap_sampler_; - common::FixedRatioSampler sampler_; scan_matching::CeresScanMatcher3D ceres_scan_matcher_; // Histograms of scan matcher scores.