Change submap_data_ in pose graph to MapById. (#593)

* Change submap_data_ in pose graph to MapById.

cartographer/mapping/id.h

@@ -79,40 +79,6 @@ inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
   return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
 }
 
-template <typename ValueType, typename IdType>
-class NestedVectorsById {
- public:
-  // Appends data to a trajectory, creating trajectories as needed.
-  IdType Append(int trajectory_id, const ValueType& value) {
-    data_.resize(std::max<size_t>(data_.size(), trajectory_id + 1));
-    const IdType id{trajectory_id,
-                    static_cast<int>(data_[trajectory_id].size())};
-    data_[trajectory_id].push_back(value);
-    return id;
-  }
-
-  const ValueType& at(const IdType& id) const {
-    return data_.at(id.trajectory_id).at(GetIndex(id));
-  }
-  ValueType& at(const IdType& id) {
-    return data_.at(id.trajectory_id).at(GetIndex(id));
-  }
-
-  int num_trajectories() const { return static_cast<int>(data_.size()); }
-  int num_indices(int trajectory_id) const {
-    return static_cast<int>(data_.at(trajectory_id).size());
-  }
-
-  // TODO(whess): Remove once no longer needed.
-  const std::vector<std::vector<ValueType>> data() const { return data_; }
-
- private:
-  static int GetIndex(const NodeId& id) { return id.node_index; }
-  static int GetIndex(const SubmapId& id) { return id.submap_index; }
-
-  std::vector<std::vector<ValueType>> data_;
-};
-
 template <typename IteratorType>
 class Range {
  public:
@@ -161,13 +127,15 @@ class MapById {
     }
 
     explicit ConstIterator(const MapById& map_by_id, const IdType& id)
-        : current_trajectory_(
+        : current_trajectory_(map_by_id.trajectories_.find(id.trajectory_id)),
-              map_by_id.trajectories_.find(id.trajectory_id)),
           end_trajectory_(map_by_id.trajectories_.end()) {
-      CHECK(current_trajectory_ != end_trajectory_);
+      if (current_trajectory_ != end_trajectory_) {
-      current_data_ =
+        current_data_ =
-          current_trajectory_->second.data_.find(MapById::GetIndex(id));
+            current_trajectory_->second.data_.find(MapById::GetIndex(id));
-      CHECK(current_data_ != current_trajectory_->second.data_.end());
+        if (current_data_ == current_trajectory_->second.data_.end()) {
+          current_trajectory_ = end_trajectory_;
+        }
+      }
     }
 
     IdDataReference operator*() const {
@@ -273,10 +241,9 @@ class MapById {
     return IdType{trajectory_id, index};
   }
 
-  // Returns an iterator to the element at 'id' which must exist.
+  // Returns an iterator to the element at 'id' or the end iterator if it does
-  ConstIterator FindChecked(const IdType& id) {
+  // not exist.
-    return ConstIterator(*this, id);
+  ConstIterator find(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) {

cartographer/mapping/id_test.cc

@@ -127,20 +127,22 @@ TEST(IdTest, InsertIntoMapById) {
   EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42));
 }
 
-TEST(IdTest, FindCheckedNodeId) {
+TEST(IdTest, FindNodeId) {
   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);
+  CHECK_EQ(map_by_id.find(NodeId{42, 1})->data, 43);
+  EXPECT_TRUE(map_by_id.find(NodeId{42, 3}) == map_by_id.end());
 }
 
-TEST(IdTest, FindCheckedSubmapId) {
+TEST(IdTest, FindSubmapId) {
   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);
+  CHECK_EQ(map_by_id.find(SubmapId{42, 1})->data, 43);
+  EXPECT_TRUE(map_by_id.find(SubmapId{42, 3}) == map_by_id.end());
 }
 
 }  // namespace

cartographer/mapping/map_builder.cc

@@ -125,20 +125,11 @@ string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
            std::to_string(num_trajectory_builders()) + " trajectories.";
   }
 
