Add support for multi-trajectory to 3D SLAM. (#146)

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -168,7 +168,7 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
       pose_estimate_ * odometry_correction_;
   const transform::Rigid3d model_prediction = pose_estimate_;
   // TODO(whess): Prefer IMU over odom orientation if configured?
-  const transform::Rigid3d pose_prediction = odometry_prediction;
+  const transform::Rigid3d& pose_prediction = odometry_prediction;
 
   // Computes the rotation without yaw, as defined by GetYaw().
   const transform::Rigid3d tracking_to_tracking_2d =

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -38,7 +38,8 @@ void GlobalTrajectoryBuilder::AddImuData(
     const Eigen::Vector3d& angular_velocity) {
   local_trajectory_builder_->AddImuData(time, linear_acceleration,
                                         angular_velocity);
-  sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity);
+  sparse_pose_graph_->AddImuData(local_trajectory_builder_->submaps(), time,
+                                 linear_acceleration, angular_velocity);
 }
 
 void GlobalTrajectoryBuilder::AddRangefinderData(

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -136,12 +136,13 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
   }
 }
 
-void SparsePoseGraph::AddImuData(common::Time time,
+void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
+                                 common::Time time,
                                  const Eigen::Vector3d& linear_acceleration,
                                  const Eigen::Vector3d& angular_velocity) {
   common::MutexLocker locker(&mutex_);
   AddWorkItem([=]() REQUIRES(mutex_) {
-    optimization_problem_.AddImuData(time, linear_acceleration,
+    optimization_problem_.AddImuData(trajectory, time, linear_acceleration,
                                      angular_velocity);
   });
 }
@@ -202,7 +203,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
       submap_transforms_[matching_index] *
       matching_submap->local_pose().inverse() * pose;
   CHECK_EQ(scan_index, optimization_problem_.node_data().size());
-  optimization_problem_.AddTrajectoryNode(time, optimized_pose);
+  const mapping::Submaps* const scan_trajectory =
+      trajectory_nodes_[scan_index].constant_data->trajectory;
+  optimization_problem_.AddTrajectoryNode(scan_trajectory, time,
+                                          optimized_pose);
   for (const Submap* submap : insertion_submaps) {
     const int submap_index = GetSubmapIndex(submap);
     CHECK(!submap_states_[submap_index].finished);
@@ -319,17 +323,13 @@ void SparsePoseGraph::RunFinalOptimization() {
 void SparsePoseGraph::RunOptimization() {
   if (!submap_transforms_.empty()) {
     transform::Rigid3d submap_0_pose;
-    std::vector<const mapping::Submaps*> trajectories;
     {
       common::MutexLocker locker(&mutex_);
       CHECK(!submap_states_.empty());
       submap_0_pose = submap_states_.front().submap->local_pose();
-      for (const auto& trajectory_node : trajectory_nodes_) {
-        trajectories.push_back(trajectory_node.constant_data->trajectory);
-      }
     }
 
-    optimization_problem_.Solve(constraints_, submap_0_pose, trajectories,
+    optimization_problem_.Solve(constraints_, submap_0_pose,
                                 &submap_transforms_);
     common::MutexLocker locker(&mutex_);
     has_new_optimized_poses_ = true;

cartographer/mapping_3d/sparse_pose_graph.h

@@ -79,7 +79,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_);
 
   // Adds new IMU data to be used in the optimization.
-  void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
+  void AddImuData(const mapping::Submaps* trajectory, common::Time time,
+                  const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity);
 
   void RunFinalOptimization() override;

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -222,16 +222,25 @@ void ConstraintBuilder::ComputeConstraint(
   float score = 0.;
   transform::Rigid3d pose_estimate = transform::Rigid3d::Identity();
 
-  CHECK(!match_full_submap) << "match_full_submap not supported for 3D.";
+  if (match_full_submap) {
-
+    if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
+            initial_pose.rotation(), filtered_point_cloud, point_cloud,
+            options_.global_localization_min_score(), &score, &pose_estimate)) {
+      CHECK_GT(score, options_.global_localization_min_score());
+      trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
+    } else {
+      return;
+    }
+  } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
-          initial_pose, filtered_point_cloud, point_cloud, options_.min_score(),
+            initial_pose, filtered_point_cloud, point_cloud,
-          &score, &pose_estimate)) {
+            options_.min_score(), &score, &pose_estimate)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
     } else {
       return;
     }
+  }
   {
     common::MutexLocker locker(&mutex_);
     score_histogram_.Add(score);

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -77,15 +77,18 @@ OptimizationProblem::OptimizationProblem(
 
 OptimizationProblem::~OptimizationProblem() {}
 
-void OptimizationProblem::AddImuData(common::Time time,
+void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory,
+                                     const common::Time time,
                                      const Eigen::Vector3d& linear_acceleration,
                                      const Eigen::Vector3d& angular_velocity) {
-  imu_data_.push_back(ImuData{time, linear_acceleration, angular_velocity});
+  imu_data_[trajectory].push_back(
+      ImuData{time, linear_acceleration, angular_velocity});
 }
 
 void OptimizationProblem::AddTrajectoryNode(
-    common::Time time, const transform::Rigid3d& point_cloud_pose) {
+    const mapping::Submaps* const trajectory, const common::Time time,
-  node_data_.push_back(NodeData{time, point_cloud_pose});
+    const transform::Rigid3d& point_cloud_pose) {
+  node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
 }
 
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@@ -96,13 +99,17 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
 void OptimizationProblem::Solve(
     const std::vector<Constraint>& constraints,
     const transform::Rigid3d& submap_0_transform,
-    const std::vector<const mapping::Submaps*>& trajectories,
+    std::vector<transform::Rigid3d>* const submap_transforms) {
-    std::vector<transform::Rigid3d>* submap_transforms) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
   }
-  CHECK(!imu_data_.empty());
+
+  // Compute the indices of consecutive nodes for each trajectory.
+  std::map<const mapping::Submaps*, std::vector<size_t>> nodes_per_trajectory;
+  for (size_t j = 0; j != node_data_.size(); ++j) {
+    nodes_per_trajectory[node_data_[j].trajectory].push_back(j);
+  }
 
   ceres::Problem::Options problem_options;
   ceres::Problem problem(problem_options);
@@ -146,33 +153,39 @@ void OptimizationProblem::Solve(
         C_point_clouds[constraint.j].translation());
   }
 
