12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add per-submap sampling. (#1758) 

This changes which submaps we select to attempt loop closing.
The subsampling of candidates is changing from randomly sampling
submap and node pairs to per-submap sampling. This enforces a
better distribution of loop closure attempts across the submaps.
This sampling achieves a much better performance which indicates
that the approach used before was sub-optimal.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-10-09 09:30:27 +02:00 committed by GitHub
parent ca8a866996
commit 8ac967a50d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 22 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer/mapping/internal/constraints/constraint_builder_2d.cc
View File

@ -24,6 +24,8 @@
#include <memory>
#include <sstream>
#include <string>
#include <tuple>
#include <utility>
#include "Eigen/Eigenvalues"
#include "absl/memory/memory.h"
@ -61,7 +63,6 @@ ConstraintBuilder2D::ConstraintBuilder2D(
thread_pool_(thread_pool),
finish_node_task_(absl::make_unique<common::Task>()),
when_done_task_(absl::make_unique<common::Task>()),
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());
}

3
cartographer/mapping/internal/constraints/constraint_builder_2d.h
View File

@ -21,6 +21,7 @@
#include <deque>
#include <functional>
#include <limits>
#include <map>
#include <vector>
#include "Eigen/Core"
@ -160,8 +161,8 @@ class ConstraintBuilder2D {
// Map of dispatched or constructed scan matchers by 'submap_id'.
std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
GUARDED_BY(mutex_);
std::map<SubmapId, common::FixedRatioSampler> per_submap_sampler_;
common::FixedRatioSampler sampler_;
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
// Histogram of scan matcher scores.

11
cartographer/mapping/internal/constraints/constraint_builder_3d.cc
View File

@ -24,6 +24,8 @@
#include <memory>
#include <sstream>
#include <string>
#include <tuple>
#include <utility>
#include "Eigen/Eigenvalues"
#include "absl/memory/memory.h"
@ -63,7 +65,6 @@ ConstraintBuilder3D::ConstraintBuilder3D(
thread_pool_(thread_pool),
finish_node_task_(absl::make_unique<common::Task>()),
when_done_task_(absl::make_unique<common::Task>()),
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());
}

3
cartographer/mapping/internal/constraints/constraint_builder_3d.h
View File

@ -21,6 +21,7 @@
#include <deque>
#include <functional>
#include <limits>
#include <map>
#include <vector>
#include "Eigen/Core"
@ -169,8 +170,8 @@ class ConstraintBuilder3D {
// Map of dispatched or constructed scan matchers by 'submap_id'.
std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
GUARDED_BY(mutex_);
std::map<SubmapId, common::FixedRatioSampler> per_submap_sampler_;
common::FixedRatioSampler sampler_;
scan_matching::CeresScanMatcher3D ceres_scan_matcher_;
// Histograms of scan matcher scores.