From 8ac967a50df6d2f46112d14473df57b30a5e177f Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@lyft.com>
Date: Fri, 9 Oct 2020 09:30:27 +0200
Subject: [PATCH] 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>
---
 .../internal/constraints/constraint_builder_2d.cc     | 11 +++++++++--
 .../internal/constraints/constraint_builder_2d.h      |  3 ++-
 .../internal/constraints/constraint_builder_3d.cc     | 11 +++++++++--
 .../internal/constraints/constraint_builder_3d.h      |  3 ++-
 4 files changed, 22 insertions(+), 6 deletions(-)

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 <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());
 }
 
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 <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.
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 <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());
 }
 
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 <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.