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