Add gauge metrics for submaps in pose graph. (#1511)

Adds a metric family that monitors:

- the number of submaps in frozen trajectories
- the number of submaps in active trajectories
- the number of deleted/trimmed submaps

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -44,6 +44,9 @@ namespace mapping {
 static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
 static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
 static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null();
+static auto* kActiveSubmapsMetric = metrics::Gauge::Null();
+static auto* kFrozenSubmapsMetric = metrics::Gauge::Null();
+static auto* kDeletedSubmapsMetric = metrics::Gauge::Null();
 
 PoseGraph2D::PoseGraph2D(
     const proto::PoseGraphOptions& options,
@@ -142,6 +145,7 @@ NodeId PoseGraph2D::AppendNode(
         data_.submap_data.Append(trajectory_id, InternalSubmapData());
     data_.submap_data.at(submap_id).submap = insertion_submaps.back();
     LOG(INFO) << "Inserted submap " << submap_id << ".";
+    kActiveSubmapsMetric->Increment();
   }
   return node_id;
 }
@@ -680,6 +684,15 @@ void PoseGraph2D::AddSubmapFromProto(
     data_.global_submap_poses_2d.Insert(
         submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
   }
+
+  // TODO(MichaelGrupp): MapBuilder does freezing before deserializing submaps,
+  // so this should be fine.
+  if (IsTrajectoryFrozen(submap_id.trajectory_id)) {
+    kFrozenSubmapsMetric->Increment();
+  } else {
+    kActiveSubmapsMetric->Increment();
+  }
+
   AddWorkItem(
       [this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) {
         absl::MutexLock locker(&mutex_);
@@ -1200,6 +1213,14 @@ void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
   parent_->constraint_builder_.DeleteScanMatcher(submap_id);
   parent_->optimization_problem_->TrimSubmap(submap_id);
 
+  // We have one submap less, update the gauge metrics.
+  kDeletedSubmapsMetric->Increment();
+  if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) {
+    kFrozenSubmapsMetric->Decrement();
+  } else {
+    kActiveSubmapsMetric->Decrement();
+  }
+
   // Remove the 'nodes_to_remove' from the pose graph and the optimization
   // problem.
   for (const NodeId& node_id : nodes_to_remove) {
@@ -1235,6 +1256,11 @@ void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory* family_factory) {
       constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}});
   kConstraintsSameTrajectoryMetric =
       constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}});
+  auto* submaps = family_factory->NewGaugeFamily(
+      "mapping_2d_pose_graph_submaps", "Number of submaps in the pose graph.");
+  kActiveSubmapsMetric = submaps->Add({{"state", "active"}});
+  kFrozenSubmapsMetric = submaps->Add({{"state", "frozen"}});
+  kDeletedSubmapsMetric = submaps->Add({{"state", "deleted"}});
 }
 
 }  // namespace mapping

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -42,6 +42,9 @@ namespace mapping {
 static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
 static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
 static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null();
+static auto* kActiveSubmapsMetric = metrics::Gauge::Null();
+static auto* kFrozenSubmapsMetric = metrics::Gauge::Null();
+static auto* kDeletedSubmapsMetric = metrics::Gauge::Null();
 
 PoseGraph3D::PoseGraph3D(
     const proto::PoseGraphOptions& options,
@@ -130,6 +133,7 @@ NodeId PoseGraph3D::AppendNode(
         data_.submap_data.Append(trajectory_id, InternalSubmapData());
     data_.submap_data.at(submap_id).submap = insertion_submaps.back();
     LOG(INFO) << "Inserted submap " << submap_id << ".";
+    kActiveSubmapsMetric->Increment();
   }
   return node_id;
 }
@@ -671,6 +675,15 @@ void PoseGraph3D::AddSubmapFromProto(
     data_.global_submap_poses_3d.Insert(
         submap_id, optimization::SubmapSpec3D{global_submap_pose});
   }
+
+  // TODO(MichaelGrupp): MapBuilder does freezing before deserializing submaps,
+  // so this should be fine.
+  if (IsTrajectoryFrozen(submap_id.trajectory_id)) {
+    kFrozenSubmapsMetric->Increment();
+  } else {
+    kActiveSubmapsMetric->Increment();
+  }
+
   AddWorkItem([this, submap_id, global_submap_pose]() LOCKS_EXCLUDED(mutex_) {
     absl::MutexLock locker(&mutex_);
     data_.submap_data.at(submap_id).state = SubmapState::kFinished;
@@ -1203,6 +1216,14 @@ void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
   parent_->constraint_builder_.DeleteScanMatcher(submap_id);
   parent_->optimization_problem_->TrimSubmap(submap_id);
 
+  // We have one submap less, update the gauge metrics.
+  kDeletedSubmapsMetric->Increment();
+  if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) {
+    kFrozenSubmapsMetric->Decrement();
+  } else {
+    kActiveSubmapsMetric->Decrement();
+  }
+
   // Remove the 'nodes_to_remove' from the pose graph and the optimization
   // problem.
   for (const NodeId& node_id : nodes_to_remove) {
@@ -1238,6 +1259,11 @@ void PoseGraph3D::RegisterMetrics(metrics::FamilyFactory* family_factory) {
       constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}});
   kConstraintsSameTrajectoryMetric =
       constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}});
+  auto* submaps = family_factory->NewGaugeFamily(
+      "mapping_3d_pose_graph_submaps", "Number of submaps in the pose graph.");
+  kActiveSubmapsMetric = submaps->Add({{"state", "active"}});
+  kFrozenSubmapsMetric = submaps->Add({{"state", "frozen"}});
+  kDeletedSubmapsMetric = submaps->Add({{"state", "deleted"}});
 }
 
 }  // namespace mapping