From e0b2196cec0ddbea719117b08c4eb5e5ee4b8d2b Mon Sep 17 00:00:00 2001
From: Michael Grupp <grupp@magazino.eu>
Date: Thu, 22 Nov 2018 10:25:43 +0100
Subject: [PATCH] Add gauge metrics for inter-submap constraints. (#1465)

Since the constraint builder metrics are ever-increasing, it makes sense
to add gauges that monitor the current counts of constraints in the pose
graph. The main advantages are that they also take trimming into account
and distinguish constraints within or across trajectories.
---
 .../mapping/internal/2d/pose_graph_2d.cc      | 28 +++++++++++++++++++
 .../mapping/internal/3d/pose_graph_3d.cc      | 28 +++++++++++++++++++
 2 files changed, 56 insertions(+)

diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
index 4e4c141..b812dd1 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.cc
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@@ -42,6 +42,8 @@ namespace cartographer {
 namespace mapping {
 
 static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
+static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
+static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null();
 
 PoseGraph2D::PoseGraph2D(
     const proto::PoseGraphOptions& options,
@@ -478,6 +480,25 @@ void PoseGraph2D::HandleWorkQueue(
         trimmers_.end());
 
     num_nodes_since_last_loop_closure_ = 0;
+
+    // Update the gauges that count the current number of constraints.
+    double inter_constraints_same_trajectory = 0;
+    double inter_constraints_different_trajectory = 0;
+    for (const auto& constraint : data_.constraints) {
+      if (constraint.tag ==
+          cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
+        continue;
+      }
+      if (constraint.node_id.trajectory_id ==
+          constraint.submap_id.trajectory_id) {
+        ++inter_constraints_same_trajectory;
+      } else {
+        ++inter_constraints_different_trajectory;
+      }
+    }
+    kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
+    kConstraintsDifferentTrajectoryMetric->Set(
+        inter_constraints_different_trajectory);
   }
 
   DrainWorkQueue();
@@ -1205,6 +1226,13 @@ void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory* family_factory) {
       "mapping_2d_pose_graph_work_queue_delay",
       "Age of the oldest entry in the work queue in seconds");
   kWorkQueueDelayMetric = latency->Add({});
+  auto* constraints = family_factory->NewGaugeFamily(
+      "mapping_2d_pose_graph_constraints",
+      "Current number of constraints in the pose graph");
+  kConstraintsDifferentTrajectoryMetric =
+      constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}});
+  kConstraintsSameTrajectoryMetric =
+      constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}});
 }
 
 }  // namespace mapping
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc
index 5a2842a..d79c186 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.cc
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc
@@ -40,6 +40,8 @@ namespace cartographer {
 namespace mapping {
 
 static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
+static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
+static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null();
 
 PoseGraph3D::PoseGraph3D(
     const proto::PoseGraphOptions& options,
@@ -472,6 +474,25 @@ void PoseGraph3D::HandleWorkQueue(
         trimmers_.end());
 
     num_nodes_since_last_loop_closure_ = 0;
+
+    // Update the gauges that count the current number of constraints.
+    double inter_constraints_same_trajectory = 0;
+    double inter_constraints_different_trajectory = 0;
+    for (const auto& constraint : data_.constraints) {
+      if (constraint.tag ==
+          cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
+        continue;
+      }
+      if (constraint.node_id.trajectory_id ==
+          constraint.submap_id.trajectory_id) {
+        ++inter_constraints_same_trajectory;
+      } else {
+        ++inter_constraints_different_trajectory;
+      }
+    }
+    kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
+    kConstraintsDifferentTrajectoryMetric->Set(
+        inter_constraints_different_trajectory);
   }
 
   DrainWorkQueue();
@@ -1208,6 +1229,13 @@ void PoseGraph3D::RegisterMetrics(metrics::FamilyFactory* family_factory) {
       "mapping_3d_pose_graph_work_queue_delay",
       "Age of the oldest entry in the work queue in seconds");
   kWorkQueueDelayMetric = latency->Add({});
+  auto* constraints = family_factory->NewGaugeFamily(
+      "mapping_3d_pose_graph_constraints",
+      "Current number constraints in the pose graph");
+  kConstraintsDifferentTrajectoryMetric =
+      constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}});
+  kConstraintsSameTrajectoryMetric =
+      constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}});
 }
 
 }  // namespace mapping