Range loop over trajectories for MapByTime (#633)

cartographer/mapping/id.h

@@ -201,7 +201,7 @@ class MapById {
     using iterator_category = std::bidirectional_iterator_tag;
     using value_type = int;
     using difference_type = int64;
-    using pointer = int*;
+    using pointer = const int*;
     using reference = const int&;
 
     explicit ConstTrajectoryIterator(

cartographer/sensor/map_by_time.h

@@ -133,6 +133,44 @@ class MapByTime {
     typename std::map<common::Time, DataType>::const_iterator iterator_;
   };
 
+  class ConstTrajectoryIterator {
+   public:
+    using iterator_category = std::bidirectional_iterator_tag;
+    using value_type = int;
+    using difference_type = int64;
+    using pointer = const int*;
+    using reference = const int&;
+
+    explicit ConstTrajectoryIterator(
+        typename std::map<int, std::map<common::Time, DataType>>::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, std::map<common::Time, DataType>>::const_iterator
+        current_trajectory_;
+  };
+
   ConstIterator BeginOfTrajectory(const int trajectory_id) const {
     return ConstIterator(data_.at(trajectory_id).begin());
   }
@@ -141,6 +179,13 @@ class MapByTime {
     return ConstIterator(data_.at(trajectory_id).end());
   }
 
+  // Returns Range object for range-based loops over the trajectory IDs.
+  mapping::Range<ConstTrajectoryIterator> trajectory_ids() const {
+    return mapping::Range<ConstTrajectoryIterator>(
+        ConstTrajectoryIterator(data_.begin()),
+        ConstTrajectoryIterator(data_.end()));
+  }
+
   mapping::Range<ConstIterator> trajectory(const int trajectory_id) const {
     return mapping::Range<ConstIterator>(BeginOfTrajectory(trajectory_id),
                                          EndOfTrajectory(trajectory_id));