-  const int num_submaps =
-      sparse_pose_graph_->num_submaps(submap_id.trajectory_id);
-  if (submap_id.submap_index < 0 || submap_id.submap_index >= num_submaps) {
-    return "Requested submap " + std::to_string(submap_id.submap_index) +
-           " from trajectory " + std::to_string(submap_id.trajectory_id) +
-           " but there are only " + std::to_string(num_submaps) +
-           " submaps in this trajectory.";
-  }
-
   const auto submap_data = sparse_pose_graph_->GetSubmapData(submap_id);
   if (submap_data.submap == nullptr) {
     return "Requested submap " + std::to_string(submap_id.submap_index) +
            " from trajectory " + std::to_string(submap_id.trajectory_id) +
-           " but it has been trimmed.";
+           " but it does not exist: maybe it has been trimmed.";
   }
   submap_data.submap->ToResponseProto(submap_data.pose, response);
   return "";

cartographer/mapping/sparse_pose_graph.h

@@ -99,11 +99,9 @@ class SparsePoseGraph {
   // Gets the current trajectory clusters.
   virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
 
-  // Return the number of submaps for the given 'trajectory_id'.
-  virtual int num_submaps(int trajectory_id) = 0;
-
   // Returns the current optimized transform and submap itself for the given
-  // 'submap_id'.
+  // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does
+  // not exist (anymore).
   virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
 
   // Returns data for all Submaps by trajectory.

cartographer/mapping/trajectory_node.cc

@@ -44,8 +44,7 @@ proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) {
     proto.add_rotational_scan_matcher_histogram(
         constant_data.rotational_scan_matcher_histogram(i));
   }
-  *proto.mutable_local_pose() =
+  *proto.mutable_local_pose() = transform::ToProto(constant_data.local_pose);
-      transform::ToProto(constant_data.local_pose);
   return proto;
 }
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -107,12 +107,9 @@ void SparsePoseGraph::AddScan(
   ++num_trajectory_nodes_;
 
   // Test if the 'insertion_submap.back()' is one we never saw before.
-  if (trajectory_id >= submap_data_.num_trajectories() ||
+  if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
-      submap_data_.num_indices(trajectory_id) == 0 ||
+      std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
-      submap_data_
+          insertion_submaps.back()) {
-              .at(mapping::SubmapId{
-                  trajectory_id, submap_data_.num_indices(trajectory_id) - 1})
-              .submap != insertion_submaps.back()) {
     // We grow 'submap_data_' as needed. This code assumes that the first
     // time we see a new submap is as 'insertion_submaps.back()'.
     const mapping::SubmapId submap_id =
@@ -249,16 +246,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                                       Constraint::INTRA_SUBMAP});
   }
 
-  for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
+  for (const auto& submap_id_data : submap_data_) {
-       ++trajectory_id) {
+    if (submap_id_data.data.state == SubmapState::kFinished) {
-    for (int submap_index = 0;
+      CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
-         submap_index < submap_data_.num_indices(trajectory_id);
+      ComputeConstraint(node_id, submap_id_data.id);
-         ++submap_index) {
-      const mapping::SubmapId submap_id{trajectory_id, submap_index};
-      if (submap_data_.at(submap_id).state == SubmapState::kFinished) {
-        CHECK_EQ(submap_data_.at(submap_id).node_ids.count(node_id), 0);
-        ComputeConstraint(node_id, submap_id);
-      }
     }
   }
 
@@ -489,8 +480,7 @@ void SparsePoseGraph::RunOptimization() {
 
     const mapping::NodeId last_optimized_node_id =
         std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
-    auto node_it =
+    auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
-        std::next(trajectory_nodes_.FindChecked(last_optimized_node_id));
     for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
          ++node_it) {
       auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
@@ -544,14 +534,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
   return trajectory_connectivity_state_.Components();
 }
 
-int SparsePoseGraph::num_submaps(const int trajectory_id) {
-  common::MutexLocker locker(&mutex_);
-  if (trajectory_id >= submap_data_.num_trajectories()) {
-    return 0;
-  }
-  return submap_data_.num_indices(trajectory_id);
-}
-
 mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
     const mapping::SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
@@ -561,20 +543,15 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
 std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
 SparsePoseGraph::GetAllSubmapData() {
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>> result;
-      all_submap_data(submap_data_.num_trajectories());
+  for (const int trajectory_id : submap_data_.trajectory_ids()) {
-  for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
+    result.resize(trajectory_id + 1);
-       ++trajectory_id) {
+    for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) {
-    all_submap_data[trajectory_id].reserve(
+      result[trajectory_id].resize(submap_id_data.id.submap_index + 1);
-        submap_data_.num_indices(trajectory_id));
+      result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id);
-    for (int submap_index = 0;
-         submap_index < submap_data_.num_indices(trajectory_id);
-         ++submap_index) {
-      all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock(
-          mapping::SubmapId{trajectory_id, submap_index}));
     }
   }
