From b91ff8fe44daceb64e8b0bc6a702e79b6efdde4a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Fri, 17 Nov 2017 12:27:31 +0100
Subject: [PATCH] Extrapolate poses for ground truth relations. (#687)

---
 .../compute_relations_metrics_main.cc         | 25 ++++++++++++++++---
 1 file changed, 21 insertions(+), 4 deletions(-)

diff --git a/cartographer/ground_truth/compute_relations_metrics_main.cc b/cartographer/ground_truth/compute_relations_metrics_main.cc
index 7b440da..3ad0f1d 100644
--- a/cartographer/ground_truth/compute_relations_metrics_main.cc
+++ b/cartographer/ground_truth/compute_relations_metrics_main.cc
@@ -109,6 +109,21 @@ std::string StatisticsString(const std::vector<Error>& errors) {
          MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n";
 }
 
+transform::Rigid3d LookupTransform(
+    const transform::TransformInterpolationBuffer&
+        transform_interpolation_buffer,
+    const common::Time time) {
+  const common::Time earliest_time =
+      transform_interpolation_buffer.earliest_time();
+  if (transform_interpolation_buffer.Has(time)) {
+    return transform_interpolation_buffer.Lookup(time);
+  } else if (time < earliest_time) {
+    return transform_interpolation_buffer.Lookup(earliest_time);
+  }
+  return transform_interpolation_buffer.Lookup(
+      transform_interpolation_buffer.latest_time());
+}
+
 void Run(const std::string& pose_graph_filename,
          const std::string& relations_filename,
          const bool read_text_file_with_unix_timestamps) {
@@ -136,10 +151,12 @@ void Run(const std::string& pose_graph_filename,
 
   std::vector<Error> errors;
   for (const auto& relation : ground_truth.relation()) {
-    const auto pose1 = transform_interpolation_buffer.Lookup(
-        common::FromUniversal(relation.timestamp1()));
-    const auto pose2 = transform_interpolation_buffer.Lookup(
-        common::FromUniversal(relation.timestamp2()));
+    const auto pose1 =
+        LookupTransform(transform_interpolation_buffer,
+                        common::FromUniversal(relation.timestamp1()));
+    const auto pose2 =
+        LookupTransform(transform_interpolation_buffer,
+                        common::FromUniversal(relation.timestamp2()));
     const transform::Rigid3d expected =
         transform::ToRigid3(relation.expected());
     errors.push_back(ComputeError(pose1, pose2, expected));