Switch trajectory_nodes in SPG2D to MapById. (#592)

cartographer/mapping/id.h

@@ -129,6 +129,8 @@ class Range {
 
 // Reminiscent of std::map, but indexed by 'IdType' which can be 'NodeId' or
 // 'SubmapId'.
+// Note: This container will only ever contain non-empty trajectories. Trimming
+// the last remaining node of a trajectory drops the trajectory.
 template <typename IdType, typename DataType>
 class MapById {
  private:
@@ -296,6 +298,9 @@ class MapById {
       trajectory.can_append_ = false;
     }
     trajectory.data_.erase(it);
+    if (trajectory.data_.empty()) {
+      trajectories_.erase(id.trajectory_id);
+    }
   }
 
   bool Contains(const IdType& id) const {

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -469,30 +469,33 @@ void SparsePoseGraph::RunOptimization() {
 
   const auto& submap_data = optimization_problem_.submap_data();
   const auto& node_data = optimization_problem_.node_data();
-  for (auto node_it = node_data.begin(); node_it != node_data.end();) {
+  for (const int trajectory_id : node_data.trajectory_ids()) {
-    const int trajectory_id = node_it->id.trajectory_id;
+    for (const auto& node : node_data.trajectory(trajectory_id)) {
-    const auto trajectory_end = node_data.EndOfTrajectory(trajectory_id);
+      auto& mutable_trajectory_node = trajectory_nodes_.at(node.id);
-    const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
+      mutable_trajectory_node.global_pose =
-    for (; node_it != trajectory_end; ++node_it) {
+          transform::Embed3D(node.data.pose) *
-      const mapping::NodeId node_id = node_it->id;
+          transform::Rigid3d::Rotation(
-      auto& node = trajectory_nodes_.at(node_id);
+              mutable_trajectory_node.constant_data->gravity_alignment);
-      node.global_pose =
-          transform::Embed3D(node_it->data.pose) *
-          transform::Rigid3d::Rotation(node.constant_data->gravity_alignment);
     }
-    // Extrapolate all point cloud poses that were added later.
+
+    // Extrapolate all point cloud poses that were not included in the
+    // 'optimization_problem_' yet.
     const auto local_to_new_global =
         ComputeLocalToGlobalTransform(submap_data, trajectory_id);
     const auto local_to_old_global = ComputeLocalToGlobalTransform(
         optimized_submap_transforms_, trajectory_id);
     const transform::Rigid3d old_global_to_new_global =
         local_to_new_global * local_to_old_global.inverse();
-    const int last_optimized_node_index = std::prev(node_it)->id.node_index;
+
-    for (int node_index = last_optimized_node_index + 1; node_index < num_nodes;
+    const mapping::NodeId last_optimized_node_id =
-         ++node_index) {
+        std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
-      const mapping::NodeId node_id{trajectory_id, node_index};
+    auto node_it =
-      auto& node_pose = trajectory_nodes_.at(node_id).global_pose;
+        std::next(trajectory_nodes_.FindChecked(last_optimized_node_id));
-      node_pose = old_global_to_new_global * node_pose;
+    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 =
+          old_global_to_new_global * mutable_trajectory_node.global_pose;
     }
   }
   optimized_submap_transforms_ = submap_data;
@@ -500,8 +503,17 @@ void SparsePoseGraph::RunOptimization() {
 
 std::vector<std::vector<mapping::TrajectoryNode>>
 SparsePoseGraph::GetTrajectoryNodes() {
+  // TODO(cschuet): Rewrite downstream code and switch to MapById.
   common::MutexLocker locker(&mutex_);
-  return trajectory_nodes_.data();
+  std::vector<std::vector<mapping::TrajectoryNode>> nodes;
+  for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
+    nodes.resize(trajectory_id + 1);
+    for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
+      nodes.at(trajectory_id).resize(node.id.node_index + 1);
+      nodes.at(trajectory_id).at(node.id.node_index) = node.data;
+    }
+  }
+  return nodes;
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

cartographer/mapping_2d/sparse_pose_graph.h

@@ -209,8 +209,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       GUARDED_BY(mutex_);
 
   // Data that are currently being shown.
-  mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> trajectory_nodes_
-      trajectory_nodes_ GUARDED_BY(mutex_);
+      GUARDED_BY(mutex_);
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Current submap transforms used for displaying data.

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -508,28 +508,29 @@ void SparsePoseGraph::RunOptimization() {
 
   const auto& submap_data = optimization_problem_.submap_data();
   const auto& node_data = optimization_problem_.node_data();
-  for (auto node_it = node_data.begin(); node_it != node_data.end();) {
+  for (const int trajectory_id : node_data.trajectory_ids()) {
-    const int trajectory_id = node_it->id.trajectory_id;
+    for (const auto& node : node_data.trajectory(trajectory_id)) {
-    const auto trajectory_end = node_data.EndOfTrajectory(trajectory_id);
+      trajectory_nodes_.at(node.id).global_pose = node.data.pose;
-    const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
-    for (; node_it != trajectory_end; ++node_it) {
-      const mapping::NodeId node_id = node_it->id;
-      auto& node = trajectory_nodes_.at(node_id);
-      node.global_pose = node_it->data.pose;
     }
-    // Extrapolate all point cloud poses that were added later.
+
+    // Extrapolate all point cloud poses that were not included in the
+    // 'optimization_problem_' yet.
     const auto local_to_new_global =
         ComputeLocalToGlobalTransform(submap_data, trajectory_id);
     const auto local_to_old_global = ComputeLocalToGlobalTransform(
         optimized_submap_transforms_, trajectory_id);
     const transform::Rigid3d old_global_to_new_global =
         local_to_new_global * local_to_old_global.inverse();
-    const int last_optimized_node_index = std::prev(node_it)->id.node_index;
+
-    for (int node_index = last_optimized_node_index + 1; node_index < num_nodes;
+    const mapping::NodeId last_optimized_node_id =
-         ++node_index) {
+        std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
-      const mapping::NodeId node_id{trajectory_id, node_index};
+    auto node_it =
-      auto& node_pose = trajectory_nodes_.at(node_id).global_pose;
+        std::next(trajectory_nodes_.FindChecked(last_optimized_node_id));
-      node_pose = old_global_to_new_global * node_pose;
+      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 =
+          old_global_to_new_global * mutable_trajectory_node.global_pose;
     }
   }
   optimized_submap_transforms_ = submap_data;
@@ -542,8 +543,17 @@ void SparsePoseGraph::RunOptimization() {
 
 std::vector<std::vector<mapping::TrajectoryNode>>
 SparsePoseGraph::GetTrajectoryNodes() {
+  // TODO(cschuet): Rewrite downstream code and switch to MapById.
   common::MutexLocker locker(&mutex_);
-  return trajectory_nodes_.data();
+  std::vector<std::vector<mapping::TrajectoryNode>> nodes;
+  for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
+    nodes.resize(trajectory_id + 1);
+    for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
+      nodes.at(trajectory_id).resize(node.id.node_index + 1);
+      nodes.at(trajectory_id).at(node.id.node_index) = node.data;
+    }
+  }
+  return nodes;
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

cartographer/mapping_3d/sparse_pose_graph.h

@@ -213,8 +213,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       GUARDED_BY(mutex_);
 
   // Data that are currently being shown.
-  mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> trajectory_nodes_
-      trajectory_nodes_ GUARDED_BY(mutex_);
+      GUARDED_BY(mutex_);
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Current submap transforms used for displaying data.