-  return all_submap_data;
+  return result;
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
@@ -597,10 +574,11 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
 
 mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
     const mapping::SubmapId& submap_id) {
-  if (submap_data_.at(submap_id).state == SubmapState::kTrimmed) {
+  const auto it = submap_data_.find(submap_id);
+  if (it == submap_data_.end()) {
     return {};
   }
-  auto submap = submap_data_.at(submap_id).submap;
+  auto submap = it->data.submap;
   if (optimized_submap_transforms_.Contains(submap_id)) {
     // We already have an optimized pose.
     return {submap, transform::Embed3D(
@@ -666,11 +644,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
   }
 
   // Mark the submap with 'submap_id' as trimmed and remove its data.
-  auto& submap_data = parent_->submap_data_.at(submap_id);
+  CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
-  CHECK(submap_data.state == SubmapState::kFinished);
+  parent_->submap_data_.Trim(submap_id);
-  submap_data.state = SubmapState::kTrimmed;
-  CHECK(submap_data.submap != nullptr);
-  submap_data.submap.reset();
   parent_->constraint_builder_.DeleteScanMatcher(submap_id);
   parent_->optimization_problem_.TrimSubmap(submap_id);
 

cartographer/mapping_2d/sparse_pose_graph.h

@@ -92,7 +92,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
-  int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
   mapping::SparsePoseGraph::SubmapData GetSubmapData(
       const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
@@ -107,7 +106,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // The current state of the submap in the background threads. When this
   // transitions to kFinished, all scans are tried to match against this submap.
   // Likewise, all new scans are matched against submaps which are finished.
-  enum class SubmapState { kActive, kFinished, kTrimmed };
+  enum class SubmapState { kActive, kFinished };
   struct SubmapData {
     std::shared_ptr<const Submap> submap;
 
@@ -205,7 +204,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Submaps get assigned an ID and state as soon as they are seen, even
   // before they take part in the background computations.
-  mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
+  mapping::MapById<mapping::SubmapId, SubmapData> submap_data_
       GUARDED_BY(mutex_);
 
   // Data that are currently being shown.

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -132,8 +132,9 @@ std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
     histograms_at_angles.emplace_back(
         node.constant_data->rotational_scan_matcher_histogram,
         transform::GetYaw(
-            node.global_pose * transform::Rigid3d::Rotation(
+            node.global_pose *
-                            node.constant_data->gravity_alignment.inverse())));
+            transform::Rigid3d::Rotation(
+                node.constant_data->gravity_alignment.inverse())));
   }
   return histograms_at_angles;
 }

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -104,12 +104,9 @@ void SparsePoseGraph::AddScan(
   ++num_trajectory_nodes_;
 
   // Test if the 'insertion_submap.back()' is one we never saw before.
-  if (trajectory_id >= submap_data_.num_trajectories() ||
+  if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
-      submap_data_.num_indices(trajectory_id) == 0 ||
+      std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
-      submap_data_
+          insertion_submaps.back()) {
-              .at(mapping::SubmapId{
-                  trajectory_id, submap_data_.num_indices(trajectory_id) - 1})
-              .submap != insertion_submaps.back()) {
     // We grow 'submap_data_' as needed. This code assumes that the first
     // time we see a new submap is as 'insertion_submaps.back()'.
     const mapping::SubmapId submap_id =
@@ -185,7 +182,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
        submap_data_.at(submap_id).node_ids) {
     submap_nodes.push_back(mapping::TrajectoryNode{
         trajectory_nodes_.at(submap_node_id).constant_data,
-        inverse_submap_pose * trajectory_nodes_.at(submap_node_id).global_pose});
+        inverse_submap_pose *
+            trajectory_nodes_.at(submap_node_id).global_pose});
   }
 
   const common::Time scan_time = GetLatestScanTime(node_id, submap_id);
@@ -268,16 +266,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                    Constraint::INTRA_SUBMAP});
   }
 
