First rough implementation of loading a map. (#382)

This implements loading of serialized maps in 2D:
We insert all submaps at the serialized optimized pose into
the pose graph and remove their poses from the optimization.

Related to #197 and #315.

cartographer/io/proto_stream.cc

@@ -84,5 +84,7 @@ bool ProtoStreamReader::Read(string* decompressed_data) {
   return true;
 }
 
+bool ProtoStreamReader::ok() const { return !in_.fail(); }
+
 }  // namespace io
 }  // namespace cartographer

cartographer/io/proto_stream.h

@@ -71,6 +71,8 @@ class ProtoStreamReader {
            proto->ParseFromString(decompressed_data);
   }
 
+  bool ok() const;
+
  private:
   bool Read(string* decompressed_data);
 

cartographer/mapping/map_builder.cc

@@ -157,6 +157,43 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
   // TODO(whess): Serialize additional sensor data: IMU, odometry.
 }
 
+void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
+  proto::SparsePoseGraph pose_graph;
+  CHECK(reader->ReadProto(&pose_graph));
+
+  // TODO(whess): Not all trajectories should be builders, i.e. support should
+  // be added for trajectories without latest pose, options, etc. Appease the
+  // trajectory builder for now.
+  proto::TrajectoryBuilderOptions unused_options;
+  unused_options.mutable_trajectory_builder_2d_options()
+      ->mutable_submaps_options()
+      ->set_resolution(0.05);
+  unused_options.mutable_trajectory_builder_2d_options()
+      ->set_num_odometry_states(1);
+
+  const std::unordered_set<string> unused_sensor_ids;
+  const int map_trajectory_id =
+      AddTrajectoryBuilder(unused_sensor_ids, unused_options);
+  FinishTrajectory(map_trajectory_id);
+  sparse_pose_graph_->FreezeTrajectory(map_trajectory_id);
+
+  for (;;) {
+    proto::SerializedData proto;
+    if (!reader->ReadProto(&proto)) {
+      break;
+    }
+    if (proto.has_submap()) {
+      const transform::Rigid3d submap_pose = transform::ToRigid3(
+          pose_graph.trajectory(proto.submap().submap_id().trajectory_id())
+              .submap(proto.submap().submap_id().submap_index())
+              .pose());
+      sparse_pose_graph_->AddSubmapFromProto(map_trajectory_id, submap_pose,
+                                             proto.submap());
+    }
+  }
+  CHECK(reader->ok());
+}
+
 int MapBuilder::num_trajectory_builders() const {
   return trajectory_builders_.size();
 }

cartographer/mapping/map_builder.h

@@ -81,6 +81,9 @@ class MapBuilder {
   // Serializes the current state to a proto stream.
   void SerializeState(io::ProtoStreamWriter* writer);
 
+  // Loads submaps from a proto stream into a new frozen trajectory.
+  void LoadMap(io::ProtoStreamReader* reader);
+
   int num_trajectory_builders() const;
 
   mapping::SparsePoseGraph* sparse_pose_graph();

cartographer/mapping/sparse_pose_graph.h

@@ -26,6 +26,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
 #include "cartographer/mapping/submaps.h"
@@ -73,6 +74,15 @@ class SparsePoseGraph {
   SparsePoseGraph(const SparsePoseGraph&) = delete;
   SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
 
+  // 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
+  // trajectory with 'trajectory_id'.
+  virtual void AddSubmapFromProto(int trajectory_id,
+                                  const transform::Rigid3d& initial_pose,
+                                  const proto::Submap& submap) = 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

@@ -371,6 +371,36 @@ void SparsePoseGraph::WaitForAllComputations() {
   locker.Await([&notification]() { return notification; });
 }
 
+void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+    CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0);
+    frozen_trajectories_.insert(trajectory_id);
+  });
+}
+
+void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
+                                         const transform::Rigid3d& initial_pose,
+                                         const mapping::proto::Submap& submap) {
+  if (!submap.has_submap_2d()) {
+    return;
+  }
+
+  std::shared_ptr<const Submap> submap_ptr =
+      std::make_shared<const Submap>(submap.submap_2d());
+
+  common::MutexLocker locker(&mutex_);
+  const mapping::SubmapId submap_id =
+      submap_data_.Append(trajectory_id, SubmapData());
+  submap_data_.at(submap_id).submap = submap_ptr;
+  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,
+                                    transform::Project2D(initial_pose));
+  });
+}
+
 void SparsePoseGraph::AddTrimmer(
     std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
   common::MutexLocker locker(&mutex_);
@@ -395,7 +425,7 @@ void SparsePoseGraph::RunOptimization() {
   if (optimization_problem_.submap_data().empty()) {
     return;
   }
-  optimization_problem_.Solve(constraints_);
+  optimization_problem_.Solve(constraints_, frozen_trajectories_);
   common::MutexLocker locker(&mutex_);
 
   std::vector<int> num_trimmed_submaps;

cartographer/mapping_2d/sparse_pose_graph.h

@@ -80,6 +80,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                   const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity);
 
+  void FreezeTrajectory(int trajectory_id) override;
+  void AddSubmapFromProto(int trajectory_id,
+                          const transform::Rigid3d& initial_pose,
+                          const mapping::proto::Submap& submap) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
@@ -208,6 +212,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_
       GUARDED_BY(mutex_);
 
+  // Set of all frozen trajectories not being optimized.
+  std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
+
   // Allows querying and manipulating the pose graph by the 'trimmers_'. The
   // 'mutex_' of the pose graph is held while this class is used.
   class TrimmingHandle : public mapping::Trimmable {

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -123,7 +123,8 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
       max_num_iterations);
 }
 
-void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
+void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
+                                const std::set<int>& frozen_trajectories) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
@@ -140,15 +141,17 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   bool first_submap = true;
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
+    const bool frozen = frozen_trajectories.count(trajectory_id);
     // Reserve guarantees that data does not move, so the pointers for Ceres
     // stay valid.
     C_submaps[trajectory_id].reserve(submap_data_[trajectory_id].size());
     for (const SubmapData& submap_data : submap_data_[trajectory_id]) {
       C_submaps[trajectory_id].push_back(FromPose(submap_data.pose));
       problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
-      if (first_submap) {
+      if (first_submap || frozen) {
         first_submap = false;
-        // Fix the pose of the first submap.
+        // Fix the pose of the first submap or all submaps of a frozen
+        // trajectory.
         problem.SetParameterBlockConstant(
             C_submaps[trajectory_id].back().data());
       }

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -70,7 +70,8 @@ class OptimizationProblem {
   void SetMaxNumIterations(int32 max_num_iterations);
 
   // Computes the optimized poses.
-  void Solve(const std::vector<Constraint>& constraints);
+  void Solve(const std::vector<Constraint>& constraints,
+             const std::set<int>& frozen_trajectories);
 
   const std::vector<std::deque<NodeData>>& node_data() const;
   const std::vector<std::deque<SubmapData>>& submap_data() const;

cartographer/mapping_3d/sparse_pose_graph.h

@@ -77,6 +77,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                   const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity);
 
+  void FreezeTrajectory(const int trajectory_id) override {
+    LOG(FATAL) << "Not yet implemented for 3D.";
+  }
+  void AddSubmapFromProto(const int trajectory_id,
+                          const transform::Rigid3d& initial_pose,
+                          const mapping::proto::Submap& submap) override {
+    LOG(FATAL) << "Not yet implemented for 3D";
+  }
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override {
     LOG(FATAL) << "Not yet implemented for 3D.";
   }