Introduce proxy object for range based loops over nodes of trajectory. (#583)

cartographer/mapping/id.h

@@ -113,6 +113,20 @@ class NestedVectorsById {
   std::vector<std::vector<ValueType>> data_;
 };
 
+template <typename IteratorType>
+class Range {
+ public:
+  Range(const IteratorType& begin, const IteratorType& end)
+      : begin_(begin), end_(end) {}
+
+  IteratorType begin() const { return begin_; }
+  IteratorType end() const { return end_; }
+
+ private:
+  IteratorType begin_;
+  IteratorType end_;
+};
+
 // Reminiscent of std::map, but indexed by 'IdType' which can be 'NodeId' or
 // 'SubmapId'.
 template <typename IdType, typename DataType>
@@ -261,6 +275,12 @@ class MapById {
                : 0;
   }
 
+  // Returns Range object for range-based loops over the nodes of a trajectory.
+  Range<ConstIterator> trajectory(const int trajectory_id) const {
+    return Range<ConstIterator>(BeginOfTrajectory(trajectory_id),
+                                EndOfTrajectory(trajectory_id));
+  }
+
   ConstIterator begin() const { return BeginOfTrajectory(0); }
   ConstIterator end() const {
     return BeginOfTrajectory(std::numeric_limits<int>::max());

cartographer/mapping/id_test.cc

@@ -59,6 +59,26 @@ TEST(IdTest, MapByIdIterator) {
   EXPECT_TRUE(expected_id_data.empty());
 }
 
+TEST(IdTest, MapByIdTrajectoryRange) {
+  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);
+
+  std::deque<std::pair<NodeId, int>> expected_data = {
+      {NodeId{0, 0}, 0},
+      {NodeId{0, 1}, 1},
+  };
+  for (const auto& entry : map_by_id.trajectory(0)) {
+    EXPECT_EQ(expected_data.front().first, entry.id);
+    EXPECT_EQ(expected_data.front().second, entry.data);
+    ASSERT_FALSE(expected_data.empty());
+    expected_data.pop_front();
+  }
+  EXPECT_TRUE(expected_data.empty());
+}
+
 TEST(IdTest, MapByIdPrevIterator) {
   MapById<NodeId, int> map_by_id;
   map_by_id.Append(42, 42);