Instrument metrics in local trajectory builders. (#946)

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

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -20,11 +20,19 @@
 #include <memory>
 
 #include "cartographer/common/make_unique.h"
+#include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/range_data.h"
 
 namespace cartographer {
 namespace mapping {
 
+static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
+static auto* kFastCorrelativeScanMatcherScoreMetric =
+    metrics::Histogram::Null();
+static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
+static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null();
+static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null();
+
 LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
     const proto::LocalTrajectoryBuilderOptions2D& options)
     : options_(options),
@@ -66,9 +74,10 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
     return nullptr;
   }
   if (options_.use_online_correlative_scan_matching()) {
-    real_time_correlative_scan_matcher_.Match(
+    double score = real_time_correlative_scan_matcher_.Match(
         pose_prediction, filtered_gravity_aligned_point_cloud,
         matching_submap->probability_grid(), &initial_ceres_pose);
+    kFastCorrelativeScanMatcherScoreMetric->Observe(score);
   }
 
   auto pose_observation = common::make_unique<transform::Rigid2d>();
@@ -77,6 +86,16 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
                             filtered_gravity_aligned_point_cloud,
                             matching_submap->probability_grid(),
                             pose_observation.get(), &summary);
+  if (pose_observation) {
+    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
+    double residual_distance =
+        (pose_observation->translation() - pose_prediction.translation())
+            .norm();
+    kScanMatcherResidualDistanceMetric->Observe(residual_distance);
+    double residual_angle = std::abs(pose_observation->rotation().angle() -
+                                     pose_prediction.rotation().angle());
+    kScanMatcherResidualAngleMetric->Observe(residual_angle);
+  }
   return pose_observation;
 }
 
@@ -104,6 +123,10 @@ LocalTrajectoryBuilder2D::AddRangeData(
     return nullptr;
   }
 
+  if (num_accumulated_ == 0) {
+    accumulation_started_ = std::chrono::steady_clock::now();
+  }
+
   std::vector<transform::Rigid3f> range_data_poses;
   range_data_poses.reserve(range_data.returns.size());
   bool warned = false;
@@ -198,6 +221,9 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
   std::unique_ptr<InsertionResult> insertion_result =
       InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
                        pose_estimate, gravity_alignment.rotation());
+  auto duration = std::chrono::steady_clock::now() - accumulation_started_;
+  kLocalSlamLatencyMetric->Set(
+      std::chrono::duration_cast<std::chrono::seconds>(duration).count());
   return common::make_unique<MatchingResult>(
       MatchingResult{time, pose_estimate, std::move(range_data_in_local),
                      std::move(insertion_result)});
@@ -269,5 +295,32 @@ void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
   extrapolator_->AddPose(time, transform::Rigid3d::Identity());
 }
 
+void LocalTrajectoryBuilder2D::RegisterMetrics(
+    metrics::FamilyFactory* family_factory) {
+  auto* latency = family_factory->NewGaugeFamily(
+      "/mapping/internal/2d/local_trajectory_builder/latency",
+      "Duration from first incoming point cloud in accumulation to local slam "
+      "result");
+  kLocalSlamLatencyMetric = latency->Add({});
+  auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
+  auto* scores = family_factory->NewHistogramFamily(
+      "/mapping/internal/2d/local_trajectory_builder/scores",
+      "Local scan matcher scores", score_boundaries);
+  kFastCorrelativeScanMatcherScoreMetric =
+      scores->Add({{"scan_matcher", "fast_correlative"}});
+  auto cost_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 100);
+  auto* costs = family_factory->NewHistogramFamily(
+      "/mapping/internal/2d/local_trajectory_builder/costs",
+      "Local scan matcher costs", cost_boundaries);
+  kCeresScanMatcherCostMetric = costs->Add({{"scan_matcher", "ceres"}});
+  auto distance_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 10);
+  auto* residuals = family_factory->NewHistogramFamily(
+      "/mapping/internal/2d/local_trajectory_builder/residuals",
+      "Local scan matcher residuals", distance_boundaries);
+  kScanMatcherResidualDistanceMetric =
+      residuals->Add({{"component", "distance"}});
+  kScanMatcherResidualAngleMetric = residuals->Add({{"component", "angle"}});
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/2d/local_trajectory_builder_2d.h

@@ -17,6 +17,7 @@
 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_H_
 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_H_
 
+#include <chrono>
 #include <memory>
 
 #include "cartographer/common/time.h"
@@ -26,6 +27,7 @@
 #include "cartographer/mapping/2d/submap_2d.h"
 #include "cartographer/mapping/internal/motion_filter.h"
 #include "cartographer/mapping/pose_extrapolator.h"
