From 01cb6b1d6f2e67d0678d7ff2fe974d7ab956c49c Mon Sep 17 00:00:00 2001
From: Michael Grupp <grupp@magazino.eu>
Date: Tue, 25 Aug 2020 11:04:59 +0200
Subject: [PATCH] Add gauge metric for current number of submap scan matchers.
 (#1738)

Useful for analyzing memory-related issues.
See: #1737

Signed-off-by: Michael Grupp <grupp@magazino.eu>
---
 .../mapping/internal/constraints/constraint_builder_2d.cc  | 7 +++++++
 .../mapping/internal/constraints/constraint_builder_3d.cc  | 7 +++++++
 2 files changed, 14 insertions(+)

diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc
index 58e830c..d612e7d 100644
--- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc
+++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc
@@ -48,6 +48,7 @@ static auto* kGlobalConstraintsFoundMetric = metrics::Counter::Null();
 static auto* kQueueLengthMetric = metrics::Gauge::Null();
 static auto* kConstraintScoresMetric = metrics::Histogram::Null();
 static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null();
+static auto* kNumSubmapScanMatchersMetric = metrics::Gauge::Null();
 
 transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) {
   return transform::Project2D(submap.local_pose());
@@ -163,6 +164,7 @@ ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
     return &submap_scan_matchers_.at(submap_id);
   }
   auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
+  kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
   submap_scan_matcher.grid = grid;
   auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();
   auto scan_matcher_task = absl::make_unique<common::Task>();
@@ -303,6 +305,7 @@ void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) {
         << "DeleteScanMatcher was called while WhenDone was scheduled.";
   }
   submap_scan_matchers_.erase(submap_id);
+  kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
 }
 
 void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) {
@@ -326,6 +329,10 @@ void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) {
       "Constraint scores built", boundaries);
   kConstraintScoresMetric = scores->Add({{"search_region", "local"}});
   kGlobalConstraintScoresMetric = scores->Add({{"search_region", "global"}});
+  auto* num_matchers = factory->NewGaugeFamily(
+      "mapping_constraints_constraint_builder_2d_num_submap_scan_matchers",
+      "Current number of constructed submap scan matchers");
+  kNumSubmapScanMatchersMetric = num_matchers->Add({});
 }
 
 }  // namespace constraints
diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc
index ef6dbd4..1672977 100644
--- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc
+++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc
@@ -54,6 +54,7 @@ static auto* kGlobalConstraintRotationalScoresMetric =
     metrics::Histogram::Null();
 static auto* kGlobalConstraintLowResolutionScoresMetric =
     metrics::Histogram::Null();
+static auto* kNumSubmapScanMatchersMetric = metrics::Gauge::Null();
 
 ConstraintBuilder3D::ConstraintBuilder3D(
     const proto::ConstraintBuilderOptions& options,
@@ -167,6 +168,7 @@ ConstraintBuilder3D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
     return &submap_scan_matchers_.at(submap_id);
   }
   auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
+  kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
   submap_scan_matcher.high_resolution_hybrid_grid =
       &submap->high_resolution_hybrid_grid();
   submap_scan_matcher.low_resolution_hybrid_grid =
@@ -334,6 +336,7 @@ void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) {
         << "DeleteScanMatcher was called while WhenDone was scheduled.";
   }
   submap_scan_matchers_.erase(submap_id);
+  kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
 }
 
 void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) {
@@ -367,6 +370,10 @@ void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) {
       scores->Add({{"search_region", "global"}, {"kind", "rotational_score"}});
   kGlobalConstraintLowResolutionScoresMetric = scores->Add(
       {{"search_region", "global"}, {"kind", "low_resolution_score"}});
+  auto* num_matchers = factory->NewGaugeFamily(
+      "mapping_constraints_constraint_builder_3d_num_submap_scan_matchers",
+      "Current number of constructed submap scan matchers");
+  kNumSubmapScanMatchersMetric = num_matchers->Add({});
 }
 
 }  // namespace constraints