From 120c216c4797947fcb70aaa3cf2b1a9fb67a2dab Mon Sep 17 00:00:00 2001
From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com>
Date: Tue, 10 Jul 2018 16:24:57 +0200
Subject: [PATCH] Measure accumulation_duration from last accumulation stop
 (#1251)

[PR 946](https://github.com/googlecartographer/cartographer/pull/946) introduced metrics in local trajectory builder. The accumulation duration was measured from accumulation_start to accumulation_stop.

This PR changes this to measure the entire time elapsed between two accumulations (i.e. from accumulation_stop to the next accumulation_stop).

Mostly the two measurements result in roughly the same, with the new way measuring slightly larger durations, as expected. But occasionally the measurements differ significantly. This is probably due to a lock contention somewhere outside of what was measured previously. Since we are interested in real time metrics, we need to track the entire time passed.
---
 .../internal/2d/local_trajectory_builder_2d.cc     | 14 +++++++-------
 .../internal/2d/local_trajectory_builder_2d.h      |  3 ++-
 .../internal/3d/local_trajectory_builder_3d.cc     | 14 +++++++-------
 .../internal/3d/local_trajectory_builder_3d.h      |  3 ++-
 4 files changed, 18 insertions(+), 16 deletions(-)

diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
index 52b9d56..228ec53 100644
--- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
+++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
@@ -134,10 +134,6 @@ 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(synchronized_data.ranges.size());
   bool warned = false;
@@ -241,9 +237,13 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
   std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
       time, range_data_in_local, filtered_gravity_aligned_point_cloud,
       pose_estimate, gravity_alignment.rotation());
-  const auto accumulation_duration =
-      std::chrono::steady_clock::now() - accumulation_started_;
-  kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
+  const auto accumulation_stop = std::chrono::steady_clock::now();
+  if (last_accumulation_stop_.has_value()) {
+    const auto accumulation_duration =
+        accumulation_stop - last_accumulation_stop_.value();
+    kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
+  }
+  last_accumulation_stop_ = accumulation_stop;
   return common::make_unique<MatchingResult>(
       MatchingResult{time, pose_estimate, std::move(range_data_in_local),
                      std::move(insertion_result)});
diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h
index 068a4a2..060cafd 100644
--- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h
+++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h
@@ -110,7 +110,8 @@ class LocalTrajectoryBuilder2D {
 
   int num_accumulated_ = 0;
   sensor::RangeData accumulated_range_data_;
-  std::chrono::steady_clock::time_point accumulation_started_;
+  common::optional<std::chrono::steady_clock::time_point>
+      last_accumulation_stop_;
 
   RangeDataCollator range_data_collator_;
 };
diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
index 7050bf3..28452f9 100644
--- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
+++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
@@ -140,10 +140,6 @@ LocalTrajectoryBuilder3D::AddRangeData(
     return nullptr;
   }
 
-  if (num_accumulated_ == 0) {
-    accumulation_started_ = std::chrono::steady_clock::now();
-  }
-
   std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
       sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
           .Filter(synchronized_data.ranges);
@@ -302,9 +298,13 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
         common::ToSeconds(sensor_duration.value());
     kLocalSlamInsertIntoSubmapFraction->Set(insert_into_submap_fraction);
   }
-  const auto accumulation_duration =
-      std::chrono::steady_clock::now() - accumulation_started_;
-  kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
+  const auto accumulation_stop = std::chrono::steady_clock::now();
+  if (last_accumulation_stop_.has_value()) {
+    const auto accumulation_duration =
+        accumulation_stop - last_accumulation_stop_.value();
+    kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
+  }
+  last_accumulation_stop_ = accumulation_stop;
   return common::make_unique<MatchingResult>(MatchingResult{
       time, *pose_estimate, std::move(filtered_range_data_in_local),
       std::move(insertion_result)});
diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h
index d98ea57..e9b7a57 100644
--- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h
+++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h
@@ -107,7 +107,8 @@ class LocalTrajectoryBuilder3D {
 
   int num_accumulated_ = 0;
   sensor::RangeData accumulated_range_data_;
-  std::chrono::steady_clock::time_point accumulation_started_;
+  common::optional<std::chrono::steady_clock::time_point>
+      last_accumulation_stop_;
 
   RangeDataCollator range_data_collator_;