Introduces mapping::MapById in the 2D pose graph for submaps. (#578)

PAIR=cschuet

cartographer/mapping/id.h

@@ -19,11 +19,13 @@
 
 #include <algorithm>
 #include <iterator>
+#include <limits>
 #include <map>
 #include <ostream>
 #include <tuple>
 #include <vector>
 
+#include "cartographer/common/port.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -109,7 +111,8 @@ class NestedVectorsById {
   std::vector<std::vector<ValueType>> data_;
 };
 
-// Like std::map, but indexed by 'IdType' which can be 'NodeId' or 'SubmapId'.
+// Reminiscent of std::map, but indexed by 'IdType' which can be 'NodeId' or
+// 'SubmapId'.
 template <typename IdType, typename DataType>
 class MapById {
  private:
@@ -121,25 +124,24 @@ class MapById {
     const DataType& data;
   };
 
-  class ConstIterator
+  class ConstIterator {
-      : public std::iterator<std::forward_iterator_tag, IdDataReference> {
    public:
-    explicit ConstIterator(const MapById& map_by_id)
+    using iterator_category = std::bidirectional_iterator_tag;
-        : current_trajectory_(map_by_id.trajectories_.begin()),
+    using value_type = IdDataReference;
+    using difference_type = int64;
+    using pointer = const IdDataReference*;
+    using reference = const IdDataReference&;
+
+    explicit ConstIterator(const MapById& map_by_id, const int trajectory_id)
+        : current_trajectory_(
+              map_by_id.trajectories_.lower_bound(trajectory_id)),
           end_trajectory_(map_by_id.trajectories_.end()) {
       if (current_trajectory_ != end_trajectory_) {
         current_data_ = current_trajectory_->second.data_.begin();
-        end_data_ = current_trajectory_->second.data_.end();
         AdvanceToValidDataIterator();
       }
     }
 
-    static ConstIterator EndIterator(const MapById& map_by_id) {
-      auto it = ConstIterator(map_by_id);
-      it.current_trajectory_ = it.end_trajectory_;
-      return it;
-    }
-
     IdDataReference operator*() const {
       CHECK(current_trajectory_ != end_trajectory_);
       return IdDataReference{
@@ -154,6 +156,16 @@ class MapById {
       return *this;
     }
 
+    ConstIterator& operator--() {
+      while (current_trajectory_ == end_trajectory_ ||
+             current_data_ == current_trajectory_->second.data_.begin()) {
+        --current_trajectory_;
+        current_data_ = current_trajectory_->second.data_.end();
+      }
+      --current_data_;
+      return *this;
+    }
+
     bool operator==(const ConstIterator& it) const {
       if (current_trajectory_ == end_trajectory_ ||
           it.current_trajectory_ == it.end_trajectory_) {
@@ -168,20 +180,18 @@ class MapById {
    private:
     void AdvanceToValidDataIterator() {
       CHECK(current_trajectory_ != end_trajectory_);
-      while (current_data_ == end_data_) {
+      while (current_data_ == current_trajectory_->second.data_.end()) {
         ++current_trajectory_;
         if (current_trajectory_ == end_trajectory_) {
           return;
         }
         current_data_ = current_trajectory_->second.data_.begin();
-        end_data_ = current_trajectory_->second.data_.end();
       }
     }
 
     typename std::map<int, MapByIndex>::const_iterator current_trajectory_;
     typename std::map<int, MapByIndex>::const_iterator end_trajectory_;
     typename std::map<int, DataType>::const_iterator current_data_;
-    typename std::map<int, DataType>::const_iterator end_data_;
   };
 
   // Appends data to a 'trajectory_id', creating trajectories as needed.
@@ -217,6 +227,11 @@ class MapById {
     trajectory.data_.erase(it);
   }
 
+  bool Contains(const IdType& id) const {
+    return trajectories_.count(id.trajectory_id) != 0 &&
+           trajectories_.at(id.trajectory_id).data_.count(GetIndex(id)) != 0;
+  }
+
   const DataType& at(const IdType& id) const {
     return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id));
   }
@@ -225,8 +240,25 @@ class MapById {
     return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id));
   }
 
-  ConstIterator begin() const { return ConstIterator(*this); }
+  // Support querying by trajectory.
-  ConstIterator end() const { return ConstIterator::EndIterator(*this); }
+  ConstIterator BeginOfTrajectory(const int trajectory_id) const {
+    return ConstIterator(*this, trajectory_id);
+  }
+  ConstIterator EndOfTrajectory(const int trajectory_id) const {
+    return BeginOfTrajectory(trajectory_id + 1);
+  }
+
+  // Returns 0 if 'trajectory_id' does not exist.
+  size_t SizeOfTrajectoryOrZero(const int trajectory_id) const {
+    return trajectories_.count(trajectory_id)
+               ? trajectories_.at(trajectory_id).data_.size()
+               : 0;
+  }
+
+  ConstIterator begin() const { return BeginOfTrajectory(0); }
+  ConstIterator end() const {
+    return BeginOfTrajectory(std::numeric_limits<int>::max());
+  }
 
   bool empty() const { return begin() == end(); }
 

cartographer/mapping/id_test.cc

@@ -17,6 +17,7 @@
 #include "cartographer/mapping/id.h"
 
 #include <deque>
+#include <iterator>
 #include <utility>
 
 #include "gtest/gtest.h"
@@ -40,6 +41,9 @@ TEST(IdTest, MapByIdIterator) {
   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));
   std::deque<std::pair<NodeId, int>> expected_id_data = {
       {NodeId{0, 0}, 0},
       {NodeId{0, 1}, 1},
@@ -55,6 +59,23 @@ TEST(IdTest, MapByIdIterator) {
   EXPECT_TRUE(expected_id_data.empty());
 }
 
+TEST(IdTest, MapByIdPrevIterator) {
+  MapById<NodeId, int> map_by_id;
+  map_by_id.Append(42, 42);
+  auto it = map_by_id.end();
+  ASSERT_TRUE(it != map_by_id.begin());
+  std::advance(it, -1);
+  EXPECT_TRUE(it == map_by_id.begin());
+}
+
+TEST(IdTest, InsertIntoMapById) {
+  MapById<NodeId, int> map_by_id;
+  EXPECT_EQ(0, map_by_id.SizeOfTrajectoryOrZero(42));
+  map_by_id.Append(42, 42);
+  map_by_id.Insert(NodeId{42, 5}, 42);
+  EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42));
+}
+
 }  // namespace
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -22,6 +22,7 @@
 #include <functional>
 #include <iomanip>
 #include <iostream>
+#include <iterator>
 #include <limits>
 #include <memory>
 #include <sstream>
@@ -55,30 +56,27 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
     const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   CHECK(!insertion_submaps.empty());
-  auto submap_data = optimization_problem_.submap_data();
+  const auto& submap_data = optimization_problem_.submap_data();
   if (insertion_submaps.size() == 1) {
     // If we don't already have an entry for the first submap, add one.
-    if (static_cast<size_t>(trajectory_id) >= submap_data.size() ||
+    if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
-        submap_data[trajectory_id].empty()) {
       optimization_problem_.AddSubmap(
           trajectory_id,
           sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
-      submap_data = optimization_problem_.submap_data();
     }
-    CHECK_EQ(submap_data[trajectory_id].size(), 1);
+    CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
     const mapping::SubmapId submap_id{trajectory_id, 0};
     CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
     return {submap_id};
   }
   CHECK_EQ(2, insertion_submaps.size());
-  CHECK(!submap_data.at(trajectory_id).empty());
+  const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
-  const mapping::SubmapId last_submap_id{
+  CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
-      trajectory_id, submap_data.at(trajectory_id).rbegin()->first};
+  const mapping::SubmapId last_submap_id = (*std::prev(end_it)).id;
   if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
     // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
     // and 'insertions_submaps.back()' is new.
-    const auto& first_submap_pose =
+    const auto& first_submap_pose = submap_data.at(last_submap_id).pose;
-        submap_data.at(trajectory_id).at(last_submap_id.submap_index).pose;
     optimization_problem_.AddSubmap(
         trajectory_id,
         first_submap_pose *
@@ -189,10 +187,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
     // submap's trajectory, it suffices to do a match constrained to a local
     // search window.
     const transform::Rigid2d initial_relative_pose =
-        optimization_problem_.submap_data()
+        optimization_problem_.submap_data().at(submap_id).pose.inverse() *
-            .at(submap_id.trajectory_id)
-            .at(submap_id.submap_index)
-            .pose.inverse() *
         optimization_problem_.node_data()
             .at(node_id.trajectory_id)
             .at(node_id.node_index)
@@ -250,10 +245,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
       constant_data->initial_pose *
       transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
   const transform::Rigid2d optimized_pose =
-      optimization_problem_.submap_data()
+      optimization_problem_.submap_data().at(matching_id).pose *
-          .at(matching_id.trajectory_id)
-          .at(matching_id.submap_index)
-          .pose *
       sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
           .inverse() *
       pose;
@@ -428,14 +420,9 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
       submap_data_.Append(trajectory_id, SubmapData());
   submap_data_.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the optimized pose.
-  CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()),
+  CHECK_EQ(submap_id,
-           optimized_submap_transforms_.size());
+           optimized_submap_transforms_.Append(
-  optimized_submap_transforms_.resize(submap_data_.num_trajectories());
+               trajectory_id, sparse_pose_graph::SubmapData{initial_pose_2d}));
-  CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(),
-           submap_id.submap_index);
-  optimized_submap_transforms_.at(trajectory_id)
-      .emplace(submap_id.submap_index,
-               sparse_pose_graph::SubmapData{initial_pose_2d});
   AddWorkItem([this, submap_id, initial_pose_2d]() REQUIRES(mutex_) {
     CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
     submap_data_.at(submap_id).state = SubmapState::kFinished;
@@ -500,7 +487,7 @@ void SparsePoseGraph::RunOptimization() {
   optimization_problem_.Solve(constraints_, frozen_trajectories_);
   common::MutexLocker locker(&mutex_);
 
-  const auto submap_data = optimization_problem_.submap_data();
+  const auto& submap_data = optimization_problem_.submap_data();
   const auto& node_data = optimization_problem_.node_data();
   for (int trajectory_id = 0;
        trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
@@ -601,19 +588,18 @@ SparsePoseGraph::GetAllSubmapData() {
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
-    const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
+    const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>&
         submap_transforms,
     const int trajectory_id) const {
-  if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
+  auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
-      submap_transforms.at(trajectory_id).empty()) {
+  auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
+  if (begin_it == end_it) {
     return transform::Rigid3d::Identity();
   }
-
+  const mapping::SubmapId last_optimized_submap_id = (*std::prev(end_it)).id;
-  const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first;
-  const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index};
   // Accessing 'local_pose' in Submap is okay, since the member is const.
   return transform::Embed3D(
-             submap_transforms.at(trajectory_id).at(submap_index).pose) *
+             submap_transforms.at(last_optimized_submap_id).pose) *
          submap_data_.at(last_optimized_submap_id)
              .submap->local_pose()
              .inverse();
@@ -625,16 +611,10 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
     return {};
   }
   auto submap = submap_data_.at(submap_id).submap;
-  if (submap_id.trajectory_id <
+  if (optimized_submap_transforms_.Contains(submap_id)) {
-          static_cast<int>(optimized_submap_transforms_.size()) &&
-      submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
-                                                    .at(submap_id.trajectory_id)
-                                                    .size())) {
     // We already have an optimized pose.
     return {submap, transform::Embed3D(
-                        optimized_submap_transforms_.at(submap_id.trajectory_id)
+                        optimized_submap_transforms_.at(submap_id).pose)};
-                            .at(submap_id.submap_index)
-                            .pose)};
   }
   // We have to extrapolate.
   return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
