From 8186316d777de5c2f779c1556c74ad3e9433967b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= <juraj.orsulic@fer.hr>
Date: Mon, 6 Nov 2017 17:14:01 +0100
Subject: [PATCH] Range loop over trajectories for MapByTime (#633)

---
 cartographer/mapping/id.h         |  2 +-
 cartographer/sensor/map_by_time.h | 45 +++++++++++++++++++++++++++++++
 2 files changed, 46 insertions(+), 1 deletion(-)

diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h
index b7dadaa..359bf96 100644
--- a/cartographer/mapping/id.h
+++ b/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(
diff --git a/cartographer/sensor/map_by_time.h b/cartographer/sensor/map_by_time.h
index 6c0b297..de7fc3c 100644
--- a/cartographer/sensor/map_by_time.h
+++ b/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));