12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Michael Grupp 2018-11-22 10:25:43 +01:00 committed by Wally B. Feed
parent 7d69ca9fa0
commit e0b2196cec
2 changed files with 56 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -42,6 +42,8 @@ namespace cartographer {
namespace mapping { namespace mapping {
static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null();
PoseGraph2D::PoseGraph2D( PoseGraph2D::PoseGraph2D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
@ -478,6 +480,25 @@ void PoseGraph2D::HandleWorkQueue(
trimmers_.end()); trimmers_.end());
num_nodes_since_last_loop_closure_ = 0; 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(); DrainWorkQueue();
@ -1205,6 +1226,13 @@ void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory* family_factory) {
"mapping_2d_pose_graph_work_queue_delay", "mapping_2d_pose_graph_work_queue_delay",
"Age of the oldest entry in the work queue in seconds"); "Age of the oldest entry in the work queue in seconds");
kWorkQueueDelayMetric = latency->Add({}); 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 } // namespace mapping

28
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -40,6 +40,8 @@ namespace cartographer {
namespace mapping { namespace mapping {
static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null();
PoseGraph3D::PoseGraph3D( PoseGraph3D::PoseGraph3D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
@ -472,6 +474,25 @@ void PoseGraph3D::HandleWorkQueue(
trimmers_.end()); trimmers_.end());
num_nodes_since_last_loop_closure_ = 0; 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(); DrainWorkQueue();
@ -1208,6 +1229,13 @@ void PoseGraph3D::RegisterMetrics(metrics::FamilyFactory* family_factory) {
"mapping_3d_pose_graph_work_queue_delay", "mapping_3d_pose_graph_work_queue_delay",
"Age of the oldest entry in the work queue in seconds"); "Age of the oldest entry in the work queue in seconds");
kWorkQueueDelayMetric = latency->Add({}); 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 } // namespace mapping