From 2adeb1f3beecf8427296ed2ff5a3e11925187521 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 5 Feb 2019 17:15:45 +0100 Subject: [PATCH] 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 --- .../mapping/internal/2d/pose_graph_2d.cc | 26 +++++++++++++++++++ .../mapping/internal/3d/pose_graph_3d.cc | 26 +++++++++++++++++++ 2 files changed, 52 insertions(+) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index ca40340..4c22be3 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/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 diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index ddee1af..a7e533e 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/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