Adding metrics (#1240)

to measure fraction of sensor time that is taken up by voxel filter, scan matcher, and inserting into submap.

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

@@ -31,6 +31,9 @@ namespace cartographer {
 namespace mapping {
 
 static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
+static auto* kLocalSlamVoxelFilterFraction = metrics::Gauge::Null();
+static auto* kLocalSlamScanMatcherFraction = metrics::Gauge::Null();
+static auto* kLocalSlamInsertIntoSubmapFraction = metrics::Gauge::Null();
 static auto* kRealTimeCorrelativeScanMatcherScoreMetric =
     metrics::Histogram::Null();
 static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
@@ -108,6 +111,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
   std::vector<transform::Rigid3f> hits_poses;
   hits_poses.reserve(hits.size());
   bool warned = false;
+
   for (const auto& hit : hits) {
     common::Time time_point = time + common::FromSeconds(hit.point_time[3]);
     if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
@@ -150,18 +154,40 @@ LocalTrajectoryBuilder3D::AddRangeData(
   ++num_accumulated_;
 
   if (num_accumulated_ >= options_.num_accumulated_range_data()) {
+    const common::Time current_sensor_time = synchronized_data.time;
+    common::optional<common::Duration> sensor_duration;
+    if (last_sensor_time_.has_value()) {
+      sensor_duration = current_sensor_time - last_sensor_time_.value();
+    }
+    last_sensor_time_ = current_sensor_time;
     num_accumulated_ = 0;
+
     transform::Rigid3f current_pose =
         extrapolator_->ExtrapolatePose(time).cast<float>();
+
+    const auto voxel_filter_start = std::chrono::steady_clock::now();
     const sensor::RangeData filtered_range_data = {
         current_pose.translation(),
         sensor::VoxelFilter(options_.voxel_filter_size())
             .Filter(accumulated_range_data_.returns),
         sensor::VoxelFilter(options_.voxel_filter_size())
             .Filter(accumulated_range_data_.misses)};
+    const auto voxel_filter_stop = std::chrono::steady_clock::now();
+    const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
+
+    if (sensor_duration.has_value()) {
+      const double voxel_filter_fraction =
+          std::chrono::duration_cast<std::chrono::duration<double>>(
+              voxel_filter_duration)
+              .count() /
+          common::ToSeconds(sensor_duration.value());
+      kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction);
+    }
+
     return AddAccumulatedRangeData(
-        time, sensor::TransformRangeData(filtered_range_data,
+        time,
-                                         current_pose.inverse()));
+        sensor::TransformRangeData(filtered_range_data, current_pose.inverse()),
+        sensor_duration);
   }
   return nullptr;
 }
@@ -169,7 +195,8 @@ LocalTrajectoryBuilder3D::AddRangeData(
 std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
 LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
     const common::Time time,
-    const sensor::RangeData& filtered_range_data_in_tracking) {
+    const sensor::RangeData& filtered_range_data_in_tracking,
+    const common::optional<common::Duration>& sensor_duration) {
   if (filtered_range_data_in_tracking.returns.empty()) {
     LOG(WARNING) << "Dropped empty range data.";
     return nullptr;
@@ -178,6 +205,8 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
   const transform::Rigid3d pose_prediction =
       extrapolator_->ExtrapolatePose(time);
 
+  const auto scan_matcher_start = std::chrono::steady_clock::now();
+
   std::shared_ptr<const mapping::Submap3D> matching_submap =
       active_submaps_.submaps().front();
   transform::Rigid3d initial_ceres_pose =
@@ -219,6 +248,18 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
        {&low_resolution_point_cloud_in_tracking,
         &matching_submap->low_resolution_hybrid_grid()}},
       &pose_observation_in_submap, &summary);
+
+  const auto scan_matcher_stop = std::chrono::steady_clock::now();
+  const auto scan_matcher_duration = scan_matcher_stop - scan_matcher_start;
+  if (sensor_duration.has_value()) {
+    const double scan_matcher_fraction =
+        std::chrono::duration_cast<std::chrono::duration<double>>(
+            scan_matcher_duration)
+            .count() /
+        common::ToSeconds(sensor_duration.value());
+    kLocalSlamScanMatcherFraction->Set(scan_matcher_fraction);
+  }
+
   kCeresScanMatcherCostMetric->Observe(summary.final_cost);
   double residual_distance = (pose_observation_in_submap.translation() -
                               initial_ceres_pose.translation())
@@ -235,10 +276,26 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
 
   sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
       filtered_range_data_in_tracking, pose_estimate.cast<float>());
+
+  const auto insert_into_submap_start = std::chrono::steady_clock::now();
   std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
       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);
+
+  const auto insert_into_submap_stop = std::chrono::steady_clock::now();
+
+  const auto insert_into_submap_duration =
+      insert_into_submap_stop - insert_into_submap_start;
+  if (sensor_duration.has_value()) {
+    double insert_into_submap_fraction =
+        std::chrono::duration_cast<std::chrono::duration<double>>(
+            insert_into_submap_duration)
+            .count() /
+        common::ToSeconds(sensor_duration.value());
+    kLocalSlamInsertIntoSubmapFraction->Set(insert_into_submap_fraction);
+  }
+
   auto duration = std::chrono::steady_clock::now() - accumulation_started_;
   kLocalSlamLatencyMetric->Set(
       std::chrono::duration_cast<std::chrono::duration<double>>(duration)
@@ -304,6 +361,23 @@ void LocalTrajectoryBuilder3D::RegisterMetrics(
       "mapping_3d_local_trajectory_builder_latency",
       "Duration from first incoming point cloud in accumulation to local slam "
       "result");
+
+  auto* voxel_filter_fraction = family_factory->NewGaugeFamily(
+      "mapping_3d_local_trajectory_builder_voxel_filter_fraction",
+      "Fraction of total sensor time taken up by voxel filter.");
+  kLocalSlamVoxelFilterFraction = voxel_filter_fraction->Add({});
+
+  auto* scan_matcher_fraction = family_factory->NewGaugeFamily(
+      "mapping_3d_local_trajectory_builder_scan_matcher_fraction",
+      "Fraction of total sensor time taken up by scan matcher.");
+  kLocalSlamScanMatcherFraction = scan_matcher_fraction->Add({});
+
+  auto* insert_into_submap_fraction = family_factory->NewGaugeFamily(
+      "mapping_3d_local_trajectory_builder"
+      "insert_into_submap_fraction",
+      "Fraction of total sensor time taken up by inserting into submap.");
+  kLocalSlamInsertIntoSubmapFraction = insert_into_submap_fraction->Add({});
+
   kLocalSlamLatencyMetric = latency->Add({});
   auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
   auto* scores = family_factory->NewHistogramFamily(

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

@@ -77,7 +77,8 @@ class LocalTrajectoryBuilder3D {
  private:
   std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
       common::Time time,
-      const sensor::RangeData& filtered_range_data_in_tracking);
+      const sensor::RangeData& filtered_range_data_in_tracking,
+      const common::optional<common::Duration>& sensor_duration);
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
       common::Time time, const sensor::RangeData& filtered_range_data_in_local,
@@ -102,6 +103,8 @@ class LocalTrajectoryBuilder3D {
   std::chrono::steady_clock::time_point accumulation_started_;
 
   RangeDataCollator range_data_collator_;
+
+  common::optional<common::Time> last_sensor_time_;
 };
 
 }  // namespace mapping