Fix MapById::lower_bound function and implement test to reproduce the bug (#1616)

This fixes the bug that has been faced during localization with an initial pose and a trimmed map (OverlappingSubmapsTrimmer2D has been used). A small test is added that reproduces the problem (an infinite loop) where a trimmed trajectory (MapById instance) is used.

cartographer/mapping/id.h

@@ -378,14 +378,23 @@ class MapById {
 
     const std::map<int, DataType>& trajectory =
         trajectories_.at(trajectory_id).data_;
+
     if (internal::GetTime(std::prev(trajectory.end())->second) < time) {
       return EndOfTrajectory(trajectory_id);
     }
+
     auto left = trajectory.begin();
     auto right = std::prev(trajectory.end());
     while (left != right) {
+      // This is never 'right' which is important to guarantee progress.
       const int middle = left->first + (right->first - left->first) / 2;
-      const auto lower_bound_middle = trajectory.lower_bound(middle);
+      // This could be 'right' in the presence of gaps, so we need to use the
+      // previous element in this case.
+      auto lower_bound_middle = trajectory.lower_bound(middle);
+      if (lower_bound_middle->first > middle) {
+        CHECK(lower_bound_middle != left);
+        lower_bound_middle = std::prev(lower_bound_middle);
+      }
       if (internal::GetTime(lower_bound_middle->second) < time) {
         left = std::next(lower_bound_middle);
       } else {

cartographer/mapping/id_test.cc

@@ -254,6 +254,53 @@ TEST(IdTest, LowerBoundFuzz) {
   }
 }
 
+TEST(IdTest, LowerBoundTrimmedTrajectory) {
+  constexpr int kTrajectoryId = 1;
+
+  std::mt19937 rng;
+  std::uniform_int_distribution<int> dt_dist(1, 20);
+
+  const int N = 500;
+  int t = 0;
+  MapById<SubmapId, Data> map_by_id;
+  for (int j = 0; j < N; ++j) {
+    t = t + dt_dist(rng);
+    map_by_id.Append(kTrajectoryId, Data(t));
+  }
+
+  // Choose random length of a trim segment.
+  std::uniform_int_distribution<int> dt_trim_segment_length(
+      1, static_cast<int>(N / 2));
+  size_t trim_segment_length = dt_trim_segment_length(rng);
+  // Choose random start for a trim_segment.
+  std::uniform_int_distribution<int> dt_trim_segment_start(
+      2, N - trim_segment_length - 1);
+  size_t trim_segment_start_index = dt_trim_segment_start(rng);
+
+  auto trim_segment_start = map_by_id.begin();
+  std::advance(trim_segment_start, trim_segment_start_index);
+
+  auto trim_segment_end = map_by_id.begin();
+  std::advance(trim_segment_end,
+               trim_segment_start_index + trim_segment_length);
+
+  for (auto it = trim_segment_start; it != trim_segment_end;) {
+    const auto this_it = it;
+    ++it;
+    map_by_id.Trim(this_it->id);
+  }
+
+  auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(0));
+
+  auto ground_truth =
+      std::lower_bound(map_by_id.BeginOfTrajectory(kTrajectoryId),
+                       map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(0),
+                       [](MapById<SubmapId, Data>::IdDataReference a,
+                          const common::Time& t) { return a.data.time() < t; });
+
+  EXPECT_EQ(ground_truth, it);
+}
+
 struct DataStruct {
   const common::Time time;
 };