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
master
Michael Grupp 2019-02-05 17:15:45 +01:00 committed by Wally B. Feed
parent 2be43a3087
commit 2adeb1f3be
2 changed files with 52 additions and 0 deletions

View File

@ -44,6 +44,9 @@ namespace mapping {
static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null(); static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
static auto* kConstraintsDifferentTrajectoryMetric = 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( PoseGraph2D::PoseGraph2D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
@ -142,6 +145,7 @@ NodeId PoseGraph2D::AppendNode(
data_.submap_data.Append(trajectory_id, InternalSubmapData()); data_.submap_data.Append(trajectory_id, InternalSubmapData());
data_.submap_data.at(submap_id).submap = insertion_submaps.back(); data_.submap_data.at(submap_id).submap = insertion_submaps.back();
LOG(INFO) << "Inserted submap " << submap_id << "."; LOG(INFO) << "Inserted submap " << submap_id << ".";
kActiveSubmapsMetric->Increment();
} }
return node_id; return node_id;
} }
@ -680,6 +684,15 @@ void PoseGraph2D::AddSubmapFromProto(
data_.global_submap_poses_2d.Insert( data_.global_submap_poses_2d.Insert(
submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); 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( AddWorkItem(
[this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) { [this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_); absl::MutexLock locker(&mutex_);
@ -1200,6 +1213,14 @@ void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_->TrimSubmap(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 // Remove the 'nodes_to_remove' from the pose graph and the optimization
// problem. // problem.
for (const NodeId& node_id : nodes_to_remove) { 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"}}); constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}});
kConstraintsSameTrajectoryMetric = kConstraintsSameTrajectoryMetric =
constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}}); 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 } // namespace mapping

View File

@ -42,6 +42,9 @@ namespace mapping {
static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null(); static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
static auto* kConstraintsDifferentTrajectoryMetric = 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( PoseGraph3D::PoseGraph3D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
@ -130,6 +133,7 @@ NodeId PoseGraph3D::AppendNode(
data_.submap_data.Append(trajectory_id, InternalSubmapData()); data_.submap_data.Append(trajectory_id, InternalSubmapData());
data_.submap_data.at(submap_id).submap = insertion_submaps.back(); data_.submap_data.at(submap_id).submap = insertion_submaps.back();
LOG(INFO) << "Inserted submap " << submap_id << "."; LOG(INFO) << "Inserted submap " << submap_id << ".";
kActiveSubmapsMetric->Increment();
} }
return node_id; return node_id;
} }
@ -671,6 +675,15 @@ void PoseGraph3D::AddSubmapFromProto(
data_.global_submap_poses_3d.Insert( data_.global_submap_poses_3d.Insert(
submap_id, optimization::SubmapSpec3D{global_submap_pose}); 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_) { AddWorkItem([this, submap_id, global_submap_pose]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.submap_data.at(submap_id).state = SubmapState::kFinished; 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_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_->TrimSubmap(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 // Remove the 'nodes_to_remove' from the pose graph and the optimization
// problem. // problem.
for (const NodeId& node_id : nodes_to_remove) { 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"}}); constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}});
kConstraintsSameTrajectoryMetric = kConstraintsSameTrajectoryMetric =
constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}}); 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 } // namespace mapping