-  for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
+  for (const auto& submap_id_data : submap_data_) {
-       ++trajectory_id) {
+    if (submap_id_data.data.state == SubmapState::kFinished) {
-    for (int submap_index = 0;
+      CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
-         submap_index < submap_data_.num_indices(trajectory_id);
+      ComputeConstraint(node_id, submap_id_data.id);
-         ++submap_index) {
-      const mapping::SubmapId submap_id{trajectory_id, submap_index};
-      if (submap_data_.at(submap_id).state == SubmapState::kFinished) {
-        CHECK_EQ(submap_data_.at(submap_id).node_ids.count(node_id), 0);
-        ComputeConstraint(node_id, submap_id);
-      }
     }
   }
 
@@ -524,9 +516,8 @@ void SparsePoseGraph::RunOptimization() {
 
     const mapping::NodeId last_optimized_node_id =
         std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
-    auto node_it =
+    auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
-        std::next(trajectory_nodes_.FindChecked(last_optimized_node_id));
+    for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
-      for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
          ++node_it) {
       auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
       mutable_trajectory_node.global_pose =
@@ -572,14 +563,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
   return trajectory_connectivity_state_.Components();
 }
 
-int SparsePoseGraph::num_submaps(const int trajectory_id) {
-  common::MutexLocker locker(&mutex_);
-  if (trajectory_id >= submap_data_.num_trajectories()) {
-    return 0;
-  }
-  return submap_data_.num_indices(trajectory_id);
-}
-
 mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
     const mapping::SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
@@ -589,20 +572,15 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
 std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
 SparsePoseGraph::GetAllSubmapData() {
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>> result;
-      all_submap_data(submap_data_.num_trajectories());
+  for (const int trajectory_id : submap_data_.trajectory_ids()) {
-  for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
+    result.resize(trajectory_id + 1);
-       ++trajectory_id) {
+    for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) {
-    all_submap_data[trajectory_id].reserve(
+      result[trajectory_id].resize(submap_id_data.id.submap_index + 1);
-        submap_data_.num_indices(trajectory_id));
+      result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id);
-    for (int submap_index = 0;
-         submap_index < submap_data_.num_indices(trajectory_id);
-         ++submap_index) {
-      all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock(
-          mapping::SubmapId{trajectory_id, submap_index}));
     }
   }
-  return all_submap_data;
+  return result;
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
@@ -624,10 +602,11 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
 
 mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
     const mapping::SubmapId& submap_id) {
-  if (submap_data_.at(submap_id).state == SubmapState::kTrimmed) {
+  const auto it = submap_data_.find(submap_id);
+  if (it == submap_data_.end()) {
     return {};
   }
-  auto submap = submap_data_.at(submap_id).submap;
+  auto submap = it->data.submap;
   if (optimized_submap_transforms_.Contains(submap_id)) {
     // We already have an optimized pose.
     return {submap, optimized_submap_transforms_.at(submap_id).pose};
@@ -692,11 +671,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
   }
 
   // Mark the submap with 'submap_id' as trimmed and remove its data.
-  auto& submap_data = parent_->submap_data_.at(submap_id);
+  CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
-  CHECK(submap_data.state == SubmapState::kFinished);
+  parent_->submap_data_.Trim(submap_id);
-  submap_data.state = SubmapState::kTrimmed;
-  CHECK(submap_data.submap != nullptr);
-  submap_data.submap.reset();
   parent_->constraint_builder_.DeleteScanMatcher(submap_id);
   parent_->optimization_problem_.TrimSubmap(submap_id);
 

cartographer/mapping_3d/sparse_pose_graph.h

@@ -92,7 +92,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
-  int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
   mapping::SparsePoseGraph::SubmapData GetSubmapData(
       const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
@@ -107,7 +106,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // The current state of the submap in the background threads. When this
   // transitions to kFinished, all scans are tried to match against this submap.
   // Likewise, all new scans are matched against submaps which are finished.
-  enum class SubmapState { kActive, kFinished, kTrimmed };
+  enum class SubmapState { kActive, kFinished };
   struct SubmapData {
     std::shared_ptr<const Submap> submap;
 
@@ -209,7 +208,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Submaps get assigned an ID and state as soon as they are seen, even
   // before they take part in the background computations.
-  mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
+  mapping::MapById<mapping::SubmapId, SubmapData> submap_data_
       GUARDED_BY(mutex_);
 
   // Data that are currently being shown.