Introduce proxy object for iteration over trajectory ids. (#585)

* Introduce proxy object for iteration over trajecotry ids.

cartographer/mapping/id.h

@@ -214,6 +214,43 @@ class MapById {
     typename std::map<int, DataType>::const_iterator current_data_;
   };
 
+  class ConstTrajectoryIterator {
+   public:
+    using iterator_category = std::bidirectional_iterator_tag;
+    using value_type = int;
+    using difference_type = int64;
+    using pointer = int*;
+    using reference = const int&;
+
+    explicit ConstTrajectoryIterator(
+        typename std::map<int, MapByIndex>::const_iterator current_trajectory)
+        : current_trajectory_(current_trajectory) {}
+
+    int operator*() const {
+      return current_trajectory_->first;
+    }
+
+    ConstTrajectoryIterator& operator++() {
+      ++current_trajectory_;
+      return *this;
+    }
+
+    ConstTrajectoryIterator& operator--() {
+      --current_trajectory_;
+      return *this;
+    }
+
+    bool operator==(const ConstTrajectoryIterator& it) const {
+      return current_trajectory_ == it.current_trajectory_;
+    }
+
+    bool operator!=(const ConstTrajectoryIterator& it) const { return !operator==(it); }
+
+   private:
+    typename std::map<int, MapByIndex>::const_iterator current_trajectory_;
+  };
+
+
   // Appends data to a 'trajectory_id', creating trajectories as needed.
   IdType Append(const int trajectory_id, const DataType& data) {
     CHECK_GE(trajectory_id, 0);
@@ -281,6 +318,13 @@ class MapById {
                                 EndOfTrajectory(trajectory_id));
   }
 
+  // Returns Range object for range-based loops over the trajectory IDs.
+  Range<ConstTrajectoryIterator> trajectory_ids() const {
+    return Range<ConstTrajectoryIterator>(
+        ConstTrajectoryIterator(trajectories_.begin()),
+        ConstTrajectoryIterator(trajectories_.end()));
+  }
+
   ConstIterator begin() const { return BeginOfTrajectory(0); }
   ConstIterator end() const {
     return BeginOfTrajectory(std::numeric_limits<int>::max());

cartographer/mapping/id_test.cc

@@ -26,6 +26,15 @@ namespace cartographer {
 namespace mapping {
 namespace {
 
+static MapById<NodeId, int> CreateTestMapById() {
+  MapById<NodeId, int> map_by_id;
+  map_by_id.Append(7, 2);
+  map_by_id.Append(42, 3);
+  map_by_id.Append(0, 0);
+  map_by_id.Append(0, 1);
+  return map_by_id;
+}
+
 TEST(IdTest, EmptyMapById) {
   MapById<NodeId, int> map_by_id;
   EXPECT_TRUE(map_by_id.empty());
@@ -36,11 +45,7 @@ TEST(IdTest, EmptyMapById) {
 }
 
 TEST(IdTest, MapByIdIterator) {
-  MapById<NodeId, int> map_by_id;
+  MapById<NodeId, int> map_by_id = CreateTestMapById();
-  map_by_id.Append(7, 2);
-  map_by_id.Append(42, 3);
-  map_by_id.Append(0, 0);
-  map_by_id.Append(0, 1);
   EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data);
   EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
               map_by_id.EndOfTrajectory(7));
@@ -60,12 +65,7 @@ TEST(IdTest, MapByIdIterator) {
 }
 
 TEST(IdTest, MapByIdTrajectoryRange) {
-  MapById<NodeId, int> map_by_id;
+  MapById<NodeId, int> map_by_id = CreateTestMapById();
-  map_by_id.Append(7, 2);
-  map_by_id.Append(42, 3);
-  map_by_id.Append(0, 0);
-  map_by_id.Append(0, 1);
-
   std::deque<std::pair<NodeId, int>> expected_data = {
       {NodeId{0, 0}, 0},
       {NodeId{0, 1}, 1},
@@ -79,6 +79,36 @@ TEST(IdTest, MapByIdTrajectoryRange) {
   EXPECT_TRUE(expected_data.empty());
 }
 
+TEST(IdTest, MapByIdTrajectoryIdRange) {
+  MapById<NodeId, int> map_by_id = CreateTestMapById();
+  std::deque<int> expected_data = {0, 7, 42};
+  for (const int trajectory_id : map_by_id.trajectory_ids()) {
+    EXPECT_EQ(expected_data.front(), trajectory_id);
+    ASSERT_FALSE(expected_data.empty());
+    expected_data.pop_front();
+  }
+  EXPECT_TRUE(expected_data.empty());
+}
+
+TEST(IdTest, MapByIdIterateByTrajectories) {
+  MapById<NodeId, int> map_by_id = CreateTestMapById();
+  std::deque<std::pair<NodeId, int>> expected_id_data = {
+      {NodeId{0, 0}, 0},
+      {NodeId{0, 1}, 1},
+      {NodeId{7, 0}, 2},
+      {NodeId{42, 0}, 3},
+  };
+  for (int trajectory_id : map_by_id.trajectory_ids()) {
+    for (const auto& entry : map_by_id.trajectory(trajectory_id)) {
+      EXPECT_EQ(expected_id_data.front().first, entry.id);
+      EXPECT_EQ(expected_id_data.front().second, entry.data);
+      ASSERT_FALSE(expected_id_data.empty());
+      expected_id_data.pop_front();
+    }
+  }
+  EXPECT_TRUE(expected_id_data.empty());
+}
+
 TEST(IdTest, MapByIdPrevIterator) {
   MapById<NodeId, int> map_by_id;
   map_by_id.Append(42, 42);