Add metrics: real time ratio and cpu time ratio. (#1275)

This PR introduces two metrics to measure performance of local slam: real time ratio, and cpu real time ratio. The latter is introduced to measure the headroom. 

Real time ratio tells us how fast local slam processes the incoming sensor data. When SLAM is falling behind, this is below 100%. In the steady state it is 100% (The real time ratio is above 100% only when slam is 'catching up'.). So this does not tell us how much faster than real time slam processes the data. For this purpose the cpu real time ratio is introduced. It measures how much cpu time slam used, compared to the real time passed to collect the sensor data. Thus it can be more than 100% if slam is faster than real time. For example, a cpu real time ratio of 200% means that slam could process the data twice as fast as it comes in. 

Three durations are measured:
- sensor_duration: the physical time it took to collect the sensor data. 
- wall_time_duration: the time it took to process that data
- thread_cpu_duration: CPU time it took to process that data. 

And the metrics then are: 
real time ratio = sensor_duration / wall_time_duration
cpu real time ratio = sensor_duration / thread_cpu_duration

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

@@ -36,6 +36,10 @@ static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
 static auto* kLocalSlamVoxelFilterFraction = metrics::Gauge::Null();
 static auto* kLocalSlamScanMatcherFraction = metrics::Gauge::Null();
 static auto* kLocalSlamInsertIntoSubmapFraction = metrics::Gauge::Null();
+// TODO(spielawa): Add the following two metrics also to
+// local_trajectory_builder_2d
+static auto* kLocalSlamRealTimeRatio = metrics::Gauge::Null();
+static auto* kLocalSlamCpuRealTimeRatio = metrics::Gauge::Null();
 static auto* kRealTimeCorrelativeScanMatcherScoreMetric =
     metrics::Histogram::Null();
 static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
@@ -302,13 +306,27 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
         common::ToSeconds(sensor_duration.value());
     kLocalSlamInsertIntoSubmapFraction->Set(insert_into_submap_fraction);
   }
-  const auto accumulation_stop = std::chrono::steady_clock::now();
+  const auto wall_time = std::chrono::steady_clock::now();
-  if (last_accumulation_stop_.has_value()) {
+  if (last_wall_time_.has_value()) {
-    const auto accumulation_duration =
+    const auto wall_time_duration = wall_time - last_wall_time_.value();
-        accumulation_stop - last_accumulation_stop_.value();
+    kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));
-    kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
+    if (sensor_duration.has_value()) {
+      kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /
+                                   common::ToSeconds(wall_time_duration));
+    }
   }
-  last_accumulation_stop_ = accumulation_stop;
+  const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
+  if (last_thread_cpu_time_seconds_.has_value()) {
+    const double thread_cpu_duration_seconds =
+        thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
+    if (sensor_duration.has_value()) {
+      kLocalSlamCpuRealTimeRatio->Set(
+          common::ToSeconds(sensor_duration.value()) /
+          thread_cpu_duration_seconds);
+    }
+  }
+  last_wall_time_ = wall_time;
+  last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
   return common::make_unique<MatchingResult>(MatchingResult{
       time, *pose_estimate, std::move(filtered_range_data_in_local),
       std::move(insertion_result)});
@@ -364,6 +382,7 @@ void LocalTrajectoryBuilder3D::RegisterMetrics(
       "mapping_3d_local_trajectory_builder_latency",
       "Duration from first incoming point cloud in accumulation to local slam "
       "result");
+  kLocalSlamLatencyMetric = latency->Add({});
 
   auto* voxel_filter_fraction = family_factory->NewGaugeFamily(
       "mapping_3d_local_trajectory_builder_voxel_filter_fraction",
@@ -376,12 +395,20 @@ void LocalTrajectoryBuilder3D::RegisterMetrics(
   kLocalSlamScanMatcherFraction = scan_matcher_fraction->Add({});
 
   auto* insert_into_submap_fraction = family_factory->NewGaugeFamily(
-      "mapping_3d_local_trajectory_builder"
+      "mapping_3d_local_trajectory_builder_insert_into_submap_fraction",
-      "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* real_time_ratio = family_factory->NewGaugeFamily(
+      "mapping_3d_local_trajectory_builder_real_time_ratio",
+      "sensor duration / wall clock duration.");
+  kLocalSlamRealTimeRatio = real_time_ratio->Add({});
+
+  auto* cpu_real_time_ratio = family_factory->NewGaugeFamily(
+      "mapping_3d_local_trajectory_builder_cpu_real_time_ratio",
+      "sensor duration / cpu duration.");
+  kLocalSlamCpuRealTimeRatio = cpu_real_time_ratio->Add({});
+
   auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
   auto* scores = family_factory->NewHistogramFamily(
       "mapping_3d_local_trajectory_builder_scores", "Local scan matcher scores",

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

@@ -107,8 +107,9 @@ class LocalTrajectoryBuilder3D {
 
   int num_accumulated_ = 0;
   sensor::RangeData accumulated_range_data_;
-  common::optional<std::chrono::steady_clock::time_point>
+  common::optional<std::chrono::steady_clock::time_point> last_wall_time_;
-      last_accumulation_stop_;
+
+  common::optional<double> last_thread_cpu_time_seconds_;
 
   RangeDataCollator range_data_collator_;