From e735203a05ba9794345bb9875bcd63d898e15174 Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Thu, 1 Feb 2018 11:58:13 +0100
Subject: [PATCH] Extend MapById::lower_bound() to support structs with 'time'
 field. (#871)

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
---
 cartographer/mapping/id.h       | 20 +++++++++++++++++--
 cartographer/mapping/id_test.cc | 35 +++++++++++++++++++++++++++++++++
 2 files changed, 53 insertions(+), 2 deletions(-)

diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h
index 90cca9e..3fc49cb 100644
--- a/cartographer/mapping/id.h
+++ b/cartographer/mapping/id.h
@@ -35,6 +35,22 @@
 
 namespace cartographer {
 namespace mapping {
+namespace internal {
+
+template <class T>
+auto GetTimeImpl(const T& t, int) -> decltype(t.time()) {
+  return t.time();
+}
+template <class T>
+auto GetTimeImpl(const T& t, unsigned) -> decltype(t.time) {
+  return t.time;
+}
+template <class T>
+common::Time GetTime(const T& t) {
+  return GetTimeImpl(t, 0);
+}
+
+}  // namespace internal
 
 // Uniquely identifies a trajectory node using a combination of a unique
 // trajectory ID and a zero-based index of the node inside that trajectory.
@@ -347,7 +363,7 @@ class MapById {
 
     const std::map<int, DataType>& trajectory =
         trajectories_.at(trajectory_id).data_;
-    if (std::prev(trajectory.end())->second.time() < time) {
+    if (internal::GetTime(std::prev(trajectory.end())->second) < time) {
       return EndOfTrajectory(trajectory_id);
     }
     auto left = trajectory.begin();
@@ -355,7 +371,7 @@ class MapById {
     while (left != right) {
       const int middle = left->first + (right->first - left->first) / 2;
       const auto lower_bound_middle = trajectory.lower_bound(middle);
-      if (lower_bound_middle->second.time() < time) {
+      if (internal::GetTime(lower_bound_middle->second) < time) {
         left = std::next(lower_bound_middle);
       } else {
         right = lower_bound_middle;
diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc
index 84ab862..1a7b8b7 100644
--- a/cartographer/mapping/id_test.cc
+++ b/cartographer/mapping/id_test.cc
@@ -221,6 +221,41 @@ TEST(IdTest, LowerBoundFuzz) {
   }
 }
 
+struct DataStruct {
+  const common::Time time;
+};
+
+TEST(IdTest, LowerBoundFuzzWithStruct) {
+  constexpr int kMaxTimeIncrement = 20;
+  constexpr int kMaxNumNodes = 20;
+  constexpr int kNumTests = 100;
+  constexpr int kTrajectoryId = 1;
+
+  std::mt19937 rng;
+  std::uniform_int_distribution<int> dt_dist(1, kMaxTimeIncrement);
+  std::uniform_int_distribution<int> N_dist(1, kMaxNumNodes);
+
+  for (int i = 0; i < kNumTests; ++i) {
+    const int N = N_dist(rng);
+    int t = 0;
+    MapById<SubmapId, DataStruct> map_by_id;
+    for (int j = 0; j < N; ++j) {
+      t = t + dt_dist(rng);
+      map_by_id.Append(kTrajectoryId, DataStruct{CreateTime(t)});
+    }
+    std::uniform_int_distribution<int> t0_dist(1, N * kMaxTimeIncrement + 1);
+    int t0 = t0_dist(rng);
+    auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(t0));
+
+    auto ground_truth = std::lower_bound(
+        map_by_id.BeginOfTrajectory(kTrajectoryId),
+        map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(t0),
+        [](MapById<SubmapId, DataStruct>::IdDataReference a,
+           const common::Time& t) { return a.data.time < t; });
+
+    CHECK(ground_truth == it);
+  }
+}
 }  // namespace
 }  // namespace mapping
 }  // namespace cartographer