From 30114e364ab9a6c7a7f627d14d963f8c01dc11d2 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 21 Feb 2018 17:31:11 +0100 Subject: [PATCH] Instrument metrics in constraint builders. (#921) RFC=[0014](https://github.com/googlecartographer/rfcs/blob/master/text/0014-monitoring.md) --- .../pose_graph/constraint_builder.cc | 43 ++++++++++++ .../pose_graph/constraint_builder.h | 3 + .../pose_graph/constraint_builder.cc | 67 +++++++++++++++++++ .../pose_graph/constraint_builder.h | 3 + cartographer/metrics/register.cc | 5 +- 5 files changed, 119 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.cc b/cartographer/mapping_2d/pose_graph/constraint_builder.cc index 7d0e0c7..0b7dd54 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/pose_graph/constraint_builder.cc @@ -31,6 +31,9 @@ #include "cartographer/common/thread_pool.h" #include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/metrics/counter.h" +#include "cartographer/metrics/gauge.h" +#include "cartographer/metrics/histogram.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -38,6 +41,14 @@ namespace cartographer { namespace mapping_2d { namespace pose_graph { +static auto* kConstraintsSearchedMetric = metrics::Counter::Null(); +static auto* kConstraintsFoundMetric = metrics::Counter::Null(); +static auto* kGlobalConstraintsSearchedMetric = metrics::Counter::Null(); +static auto* kGlobalConstraintsFoundMetric = metrics::Counter::Null(); +static auto* kQueueLengthMetric = metrics::Gauge::Null(); +static auto* kConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null(); + transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap) { return transform::Project2D(submap.local_pose()); } @@ -70,6 +81,7 @@ void ConstraintBuilder::MaybeAddConstraint( if (sampler_.Pulse()) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; @@ -89,6 +101,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::TrajectoryNode::Data* const constant_data) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; @@ -182,21 +195,27 @@ void ConstraintBuilder::ComputeConstraint( // 2. Prune if the score is too low. // 3. Refine. if (match_full_submap) { + kGlobalConstraintsSearchedMetric->Increment(); if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( constant_data->filtered_gravity_aligned_point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); + kGlobalConstraintsFoundMetric->Increment(); + kGlobalConstraintScoresMetric->Observe(score); } else { return; } } else { + kConstraintsSearchedMetric->Increment(); if (submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, constant_data->filtered_gravity_aligned_point_cloud, options_.min_score(), &score, &pose_estimate)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); + kConstraintsFoundMetric->Increment(); + kConstraintScoresMetric->Observe(score); } else { return; } @@ -269,6 +288,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { when_done_.reset(); } } + kQueueLengthMetric->Set(constraints_.size()); } if (callback != nullptr) { (*callback)(result); @@ -289,6 +309,29 @@ void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) { submap_scan_matchers_.erase(submap_id); } +void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) { + auto* counts = factory->NewCounterFamily( + "/mapping_2d/pose_graph/constraint_builder/constraints", + "Constraints computed"); + kConstraintsSearchedMetric = + counts->Add({{"search_region", "local"}, {"matcher", "searched"}}); + kConstraintsFoundMetric = + counts->Add({{"search_region", "local"}, {"matcher", "found"}}); + kGlobalConstraintsSearchedMetric = + counts->Add({{"search_region", "global"}, {"matcher", "searched"}}); + kGlobalConstraintsFoundMetric = + counts->Add({{"search_region", "global"}, {"matcher", "found"}}); + auto* queue_length = factory->NewGaugeFamily( + "/mapping_2d/pose_graph/constraint_builder/queue_length", "Queue length"); + kQueueLengthMetric = queue_length->Add({{}}); + auto boundaries = metrics::Histogram::FixedWidth(0.05, 20); + auto* scores = factory->NewHistogramFamily( + "/mapping_2d/pose_graph/constraint_builder/scores", + "Constraint scores built", boundaries); + kConstraintScoresMetric = scores->Add({{"search_region", "local"}}); + kGlobalConstraintScoresMetric = scores->Add({{"search_region", "global"}}); +} + } // namespace pose_graph } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.h b/cartographer/mapping_2d/pose_graph/constraint_builder.h index f8abf65..68f3940 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/pose_graph/constraint_builder.h @@ -35,6 +35,7 @@ #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_2d/submap_2d.h" +#include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/voxel_filter.h" @@ -103,6 +104,8 @@ class ConstraintBuilder { // Delete data related to 'submap_id'. void DeleteScanMatcher(const mapping::SubmapId& submap_id); + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + private: struct SubmapScanMatcher { const mapping::ProbabilityGrid* probability_grid; diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.cc b/cartographer/mapping_3d/pose_graph/constraint_builder.cc index 73f1bcd..5106a8f 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/pose_graph/constraint_builder.cc @@ -31,6 +31,9 @@ #include "cartographer/common/thread_pool.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/metrics/counter.h" +#include "cartographer/metrics/gauge.h" +#include "cartographer/metrics/histogram.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -38,6 +41,20 @@ namespace cartographer { namespace mapping_3d { namespace pose_graph { +static auto* kConstraintsSearchedMetric = metrics::Counter::Null(); +static auto* kConstraintsFoundMetric = metrics::Counter::Null(); +static auto* kGlobalConstraintsSearchedMetric = metrics::Counter::Null(); +static auto* kGlobalConstraintsFoundMetric = metrics::Counter::Null(); +static auto* kQueueLengthMetric = metrics::Gauge::Null(); +static auto* kConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kConstraintRotationalScoresMetric = metrics::Histogram::Null(); +static auto* kConstraintLowResolutionScoresMetric = metrics::Histogram::Null(); +static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kGlobalConstraintRotationalScoresMetric = + metrics::Histogram::Null(); +static auto* kGlobalConstraintLowResolutionScoresMetric = + metrics::Histogram::Null(); + ConstraintBuilder::ConstraintBuilder( const mapping::pose_graph::proto::ConstraintBuilderOptions& options, common::ThreadPool* const thread_pool) @@ -68,6 +85,7 @@ void ConstraintBuilder::MaybeAddConstraint( if (sampler_.Pulse()) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; @@ -90,6 +108,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( const Eigen::Quaterniond& global_submap_rotation) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; @@ -188,6 +207,7 @@ void ConstraintBuilder::ComputeConstraint( // 2. Prune if the score is too low. // 3. Refine. if (match_full_submap) { + kGlobalConstraintsSearchedMetric->Increment(); match_result = submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( global_node_pose.rotation(), global_submap_pose.rotation(), @@ -196,16 +216,29 @@ void ConstraintBuilder::ComputeConstraint( CHECK_GT(match_result->score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); + kGlobalConstraintsFoundMetric->Increment(); + kGlobalConstraintScoresMetric->Observe(match_result->score); + kGlobalConstraintRotationalScoresMetric->Observe( + match_result->rotational_score); + kGlobalConstraintLowResolutionScoresMetric->Observe( + match_result->low_resolution_score); } else { return; } } else { + kConstraintsSearchedMetric->Increment(); match_result = submap_scan_matcher->fast_correlative_scan_matcher->Match( global_node_pose, global_submap_pose, *constant_data, options_.min_score()); if (match_result != nullptr) { // We've reported a successful local match. CHECK_GT(match_result->score, options_.min_score()); + kConstraintsFoundMetric->Increment(); + kConstraintScoresMetric->Observe(match_result->score); + kConstraintRotationalScoresMetric->Observe( + match_result->rotational_score); + kConstraintLowResolutionScoresMetric->Observe( + match_result->low_resolution_score); } else { return; } @@ -290,6 +323,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { when_done_.reset(); } } + kQueueLengthMetric->Set(constraints_.size()); } if (callback != nullptr) { (*callback)(result); @@ -310,6 +344,39 @@ void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) { submap_scan_matchers_.erase(submap_id); } +void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) { + auto* counts = factory->NewCounterFamily( + "/mapping_3d/pose_graph/constraint_builder/constraints", + "Constraints computed"); + kConstraintsSearchedMetric = + counts->Add({{"search_region", "local"}, {"matcher", "searched"}}); + kConstraintsFoundMetric = + counts->Add({{"search_region", "local"}, {"matcher", "found"}}); + kGlobalConstraintsSearchedMetric = + counts->Add({{"search_region", "global"}, {"matcher", "searched"}}); + kGlobalConstraintsFoundMetric = + counts->Add({{"search_region", "global"}, {"matcher", "found"}}); + auto* queue_length = factory->NewGaugeFamily( + "/mapping_3d/pose_graph/constraint_builder/queue_length", "Queue length"); + kQueueLengthMetric = queue_length->Add({{}}); + auto boundaries = metrics::Histogram::FixedWidth(0.05, 20); + auto* scores = factory->NewHistogramFamily( + "/mapping_3d/pose_graph/constraint_builder/scores", + "Constraint scores built", boundaries); + kConstraintScoresMetric = + scores->Add({{"search_region", "local"}, {"kind", "score"}}); + kConstraintRotationalScoresMetric = + scores->Add({{"search_region", "local"}, {"kind", "rotational_score"}}); + kConstraintLowResolutionScoresMetric = scores->Add( + {{"search_region", "local"}, {"kind", "low_resolution_score"}}); + kGlobalConstraintScoresMetric = + scores->Add({{"search_region", "global"}, {"kind", "score"}}); + kGlobalConstraintRotationalScoresMetric = + scores->Add({{"search_region", "global"}, {"kind", "rotational_score"}}); + kGlobalConstraintLowResolutionScoresMetric = scores->Add( + {{"search_region", "global"}, {"kind", "low_resolution_score"}}); +} + } // namespace pose_graph } // namespace mapping_3d } // namespace cartographer diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.h b/cartographer/mapping_3d/pose_graph/constraint_builder.h index 5157163..5df4760 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/pose_graph/constraint_builder.h @@ -37,6 +37,7 @@ #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_3d/submap_3d.h" +#include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/voxel_filter.h" @@ -112,6 +113,8 @@ class ConstraintBuilder { // Delete data related to 'submap_id'. void DeleteScanMatcher(const mapping::SubmapId& submap_id); + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + private: struct SubmapScanMatcher { const mapping::HybridGrid* high_resolution_hybrid_grid; diff --git a/cartographer/metrics/register.cc b/cartographer/metrics/register.cc index 3b23e1e..18bb7d5 100644 --- a/cartographer/metrics/register.cc +++ b/cartographer/metrics/register.cc @@ -22,8 +22,9 @@ namespace cartographer { namespace metrics { -void RegisterAllMetrics(FamilyFactory *registry) { - // TODO(gaschler): Register classes that instrument metrics. +void RegisterAllMetrics(FamilyFactory* registry) { + mapping_2d::pose_graph::ConstraintBuilder::RegisterMetrics(registry); + mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry); } } // namespace metrics