+#include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/range_data.h"
@@ -69,6 +71,8 @@ class LocalTrajectoryBuilder2D {
   void AddImuData(const sensor::ImuData& imu_data);
   void AddOdometryData(const sensor::OdometryData& odometry_data);
 
+  static void RegisterMetrics(metrics::FamilyFactory* family_factory);
+
  private:
   std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
       common::Time time, const sensor::RangeData& gravity_aligned_range_data,
@@ -104,6 +108,7 @@ class LocalTrajectoryBuilder2D {
 
   int num_accumulated_ = 0;
   sensor::RangeData accumulated_range_data_;
+  std::chrono::steady_clock::time_point accumulation_started_;
 };
 
 }  // namespace mapping

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -30,6 +30,13 @@
 namespace cartographer {
 namespace mapping {
 
+static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
+static auto* kRealTimeCorrelativeScanMatcherScoreMetric =
+    metrics::Histogram::Null();
+static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
+static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null();
+static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null();
+
 LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
     const mapping::proto::LocalTrajectoryBuilderOptions3D& options)
     : options_(options),
@@ -78,6 +85,10 @@ LocalTrajectoryBuilder3D::AddRangeData(
     return nullptr;
   }
 
+  if (num_accumulated_ == 0) {
+    accumulation_started_ = std::chrono::steady_clock::now();
+  }
+
   sensor::TimedPointCloud hits =
       sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
           .Filter(range_data.returns);
@@ -168,9 +179,10 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
   if (options_.use_online_correlative_scan_matching()) {
     // We take a copy since we use 'initial_ceres_pose' as an output argument.
     const transform::Rigid3d initial_pose = initial_ceres_pose;
-    real_time_correlative_scan_matcher_->Match(
+    double score = real_time_correlative_scan_matcher_->Match(
         initial_pose, high_resolution_point_cloud_in_tracking,
         matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose);
+    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
   }
 
   transform::Rigid3d pose_observation_in_submap;
@@ -193,6 +205,14 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
        {&low_resolution_point_cloud_in_tracking,
         &matching_submap->low_resolution_hybrid_grid()}},
       &pose_observation_in_submap, &summary);
+  kCeresScanMatcherCostMetric->Observe(summary.final_cost);
+  double residual_distance = (pose_observation_in_submap.translation() -
+                              initial_ceres_pose.translation())
+                                 .norm();
+  kScanMatcherResidualDistanceMetric->Observe(residual_distance);
+  double residual_angle = pose_observation_in_submap.rotation().angularDistance(
+      initial_ceres_pose.rotation());
+  kScanMatcherResidualAngleMetric->Observe(residual_angle);
   const transform::Rigid3d pose_estimate =
       matching_submap->local_pose() * pose_observation_in_submap;
   extrapolator_->AddPose(time, pose_estimate);
@@ -205,6 +225,9 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
       time, filtered_range_data_in_local, filtered_range_data_in_tracking,
       high_resolution_point_cloud_in_tracking,
       low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);
+  auto duration = std::chrono::steady_clock::now() - accumulation_started_;
+  kLocalSlamLatencyMetric->Set(
+      std::chrono::duration_cast<std::chrono::seconds>(duration).count());
   return common::make_unique<MatchingResult>(MatchingResult{
       time, pose_estimate, std::move(filtered_range_data_in_local),
       std::move(insertion_result)});
@@ -260,5 +283,32 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
                       std::move(insertion_submaps)});
 }
 
+void LocalTrajectoryBuilder3D::RegisterMetrics(
+    metrics::FamilyFactory* family_factory) {
+  auto* latency = family_factory->NewGaugeFamily(
+      "/mapping/internal/3d/local_trajectory_builder/latency",
+      "Duration from first incoming point cloud in accumulation to local slam "
+      "result");
+  kLocalSlamLatencyMetric = latency->Add({});
+  auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
+  auto* scores = family_factory->NewHistogramFamily(
+      "/mapping/internal/3d/local_trajectory_builder/scores",
+      "Local scan matcher scores", score_boundaries);
+  kRealTimeCorrelativeScanMatcherScoreMetric =
+      scores->Add({{"scan_matcher", "real_time_correlative"}});
+  auto cost_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 100);
+  auto* costs = family_factory->NewHistogramFamily(
+      "/mapping/internal/3d/local_trajectory_builder/costs",
+      "Local scan matcher costs", cost_boundaries);
+  kCeresScanMatcherCostMetric = costs->Add({{"scan_matcher", "ceres"}});
+  auto distance_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 10);
+  auto* residuals = family_factory->NewHistogramFamily(
+      "/mapping/internal/3d/local_trajectory_builder/residuals",
+      "Local scan matcher residuals", distance_boundaries);
+  kScanMatcherResidualDistanceMetric =
+      residuals->Add({{"component", "distance"}});
+  kScanMatcherResidualAngleMetric = residuals->Add({{"component", "angle"}});
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/3d/local_trajectory_builder_3d.h

@@ -17,6 +17,7 @@
 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_
 #define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_
 
+#include <chrono>
 #include <memory>
 
 #include "cartographer/common/time.h"
@@ -26,6 +27,7 @@
 #include "cartographer/mapping/3d/submap_3d.h"
 #include "cartographer/mapping/internal/motion_filter.h"
 #include "cartographer/mapping/pose_extrapolator.h"
+#include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/range_data.h"
@@ -67,6 +69,8 @@ class LocalTrajectoryBuilder3D {
       common::Time time, const sensor::TimedRangeData& range_data);
   void AddOdometryData(const sensor::OdometryData& odometry_data);
 
+  static void RegisterMetrics(metrics::FamilyFactory* family_factory);
+
  private:
   std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
       common::Time time,
@@ -92,6 +96,7 @@ class LocalTrajectoryBuilder3D {
 
   int num_accumulated_ = 0;
   sensor::RangeData accumulated_range_data_;
+  std::chrono::steady_clock::time_point accumulation_started_;
 };
 
 }  // namespace mapping

cartographer/metrics/register.cc

@@ -18,6 +18,8 @@
 
 #include "cartographer/mapping/2d/pose_graph/constraint_builder_2d.h"
 #include "cartographer/mapping/3d/pose_graph/constraint_builder_3d.h"
+#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h"
+#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h"
 
 namespace cartographer {
 namespace metrics {
@@ -25,6 +27,8 @@ namespace metrics {
 void RegisterAllMetrics(FamilyFactory* registry) {
   mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry);
   mapping::pose_graph::ConstraintBuilder3D::RegisterMetrics(registry);
+  mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry);
+  mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry);
 }
 
 }  // namespace metrics