From 6b447c45779cdf59b5c49e7975ebc892332306c0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Mon, 16 Oct 2017 14:15:53 +0200
Subject: [PATCH] Add FindChecked to MapById. (#586)

* Add FindChecked to MapById.
---
 cartographer/mapping/id.h       | 15 +++++++++++++++
 cartographer/mapping/id_test.cc | 29 +++++++++++++++++++++++------
 2 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h
index ef74930..549d9fd 100644
--- a/cartographer/mapping/id.h
+++ b/cartographer/mapping/id.h
@@ -158,6 +158,16 @@ class MapById {
       }
     }
 
+    explicit ConstIterator(const MapById& map_by_id, const IdType& id)
+        : current_trajectory_(
+              map_by_id.trajectories_.find(id.trajectory_id)),
+          end_trajectory_(map_by_id.trajectories_.end()) {
+      CHECK(current_trajectory_ != end_trajectory_);
+      current_data_ =
+          current_trajectory_->second.data_.find(MapById::GetIndex(id));
+      CHECK(current_data_ != current_trajectory_->second.data_.end());
+    }
+
     IdDataReference operator*() const {
       CHECK(current_trajectory_ != end_trajectory_);
       return IdDataReference{
@@ -262,6 +272,11 @@ class MapById {
     return IdType{trajectory_id, index};
   }
 
+  // Returns an iterator to the element at 'id' which must exist.
+  ConstIterator FindChecked(const IdType& id) {
+    return ConstIterator(*this, id);
+  }
+
   // Inserts data (which must not exist already) into a trajectory.
   void Insert(const IdType& id, const DataType& data) {
     auto& trajectory = trajectories_[id.trajectory_id];
diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc
index 2fedd7d..c4f4ac4 100644
--- a/cartographer/mapping/id_test.cc
+++ b/cartographer/mapping/id_test.cc
@@ -26,8 +26,9 @@ namespace cartographer {
 namespace mapping {
 namespace {
 
-static MapById<NodeId, int> CreateTestMapById() {
-  MapById<NodeId, int> map_by_id;
+template <typename IdType>
+static MapById<IdType, int> CreateTestMapById() {
+  MapById<IdType, int> map_by_id;
   map_by_id.Append(7, 2);
   map_by_id.Append(42, 3);
   map_by_id.Append(0, 0);
@@ -45,7 +46,7 @@ TEST(IdTest, EmptyMapById) {
 }
 
 TEST(IdTest, MapByIdIterator) {
-  MapById<NodeId, int> map_by_id = CreateTestMapById();
+  MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
   EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data);
   EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
               map_by_id.EndOfTrajectory(7));
@@ -65,7 +66,7 @@ TEST(IdTest, MapByIdIterator) {
 }
 
 TEST(IdTest, MapByIdTrajectoryRange) {
-  MapById<NodeId, int> map_by_id = CreateTestMapById();
+  MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
   std::deque<std::pair<NodeId, int>> expected_data = {
       {NodeId{0, 0}, 0},
       {NodeId{0, 1}, 1},
@@ -80,7 +81,7 @@ TEST(IdTest, MapByIdTrajectoryRange) {
 }
 
 TEST(IdTest, MapByIdTrajectoryIdRange) {
-  MapById<NodeId, int> map_by_id = CreateTestMapById();
+  MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
   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);
@@ -91,7 +92,7 @@ TEST(IdTest, MapByIdTrajectoryIdRange) {
 }
 
 TEST(IdTest, MapByIdIterateByTrajectories) {
-  MapById<NodeId, int> map_by_id = CreateTestMapById();
+  MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
   std::deque<std::pair<NodeId, int>> expected_id_data = {
       {NodeId{0, 0}, 0},
       {NodeId{0, 1}, 1},
@@ -126,6 +127,22 @@ TEST(IdTest, InsertIntoMapById) {
   EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42));
 }
 
+TEST(IdTest, FindCheckedNodeId) {
+  MapById<NodeId, int> map_by_id;
+  map_by_id.Append(42, 42);
+  map_by_id.Append(42, 43);
+  map_by_id.Append(42, 44);
+  CHECK_EQ(map_by_id.FindChecked(NodeId{42, 1})->data, 43);
+}
+
+TEST(IdTest, FindCheckedSubmapId) {
+  MapById<SubmapId, int> map_by_id;
+  map_by_id.Append(42, 42);
+  map_by_id.Append(42, 43);
+  map_by_id.Append(42, 44);
+  CHECK_EQ(map_by_id.FindChecked(SubmapId{42, 1})->data, 43);
+}
+
 }  // namespace
 }  // namespace mapping
 }  // namespace cartographer