Add 3D localization support. (#632)

This adds information about which nodes are contained in which
deserialized submaps. This is necessary for the rotational scan
matcher.

Also skips frozen trajectories when tying together nodes of a
trajectory with IMU data which is unavailable in this case.

cartographer/mapping/map_builder.cc

@@ -229,6 +229,22 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
       sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
     }
   }
+
+  // Add information about which nodes belong to which submap.
+  for (const proto::SparsePoseGraph::Constraint& constraint_proto :
+       pose_graph.constraint()) {
+    if (constraint_proto.tag() !=
+        mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) {
+      continue;
+    }
+    const NodeId node_id{
+        trajectory_remapping.at(constraint_proto.node_id().trajectory_id()),
+        constraint_proto.node_id().node_index()};
+    const SubmapId submap_id{
+        trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()),
+        constraint_proto.submap_id().submap_index()};
+    sparse_pose_graph_->AddNodeToSubmap(node_id, submap_id);
+  }
   CHECK(reader->eof());
 }
 

cartographer/mapping/sparse_pose_graph.h

@@ -87,6 +87,11 @@ class SparsePoseGraph {
   virtual void AddNodeFromProto(const transform::Rigid3d& global_pose,
                                 const proto::Node& node) = 0;
 
+  // Adds information that 'node_id' was inserted into 'submap_id'. The submap
+  // has to be deserialized first.
+  virtual void AddNodeToSubmap(const NodeId& node_id,
+                               const SubmapId& submap_id) = 0;
+
   // Adds serialized constraints. The corresponding trajectory nodes and submaps
   // have to be deserialized before calling this function.
   virtual void AddSerializedConstraints(

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -427,6 +427,14 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
   });
 }
 
+void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
+                                      const mapping::SubmapId& submap_id) {
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
+    submap_data_.at(submap_id).node_ids.insert(node_id);
+  });
+}
+
 void SparsePoseGraph::AddSerializedConstraints(
     const std::vector<Constraint>& constraints) {
   common::MutexLocker locker(&mutex_);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -88,6 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                           const mapping::proto::Submap& submap) override;
   void AddNodeFromProto(const transform::Rigid3d& global_pose,
                         const mapping::proto::Node& node) override;
+  void AddNodeToSubmap(const mapping::NodeId& node_id,
+                       const mapping::SubmapId& submap_id) override;
   void AddSerializedConstraints(
       const std::vector<Constraint>& constraints) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -441,6 +441,14 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
   });
 }
 
+void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
+                                      const mapping::SubmapId& submap_id) {
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
+    submap_data_.at(submap_id).node_ids.insert(node_id);
+  });
+}
+
 void SparsePoseGraph::AddSerializedConstraints(
     const std::vector<Constraint>& constraints) {
   common::MutexLocker locker(&mutex_);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -88,6 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                           const mapping::proto::Submap& submap) override;
   void AddNodeFromProto(const transform::Rigid3d& global_pose,
                         const mapping::proto::Node& node) override;
+  void AddNodeToSubmap(const mapping::NodeId& node_id,
+                       const mapping::SubmapId& submap_id) override;
   void AddSerializedConstraints(
       const std::vector<Constraint>& constraints) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -216,6 +216,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
     for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
       const int trajectory_id = node_it->id.trajectory_id;
       const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
+      if (frozen_trajectories.count(trajectory_id) != 0) {
+        // We skip frozen trajectories.
+        node_it = trajectory_end;
+        continue;
+      }
       TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
 
       problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,