Instrument metrics in constraint builders. (#921)

RFC=[0014](https://github.com/googlecartographer/rfcs/blob/master/text/0014-monitoring.md)

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

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;

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

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;

cartographer/metrics/register.cc

@@ -23,7 +23,8 @@ namespace cartographer {
 namespace metrics {
 
 void RegisterAllMetrics(FamilyFactory* registry) {
-  // TODO(gaschler): Register classes that instrument metrics.
+  mapping_2d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
+  mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
 }
 
 }  // namespace metrics