Support multiple trajectories in LoadMap. (#567)

cartographer/mapping/id.h

@@ -249,6 +249,8 @@ class MapById {
 
   // Inserts data (which must not exist already) into a trajectory.
   void Insert(const IdType& id, const DataType& data) {
+    CHECK_GE(id.trajectory_id, 0);
+    CHECK_GE(GetIndex(id), 0);
     auto& trajectory = trajectories_[id.trajectory_id];
     trajectory.can_append_ = false;
     CHECK(trajectory.data_.emplace(GetIndex(id), data).second);

cartographer/mapping/map_builder.cc

@@ -177,8 +177,16 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
   proto::SparsePoseGraph pose_graph;
   CHECK(reader->ReadProto(&pose_graph));
 
-  const int map_trajectory_id = AddTrajectoryForDeserialization();
+  std::map<int, int> trajectory_remapping;
-  sparse_pose_graph_->FreezeTrajectory(map_trajectory_id);
+  for (auto& trajectory_proto : *pose_graph.mutable_trajectory()) {
+    const int new_trajectory_id = AddTrajectoryForDeserialization();
+    CHECK(trajectory_remapping
+              .emplace(trajectory_proto.trajectory_id(), new_trajectory_id)
+              .second)
+        << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
+    trajectory_proto.set_trajectory_id(new_trajectory_id);
+    sparse_pose_graph_->FreezeTrajectory(new_trajectory_id);
+  }
 
   MapById<SubmapId, transform::Rigid3d> submap_poses;
   for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
@@ -205,18 +213,20 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
       break;
     }
     if (proto.has_node()) {
+      proto.mutable_node()->mutable_node_id()->set_trajectory_id(
+          trajectory_remapping.at(proto.node().node_id().trajectory_id()));
       const transform::Rigid3d node_pose =
           node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
                                proto.node().node_id().node_index()});
-      sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, node_pose,
+      sparse_pose_graph_->AddNodeFromProto(node_pose, proto.node());
-                                           proto.node());
     }
     if (proto.has_submap()) {
+      proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
+          trajectory_remapping.at(proto.submap().submap_id().trajectory_id()));
       const transform::Rigid3d submap_pose =
           submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(),
                                    proto.submap().submap_id().submap_index()});
-      sparse_pose_graph_->AddSubmapFromProto(map_trajectory_id, submap_pose,
+      sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
-                                             proto.submap());
     }
   }
   CHECK(reader->eof());

cartographer/mapping/sparse_pose_graph.h

@@ -77,16 +77,14 @@ class SparsePoseGraph {
   // Freezes a trajectory. Poses in this trajectory will not be optimized.
   virtual void FreezeTrajectory(int trajectory_id) = 0;
 
-  // Adds a 'submap' from a proto with the given 'initial_pose' to the frozen
+  // Adds a 'submap' from a proto with the given 'global_pose' to the
-  // trajectory with 'trajectory_id'.
+  // appropriate frozen trajectory.
-  virtual void AddSubmapFromProto(int trajectory_id,
+  virtual void AddSubmapFromProto(const transform::Rigid3d& global_pose,
-                                  const transform::Rigid3d& initial_pose,
                                   const proto::Submap& submap) = 0;
 
-  // Adds a 'node' from a proto with the given 'pose' to the frozen trajectory
+  // Adds a 'node' from a proto with the given 'global_pose' to the
-  // with 'trajectory_id'.
+  // appropriate frozen trajectory.
-  virtual void AddNodeFromProto(int trajectory_id,
+  virtual void AddNodeFromProto(const transform::Rigid3d& global_pose,
-                                const transform::Rigid3d& pose,
                                 const proto::Node& node) = 0;
 
   // Adds a 'trimmer'. It will be used after all data added before it has been

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -373,55 +373,55 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
   });
 }
 
-void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
+void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
-                                         const transform::Rigid3d& initial_pose,
                                          const mapping::proto::Submap& submap) {
   if (!submap.has_submap_2d()) {
     return;
   }
 
+  const mapping::SubmapId submap_id = {submap.submap_id().trajectory_id(),
+                                       submap.submap_id().submap_index()};
   std::shared_ptr<const Submap> submap_ptr =
       std::make_shared<const Submap>(submap.submap_2d());
-  const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose);
+  const transform::Rigid2d global_pose_2d = transform::Project2D(global_pose);
 
   common::MutexLocker locker(&mutex_);
-  AddTrajectoryIfNeeded(trajectory_id);
+  AddTrajectoryIfNeeded(submap_id.trajectory_id);
-  const mapping::SubmapId submap_id =
+  submap_data_.Insert(submap_id, SubmapData());
-      submap_data_.Append(trajectory_id, SubmapData());
   submap_data_.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the optimized pose.
-  CHECK_EQ(submap_id,
+  optimized_submap_transforms_.Insert(
-           optimized_submap_transforms_.Append(
+      submap_id, sparse_pose_graph::SubmapData{global_pose_2d});
-               trajectory_id, sparse_pose_graph::SubmapData{initial_pose_2d}));
+  AddWorkItem([this, submap_id, global_pose_2d]() REQUIRES(mutex_) {
-  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;
-    optimization_problem_.AddSubmap(submap_id.trajectory_id, initial_pose_2d);
+    optimization_problem_.InsertSubmap(submap_id, global_pose_2d);
   });
 }
 
-void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
+void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
-                                       const transform::Rigid3d& pose,
                                        const mapping::proto::Node& node) {
+  const mapping::NodeId node_id = {node.node_id().trajectory_id(),
+                                   node.node_id().node_index()};
   std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data =
       std::make_shared<const mapping::TrajectoryNode::Data>(
           mapping::FromProto(node.node_data()));
 
   common::MutexLocker locker(&mutex_);
-  AddTrajectoryIfNeeded(trajectory_id);
+  AddTrajectoryIfNeeded(node_id.trajectory_id);
-  const mapping::NodeId node_id = trajectory_nodes_.Append(
+  trajectory_nodes_.Insert(node_id,
-      trajectory_id, mapping::TrajectoryNode{constant_data, pose});
+                           mapping::TrajectoryNode{constant_data, global_pose});
 
-  AddWorkItem([this, node_id, pose]() REQUIRES(mutex_) {
+  AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
     CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
     const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
     const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
         constant_data->gravity_alignment.inverse());
-    optimization_problem_.AddTrajectoryNode(
+    optimization_problem_.InsertTrajectoryNode(
-        node_id.trajectory_id, constant_data->time,
+        node_id, constant_data->time,
         transform::Project2D(constant_data->local_pose *
                              gravity_alignment_inverse),
-        transform::Project2D(pose * gravity_alignment_inverse),
+        transform::Project2D(global_pose * gravity_alignment_inverse),
         constant_data->gravity_alignment);
   });
 }

cartographer/mapping_2d/sparse_pose_graph.h

@@ -84,10 +84,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const sensor::FixedFramePoseData& fixed_frame_pose_data);
 
   void FreezeTrajectory(int trajectory_id) override;
-  void AddSubmapFromProto(int trajectory_id,
+  void AddSubmapFromProto(const transform::Rigid3d& global_pose,
-                          const transform::Rigid3d& initial_pose,
                           const mapping::proto::Submap& submap) override;
-  void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose,
+  void AddNodeFromProto(const transform::Rigid3d& global_pose,
                         const mapping::proto::Node& node) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -86,6 +86,15 @@ void OptimizationProblem::AddTrajectoryNode(
                     NodeData{time, initial_pose, pose, gravity_alignment});
 }
 
+void OptimizationProblem::InsertTrajectoryNode(
+    const mapping::NodeId& node_id, const common::Time time,
+    const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
+    const Eigen::Quaterniond& gravity_alignment) {
+  node_data_.Insert(node_id,
+                    NodeData{time, initial_pose, pose, gravity_alignment});
+}
+
+
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
   node_data_.Trim(node_id);
 
@@ -106,6 +115,11 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
   submap_data_.Append(trajectory_id, SubmapData{submap_pose});
 }
 
+void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id,
+                                       const transform::Rigid2d& submap_pose) {
+  submap_data_.Insert(submap_id, SubmapData{submap_pose});
+}
+
 void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
   submap_data_.Trim(submap_id);
 }

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -69,8 +69,14 @@ class OptimizationProblem {
                          const transform::Rigid2d& initial_pose,
                          const transform::Rigid2d& pose,
                          const Eigen::Quaterniond& gravity_alignment);
+  void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
+                            const transform::Rigid2d& initial_pose,
+                            const transform::Rigid2d& pose,
+                            const Eigen::Quaterniond& gravity_alignment);
   void TrimTrajectoryNode(const mapping::NodeId& node_id);
   void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
+  void InsertSubmap(const mapping::SubmapId& submap_id,
+                    const transform::Rigid2d& submap_pose);
   void TrimSubmap(const mapping::SubmapId& submap_id);
 
   void SetMaxNumIterations(int32 max_num_iterations);

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -393,50 +393,49 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
   });
 }
 
-void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
+void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
-                                         const transform::Rigid3d& initial_pose,
                                          const mapping::proto::Submap& submap) {
   if (!submap.has_submap_3d()) {
     return;
   }
 
+  const mapping::SubmapId submap_id = {submap.submap_id().trajectory_id(),
+                                       submap.submap_id().submap_index()};
   std::shared_ptr<const Submap> submap_ptr =
       std::make_shared<const Submap>(submap.submap_3d());
 
   common::MutexLocker locker(&mutex_);
-  AddTrajectoryIfNeeded(trajectory_id);
+  AddTrajectoryIfNeeded(submap_id.trajectory_id);
-  const mapping::SubmapId submap_id =
+  submap_data_.Insert(submap_id, SubmapData());
-      submap_data_.Append(trajectory_id, SubmapData());
   submap_data_.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the optimized pose.
-  CHECK_EQ(submap_id,
+  optimized_submap_transforms_.Insert(
-           optimized_submap_transforms_.Append(
+      submap_id, sparse_pose_graph::SubmapData{global_pose});
-               trajectory_id, sparse_pose_graph::SubmapData{initial_pose}));
+  AddWorkItem([this, submap_id, global_pose]() REQUIRES(mutex_) {
-  AddWorkItem([this, submap_id, initial_pose]() REQUIRES(mutex_) {
     CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
     submap_data_.at(submap_id).state = SubmapState::kFinished;
-    optimization_problem_.AddSubmap(submap_id.trajectory_id, initial_pose);
+    optimization_problem_.InsertSubmap(submap_id, global_pose);
   });
 }
 
-void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
+void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
-                                       const transform::Rigid3d& pose,
                                        const mapping::proto::Node& node) {
+  const mapping::NodeId node_id = {node.node_id().trajectory_id(),
+                                   node.node_id().node_index()};
   std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data =
       std::make_shared<const mapping::TrajectoryNode::Data>(
           mapping::FromProto(node.node_data()));
 
   common::MutexLocker locker(&mutex_);
-  AddTrajectoryIfNeeded(trajectory_id);
+  AddTrajectoryIfNeeded(node_id.trajectory_id);
-  const mapping::NodeId node_id = trajectory_nodes_.Append(
+  trajectory_nodes_.Insert(node_id,
-      trajectory_id, mapping::TrajectoryNode{constant_data, pose});
+                           mapping::TrajectoryNode{constant_data, global_pose});
 
-  AddWorkItem([this, node_id, pose]() REQUIRES(mutex_) {
+  AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
     CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
     const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
-    optimization_problem_.AddTrajectoryNode(node_id.trajectory_id,
+    optimization_problem_.InsertTrajectoryNode(
-                                            constant_data->time,
+        node_id, constant_data->time, constant_data->local_pose, global_pose);
-                                            constant_data->local_pose, pose);
   });
 }
 

cartographer/mapping_3d/sparse_pose_graph.h

@@ -84,10 +84,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const sensor::FixedFramePoseData& fixed_frame_pose_data);
 
   void FreezeTrajectory(int trajectory_id) override;
-  void AddSubmapFromProto(int trajectory_id,
+  void AddSubmapFromProto(const transform::Rigid3d& global_pose,
-                          const transform::Rigid3d& initial_pose,
                           const mapping::proto::Submap& submap) override;
-  void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose,
+  void AddNodeFromProto(const transform::Rigid3d& global_pose,
                         const mapping::proto::Node& node) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -89,6 +89,15 @@ void OptimizationProblem::AddTrajectoryNode(
   node_data_.Append(trajectory_id, NodeData{time, initial_pose, pose});
 }
 
+void OptimizationProblem::InsertTrajectoryNode(
+    const mapping::NodeId& node_id, const common::Time time,
+    const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose) {
+  CHECK_GE(node_id.trajectory_id, 0);
+  trajectory_data_.resize(std::max(
+      trajectory_data_.size(), static_cast<size_t>(node_id.trajectory_id) + 1));
+  node_data_.Insert(node_id, NodeData{time, initial_pose, pose});
+}
+
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
   node_data_.Trim(node_id);
 
@@ -112,6 +121,15 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
   submap_data_.Append(trajectory_id, SubmapData{submap_pose});
 }
 
+void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id,
+                                       const transform::Rigid3d& submap_pose) {
+  CHECK_GE(submap_id.trajectory_id, 0);
+  trajectory_data_.resize(
+      std::max(trajectory_data_.size(),
+               static_cast<size_t>(submap_id.trajectory_id) + 1));
+  submap_data_.Insert(submap_id, SubmapData{submap_pose});
+}
+
 void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
   submap_data_.Trim(submap_id);
 }

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -74,8 +74,13 @@ class OptimizationProblem {
   void AddTrajectoryNode(int trajectory_id, common::Time time,
                          const transform::Rigid3d& initial_pose,
                          const transform::Rigid3d& pose);
+  void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
+                            const transform::Rigid3d& initial_pose,
+                            const transform::Rigid3d& pose);
   void TrimTrajectoryNode(const mapping::NodeId& node_id);
   void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
+  void InsertSubmap(const mapping::SubmapId& submap_id,
+                    const transform::Rigid3d& submap_pose);
   void TrimSubmap(const mapping::SubmapId& submap_id);
 
   void SetMaxNumIterations(int32 max_num_iterations);