-  CHECK(!node_data_.empty());
+  // Add constraints based on IMU observations of angular velocities and
-  CHECK_GE(trajectories.size(), node_data_.size());
+  // linear acceleration.
+  for (const auto& trajectory_nodes_pair : nodes_per_trajectory) {
+    const mapping::Submaps* const trajectory = trajectory_nodes_pair.first;
+    const std::vector<size_t>& node_indices = trajectory_nodes_pair.second;
+    const std::deque<ImuData>& imu_data = imu_data_.at(trajectory);
+    CHECK(!node_indices.empty());
+    CHECK(!imu_data.empty());
 
-  // Add constraints for IMU observed data: angular velocities and
+    // Skip IMU data before the first node of this trajectory.
-  // accelerations.
+    auto it = imu_data.cbegin();
-  auto it = imu_data_.cbegin();
+    while ((it + 1) != imu_data.cend() &&
-  while ((it + 1) != imu_data_.cend() && (it + 1)->time <= node_data_[0].time) {
+           (it + 1)->time <= node_data_[node_indices[0]].time) {
       ++it;
     }
 
-  for (size_t j = 1; j < node_data_.size(); ++j) {
+    for (size_t j = 1; j < node_indices.size(); ++j) {
       auto it2 = it;
-    const IntegrateImuResult<double> result = IntegrateImu(
+      const IntegrateImuResult<double> result =
-        imu_data_, node_data_[j - 1].time, node_data_[j].time, &it);
+          IntegrateImu(imu_data, node_data_[node_indices[j - 1]].time,
-    if (j + 1 < node_data_.size()) {
+                       node_data_[node_indices[j]].time, &it);
-      const common::Duration first_delta_time =
+      if (j + 1 < node_indices.size()) {
-          node_data_[j].time - node_data_[j - 1].time;
+        const common::Time first_time = node_data_[node_indices[j - 1]].time;
-      const common::Duration second_delta_time =
+        const common::Time second_time = node_data_[node_indices[j]].time;
-          node_data_[j + 1].time - node_data_[j].time;
+        const common::Time third_time = node_data_[node_indices[j + 1]].time;
-      const common::Time first_center =
+        const common::Duration first_duration = second_time - first_time;
-          node_data_[j - 1].time + first_delta_time / 2;
+        const common::Duration second_duration = third_time - second_time;
-      const common::Time second_center =
+        const common::Time first_center = first_time + first_duration / 2;
-          node_data_[j].time + second_delta_time / 2;
+        const common::Time second_center = second_time + second_duration / 2;
         const IntegrateImuResult<double> result_to_first_center =
-          IntegrateImu(imu_data_, node_data_[j - 1].time, first_center, &it2);
+            IntegrateImu(imu_data, first_time, first_center, &it2);
         const IntegrateImuResult<double> result_center_to_center =
-          IntegrateImu(imu_data_, first_center, second_center, &it2);
+            IntegrateImu(imu_data, first_center, second_center, &it2);
         // 'delta_velocity' is the change in velocity from the point in time
         // halfway between the first and second poses to halfway between second
         // and third pose. It is computed from IMU data and still contains a
@@ -183,21 +196,25 @@ void OptimizationProblem::Solve(
              result_to_first_center.delta_rotation) *
             result_center_to_center.delta_velocity;
         problem.AddResidualBlock(
-          new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3, 3,
+            new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
-                                          3, 1>(new AccelerationCostFunction(
+                                            3, 3, 1>(
+                new AccelerationCostFunction(
                     options_.acceleration_weight(), delta_velocity,
-              common::ToSeconds(first_delta_time),
+                    common::ToSeconds(first_duration),
-              common::ToSeconds(second_delta_time))),
+                    common::ToSeconds(second_duration))),
-          nullptr, C_point_clouds[j].rotation(),
+            nullptr, C_point_clouds[node_indices[j]].rotation(),
-          C_point_clouds[j - 1].translation(), C_point_clouds[j].translation(),
+            C_point_clouds[node_indices[j - 1]].translation(),
-          C_point_clouds[j + 1].translation(), &gravity_constant_);
+            C_point_clouds[node_indices[j]].translation(),
+            C_point_clouds[node_indices[j + 1]].translation(),
+            &gravity_constant_);
       }
       problem.AddResidualBlock(
           new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
               new RotationCostFunction(options_.rotation_weight(),
                                        result.delta_rotation)),
-        nullptr, C_point_clouds[j - 1].rotation(),
+          nullptr, C_point_clouds[node_indices[j - 1]].rotation(),
-        C_point_clouds[j].rotation());
+          C_point_clouds[node_indices[j]].rotation());
+    }
   }
 
   // Solve.

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -36,6 +36,8 @@ namespace mapping_3d {
 namespace sparse_pose_graph {
 
 struct NodeData {
+  // TODO(whess): Keep nodes per trajectory instead.
+  const mapping::Submaps* trajectory;
   common::Time time;
   transform::Rigid3d point_cloud_pose;
 };
@@ -53,26 +55,24 @@ class OptimizationProblem {
   OptimizationProblem(const OptimizationProblem&) = delete;
   OptimizationProblem& operator=(const OptimizationProblem&) = delete;
 
-  void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
+  void AddImuData(const mapping::Submaps* trajectory, common::Time time,
+                  const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity);
-  void AddTrajectoryNode(common::Time time,
+  void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
                          const transform::Rigid3d& point_cloud_pose);
 
   void SetMaxNumIterations(int32 max_num_iterations);
 
-  // Computes the optimized poses. The point cloud at 'point_cloud_poses[i]'
+  // Computes the optimized poses.
-  // belongs to 'trajectories[i]'. Within a given trajectory, scans are expected
-  // to be contiguous.
   void Solve(const std::vector<Constraint>& constraints,
              const transform::Rigid3d& submap_0_transform,
-             const std::vector<const mapping::Submaps*>& trajectories,
              std::vector<transform::Rigid3d>* submap_transforms);
 
   const std::vector<NodeData>& node_data() const;
 
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
-  std::deque<ImuData> imu_data_;
+  std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
   std::vector<NodeData> node_data_;
   double gravity_constant_ = 9.8;
 };

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -118,15 +118,14 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
         NoisyNode{RandomTransform(10., 3.), RandomYawOnlyTransform(0.2, 0.3)});
   }
 
-  std::vector<const mapping::Submaps*> trajectories;
   common::Time now = common::FromUniversal(0);
   for (const NoisyNode& node : test_data) {
-    trajectories.push_back(kTrajectory);
     const transform::Rigid3d pose =
         AddNoise(node.ground_truth_pose, node.noise);
-    optimization_problem_.AddImuData(now, Eigen::Vector3d::UnitZ() * 9.81,
+    optimization_problem_.AddImuData(kTrajectory, now,
+                                     Eigen::Vector3d::UnitZ() * 9.81,
                                      Eigen::Vector3d::Zero());
-    optimization_problem_.AddTrajectoryNode(now, pose);
+    optimization_problem_.AddTrajectoryNode(kTrajectory, now, pose);
     now += common::FromSeconds(0.01);
   }
 
@@ -166,7 +165,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
                             node_data[j].point_cloud_pose);
   }
 
-  optimization_problem_.Solve(constraints, kSubmap0Transform, trajectories,
+  optimization_problem_.Solve(constraints, kSubmap0Transform,
                               &submap_transforms);
 
   double translation_error_after = 0.;