Towards localization in 3D. (#438)

Adds loading maps to 3D, deserializes submaps and supports trajectories
without any node. Still no trimming support.

cartographer/mapping/map_builder.cc

@@ -201,6 +201,8 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
       ->set_resolution(0.05);
   unused_options.mutable_trajectory_builder_2d_options()
       ->set_num_odometry_states(1);
+  unused_options.mutable_trajectory_builder_3d_options()
+      ->set_num_odometry_states(1);
 
   const std::unordered_set<string> unused_sensor_ids;
   const int map_trajectory_id =

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -376,6 +376,35 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
   });
 }
 
+void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
+                                         const transform::Rigid3d& initial_pose,
+                                         const mapping::proto::Submap& submap) {
+  if (!submap.has_submap_3d()) {
+    return;
+  }
+
+  std::shared_ptr<const Submap> submap_ptr =
+      std::make_shared<const Submap>(submap.submap_3d());
+
+  common::MutexLocker locker(&mutex_);
+  const mapping::SubmapId submap_id =
+      submap_data_.Append(trajectory_id, SubmapData());
+  submap_data_.at(submap_id).submap = submap_ptr;
+  // Immediately show the submap at the optimized pose.
+  CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()),
+           optimized_submap_transforms_.size());
+  optimized_submap_transforms_.resize(submap_data_.num_trajectories());
+  CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(),
+           submap_id.submap_index);
+  optimized_submap_transforms_.at(trajectory_id)
+      .push_back(sparse_pose_graph::SubmapData{initial_pose});
+  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);
+  });
+}
+
 void SparsePoseGraph::AddTrimmer(
     std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
   common::MutexLocker locker(&mutex_);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -22,6 +22,7 @@
 #include <limits>
 #include <map>
 #include <memory>
+#include <set>
 #include <unordered_map>
 #include <vector>
 
@@ -81,9 +82,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void FreezeTrajectory(int trajectory_id) override;
   void AddSubmapFromProto(int trajectory_id,
                           const transform::Rigid3d& initial_pose,
-                          const mapping::proto::Submap& submap) override {
-    LOG(FATAL) << "Not yet implemented for 3D";
-  }
+                          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;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -210,14 +210,16 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   trajectory_data_.resize(imu_data_.size());
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
+    const auto& node_data = node_data_[trajectory_id];
+    if (node_data.empty()) {
+      // We skip empty trajectories which might not have any IMU data.
+      continue;
+    }
     TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
     problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
                               new ceres::QuaternionParameterization());
     const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
     CHECK(!imu_data.empty());
-    // TODO(whess): Add support for empty trajectories.
-    const auto& node_data = node_data_[trajectory_id];
-    CHECK(!node_data.empty());
 
     // Skip IMU data before the first node of this trajectory.
     auto it = imu_data.cbegin();