@@ -647,7 +627,8 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
 
 int SparsePoseGraph::TrimmingHandle::num_submaps(
     const int trajectory_id) const {
-  return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
+  const auto& submap_data = parent_->optimization_problem_.submap_data();
+  return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
 }
 
 void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(

cartographer/mapping_2d/sparse_pose_graph.h

@@ -161,7 +161,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Computes the local to global frame transform based on the given optimized
   // 'submap_transforms'.
   transform::Rigid3d ComputeLocalToGlobalTransform(
-      const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
+      const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>&
           submap_transforms,
       int trajectory_id) const REQUIRES(mutex_);
 
@@ -214,7 +214,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Current submap transforms used for displaying data.
-  std::vector<std::map<int, sparse_pose_graph::SubmapData>>
+  mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>
       optimized_submap_transforms_ GUARDED_BY(mutex_);
 
   // List of all trimmers to consult when optimizations finish.

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -255,15 +255,9 @@ const std::vector<std::map<int, NodeData>>& OptimizationProblem::node_data()
   return node_data_;
 }
 
-std::vector<std::map<int, SubmapData>> OptimizationProblem::submap_data()
+const mapping::MapById<mapping::SubmapId, SubmapData>&
-    const {
+OptimizationProblem::submap_data() const {
-  std::vector<std::map<int, SubmapData>> result;
+  return submap_data_;
-  for (const auto& submap_id_data : submap_data_) {
-    result.resize(submap_id_data.id.trajectory_id + 1);
-    result[submap_id_data.id.trajectory_id].emplace(
-        submap_id_data.id.submap_index, submap_id_data.data);
-  }
-  return result;
 }
 
 }  // namespace sparse_pose_graph

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -80,7 +80,7 @@ class OptimizationProblem {
              const std::set<int>& frozen_trajectories);
 
   const std::vector<std::map<int, NodeData>>& node_data() const;
-  std::vector<std::map<int, SubmapData>> submap_data() const;
+  const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
 
  private:
   struct TrajectoryData {

cartographer/sensor/compressed_point_cloud.h

@@ -58,9 +58,14 @@ class CompressedPointCloud {
 };
 
 // Forward iterator for compressed point clouds.
-class CompressedPointCloud::ConstIterator
+class CompressedPointCloud::ConstIterator {
-    : public std::iterator<std::forward_iterator_tag, Eigen::Vector3f> {
  public:
+  using iterator_category = std::forward_iterator_tag;
+  using value_type = Eigen::Vector3f;
+  using difference_type = int64;
+  using pointer = const Eigen::Vector3f*;
+  using reference = const Eigen::Vector3f&;
+
   // Creates begin iterator.
   explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud);