Deserialize trajectory nodes. (#569)

cartographer/mapping/id.h

@@ -31,6 +31,13 @@ struct NodeId {
   int trajectory_id;
   int node_index;
 
+  bool operator==(const NodeId& other) const {
+    return std::forward_as_tuple(trajectory_id, node_index) ==
+        std::forward_as_tuple(other.trajectory_id, other.node_index);
+  }
+
+  bool operator!=(const NodeId& other) const { return !operator==(other); }
+
   bool operator<(const NodeId& other) const {
     return std::forward_as_tuple(trajectory_id, node_index) <
            std::forward_as_tuple(other.trajectory_id, other.node_index);

cartographer/mapping/map_builder.cc

@@ -218,6 +218,15 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
     if (!reader->ReadProto(&proto)) {
       break;
     }
+    if (proto.has_node()) {
+      const auto& pose_graph_node =
+          pose_graph.trajectory(proto.node().node_id().trajectory_id())
+              .node(proto.node().node_id().node_index());
+      const transform::Rigid3d pose =
+          transform::ToRigid3(pose_graph_node.pose());
+      sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, pose,
+                                           proto.node());
+    }
     if (proto.has_submap()) {
       const transform::Rigid3d submap_pose = transform::ToRigid3(
           pose_graph.trajectory(proto.submap().submap_id().trajectory_id())

cartographer/mapping/sparse_pose_graph.h

@@ -83,6 +83,12 @@ class SparsePoseGraph {
                                   const transform::Rigid3d& initial_pose,
                                   const proto::Submap& submap) = 0;
 
+  // Adds a 'node' from a proto with the given 'pose' to the frozen trajectory
+  // with 'trajectory_id'.
+  virtual void AddNodeFromProto(int trajectory_id,
+                                const transform::Rigid3d& pose,
+                                const proto::Node& node) = 0;
+
   // Adds a 'trimmer'. It will be used after all data added before it has been
   // included in the pose graph.
   virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -439,6 +439,32 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
   });
 }
 
+void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
+                                       const transform::Rigid3d& pose,
+                                       const mapping::proto::Node& node) {
+  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_);
+  trajectory_connectivity_state_.Add(trajectory_id);
+  const mapping::NodeId node_id = trajectory_nodes_.Append(
+      trajectory_id, mapping::TrajectoryNode{constant_data, pose});
+
+  AddWorkItem([this, node_id, 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(
+        node_id.trajectory_id, constant_data->time,
+        transform::Project2D(constant_data->initial_pose *
+                             gravity_alignment_inverse),
+        transform::Project2D(pose * gravity_alignment_inverse),
+        constant_data->gravity_alignment);
+  });
+}
+
 void SparsePoseGraph::AddTrimmer(
     std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
   common::MutexLocker locker(&mutex_);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -86,6 +86,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void AddSubmapFromProto(int trajectory_id,
                           const transform::Rigid3d& initial_pose,
                           const mapping::proto::Submap& submap) override;
+  void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose,
+                        const mapping::proto::Node& node) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -458,6 +458,27 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
   });
 }
 
+void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
+                                       const transform::Rigid3d& pose,
+                                       const mapping::proto::Node& node) {
+  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_);
+  trajectory_connectivity_state_.Add(trajectory_id);
+  const mapping::NodeId node_id = trajectory_nodes_.Append(
+      trajectory_id, mapping::TrajectoryNode{constant_data, pose});
+
+  AddWorkItem([this, node_id, 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,
+                                            constant_data->time,
+                                            constant_data->initial_pose, pose);
+  });
+}
+
 void SparsePoseGraph::AddTrimmer(
     std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
   common::MutexLocker locker(&mutex_);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -87,6 +87,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void AddSubmapFromProto(int trajectory_id,
                           const transform::Rigid3d& initial_pose,
                           const mapping::proto::Submap& submap) override;
+  void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose,
+                        const mapping::proto::Node& node) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;