Fix 3D SLAM loop closure issues. (#296)

3D submaps are now oriented approximately gravity aligned.
This is so that accumulating error in the local SLAM frame is
no longer a problem for finding loop closures. It also ensures
that the z search window size is approximately in the gravity
direction.

We now also pass an estimate of gravity orientation when doing
multi-trajectory matches. Otherwise trajectories starting with
different orientation relative to gravity could not be connected.

The gravity alignment is currently derived from the ImuTracker.
It might be possible to further improve on this by using the
latest gravity direction from the optimized poses.

cartographer/kalman_filter/pose_tracker.h

@@ -122,6 +122,10 @@ class PoseTracker {
 
   const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
 
+  Eigen::Quaterniond gravity_orientation() const {
+    return imu_tracker_.orientation();
+  }
+
  private:
   // Returns the distribution required to initialize the KalmanFilter.
   static Distribution KalmanFilterInit();

cartographer/mapping/imu_tracker.h

@@ -41,7 +41,7 @@ class ImuTracker {
       const Eigen::Vector3d& imu_angular_velocity);
 
   // Query the current orientation estimate.
-  Eigen::Quaterniond orientation() { return orientation_; }
+  Eigen::Quaterniond orientation() const { return orientation_; }
 
  private:
   const double imu_gravity_time_constant_;

cartographer/mapping/submaps.h

@@ -52,19 +52,15 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
   return value;
 }
 
-// An individual submap, which has an initial position 'origin', keeps track of
+// An individual submap, which has a 'local_pose' in the local SLAM frame, keeps
-// how many range data were inserted into it, and sets the
+// track of how many range data were inserted into it, and sets the
 // 'finished_probability_grid' to be used for loop closing once the map no
 // longer changes.
 struct Submap {
-  Submap(const Eigen::Vector3f& origin) : origin(origin) {}
+  Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {}
 
-  transform::Rigid3d local_pose() const {
+  // Local SLAM pose of this submap.
-    return transform::Rigid3d::Translation(origin.cast<double>());
+  const transform::Rigid3d local_pose;
-  }
-
-  // Origin of this submap.
-  const Eigen::Vector3f origin;
 
   // Number of RangeData inserted.
   int num_range_data = 0;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -441,8 +441,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
   return extrapolated_submap_transforms.back() *
          submap_states_.at(trajectory_id)
              .at(extrapolated_submap_transforms.size() - 1)
-             .submap->local_pose()
+             .submap->local_pose.inverse();
-             .inverse();
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@@ -476,14 +475,13 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     } else if (result.empty()) {
       result.push_back(transform::Rigid3d::Identity());
     } else {
-      // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
+      // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
       // is okay, since the member is const.
       result.push_back(result.back() *
                        submap_states_.at(trajectory_id)
                            .at(result.size() - 1)
-                           .submap->local_pose()
+                           .submap->local_pose.inverse() *
-                           .inverse() *
+                       state.submap->local_pose);
-                       state.submap->local_pose());
     }
   }
 

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -40,7 +40,7 @@ namespace mapping_2d {
 namespace sparse_pose_graph {
 
 transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
-  return transform::Project2D(submap.local_pose());
+  return transform::Project2D(submap.local_pose);
 }
 
 ConstraintBuilder::ConstraintBuilder(

cartographer/mapping_2d/submaps.cc

@@ -102,7 +102,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
 }
 
 Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
-    : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.)),
+    : mapping::Submap(transform::Rigid3d::Translation(
+          Eigen::Vector3d(origin.x(), origin.y(), 0.))),
       probability_grid(limits) {}
 
 Submaps::Submaps(const proto::SubmapsOptions& options)
@@ -137,7 +138,7 @@ int Submaps::size() const { return submaps_.size(); }
 void Submaps::SubmapToProto(
     const int index, const transform::Rigid3d&,
     mapping::proto::SubmapQuery::Response* const response) const {
-  AddProbabilityGridToResponse(Get(index)->local_pose(),
+  AddProbabilityGridToResponse(Get(index)->local_pose,
                                Get(index)->probability_grid, response);
 }
 

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -143,7 +143,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
   const Submap* const matching_submap =
       submaps_->Get(submaps_->matching_index());
   transform::Rigid3d initial_ceres_pose =
-      matching_submap->local_pose().inverse() * pose_prediction;
+      matching_submap->local_pose.inverse() * pose_prediction;
   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
       options_.high_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud filtered_point_cloud_in_tracking =
@@ -171,7 +171,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
                                &matching_submap->low_resolution_hybrid_grid}},
                              &pose_observation_in_submap, &summary);
   const transform::Rigid3d pose_observation =
-      matching_submap->local_pose() * pose_observation_in_submap;
+      matching_submap->local_pose * pose_observation_in_submap;
   pose_tracker_->AddPoseObservation(
       time, pose_observation,
       options_.kalman_local_trajectory_builder_options()
@@ -227,8 +227,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
-  submaps_->InsertRangeData(sensor::TransformRangeData(
+  submaps_->InsertRangeData(
-      range_data_in_tracking, pose_observation.cast<float>()));
+      sensor::TransformRangeData(range_data_in_tracking,
+                                 pose_observation.cast<float>()),
+      pose_tracker_->gravity_orientation());
   return std::unique_ptr<InsertionResult>(new InsertionResult{
       time, range_data_in_tracking, pose_observation, covariance_estimate,
       matching_submap, insertion_submaps});

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -242,7 +242,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
   // We transform the states in 'batches_' in place to be in the submap frame as
   // expected by the OccupiedSpaceCostFunctor. This is reverted after solving
   // the optimization problem.
-  TransformStates(matching_submap->local_pose().inverse());
+  TransformStates(matching_submap->local_pose.inverse());
   for (size_t i = 0; i < batches_.size(); ++i) {
     Batch& batch = batches_[i];
     problem.AddResidualBlock(
@@ -351,7 +351,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
   ceres::Solve(ceres_solver_options_, &problem, &summary);
   // The optimized states in 'batches_' are in the submap frame and we transform
   // them in place to be in the local SLAM frame again.
-  TransformStates(matching_submap->local_pose());
+  TransformStates(matching_submap->local_pose);
   if (num_accumulated_ < options_.scans_per_accumulation()) {
     return nullptr;
   }
@@ -416,8 +416,13 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
-  submaps_->InsertRangeData(sensor::TransformRangeData(
+  // TODO(whess): Use an ImuTracker to track the gravity direction.
-      range_data_in_tracking, pose_observation.cast<float>()));
+  const Eigen::Quaterniond kFakeGravityOrientation =
+      Eigen::Quaterniond::Identity();
+  submaps_->InsertRangeData(
+      sensor::TransformRangeData(range_data_in_tracking,
+                                 pose_observation.cast<float>()),
+      kFakeGravityOrientation);
 
   const kalman_filter::PoseCovariance kCovariance =
       1e-7 * kalman_filter::PoseCovariance::Identity();

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -81,8 +81,8 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
         submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose;
     optimization_problem_.AddSubmap(
         trajectory_id, first_submap_pose *
-                           insertion_submaps[0]->local_pose().inverse() *
+                           insertion_submaps[0]->local_pose.inverse() *
-                           insertion_submaps[1]->local_pose());
+                           insertion_submaps[1]->local_pose);
   }
 }
 
@@ -164,6 +164,12 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
           .at(submap_id.submap_index)
           .pose.inverse();
 
+  const transform::Rigid3d initial_relative_pose =
+      inverse_submap_pose * optimization_problem_.node_data()
+                                .at(node_id.trajectory_id)
+                                .at(node_id.node_index)
+                                .point_cloud_pose;
+
   std::vector<mapping::TrajectoryNode> submap_nodes;
   for (const mapping::NodeId& submap_node_id :
        submap_states_.at(submap_id.trajectory_id)
@@ -181,6 +187,18 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
   // Only globally match against submaps not in this trajectory.
   if (node_id.trajectory_id != submap_id.trajectory_id &&
       global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
+    // In this situation, 'initial_relative_pose' is:
+    //
+    // submap <- global map 2 <- global map 1 <- tracking
+    //               (agreeing on gravity)
+    //
+    // Since they possibly came from two disconnected trajectories, the only
+    // guaranteed connection between the tracking and the submap frames is
+    // an agreement on the direction of gravity. Therefore, excluding yaw,
+    // 'initial_relative_pose.rotation()' is a good estimate of the relative
+    // orientation of the point cloud in the submap frame. Finding the correct
+    // yaw component will be handled by the matching procedure in the
+    // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
     constraint_builder_.MaybeAddGlobalConstraint(
         submap_id,
         submap_states_.at(submap_id.trajectory_id)
@@ -190,7 +208,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         &trajectory_nodes_.at(node_id.trajectory_id)
              .at(node_id.node_index)
              .constant_data->range_data_3d.returns,
-        submap_nodes, &trajectory_connectivity_);
+        submap_nodes, initial_relative_pose.rotation(),
+        &trajectory_connectivity_);
   } else {
     const bool scan_and_submap_trajectories_connected =
         reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
@@ -199,11 +218,6 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
             reverse_connected_components_.at(submap_id.trajectory_id);
     if (node_id.trajectory_id == submap_id.trajectory_id ||
         scan_and_submap_trajectories_connected) {
-      const transform::Rigid3d initial_relative_pose =
-          inverse_submap_pose * optimization_problem_.node_data()
-                                    .at(node_id.trajectory_id)
-                                    .at(node_id.node_index)
-                                    .point_cloud_pose;
       constraint_builder_.MaybeAddConstraint(
           submap_id,
           submap_states_.at(submap_id.trajectory_id)
@@ -242,7 +256,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
           .at(matching_id.trajectory_id)
           .at(matching_id.submap_index)
           .pose *
-      matching_submap->local_pose().inverse() * pose;
+      matching_submap->local_pose.inverse() * pose;
   CHECK_EQ(scan_index, scan_index_to_node_id_.size());
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
@@ -266,7 +280,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
         .node_ids.emplace(node_id);
     // Unchanged covariance as (submap <- map) is a translation.
     const transform::Rigid3d constraint_transform =
-        submap->local_pose().inverse() * pose;
+        submap->local_pose.inverse() * pose;
     constraints_.push_back(
         Constraint{submap_id,
                    node_id,
@@ -447,8 +461,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
   return extrapolated_submap_transforms.back() *
          submap_states_.at(trajectory_id)
              .at(extrapolated_submap_transforms.size() - 1)
-             .submap->local_pose()
+             .submap->local_pose.inverse();
-             .inverse();
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@@ -482,14 +495,13 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     } else if (result.empty()) {
       result.push_back(transform::Rigid3d::Identity());
     } else {
-      // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
+      // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
       // is okay, since the member is const.
       result.push_back(result.back() *
                        submap_states_.at(trajectory_id)
                            .at(result.size() - 1)
-                           .submap->local_pose()
+                           .submap->local_pose.inverse() *
-                           .inverse() *
+                       state.submap->local_pose);
-                       state.submap->local_pose());
     }
   }
 

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -88,6 +88,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::NodeId& node_id,
     const sensor::CompressedPointCloud* const compressed_point_cloud,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
+    const Eigen::Quaterniond& gravity_alignment,
     mapping::TrajectoryConnectivity* const trajectory_connectivity) {
   common::MutexLocker locker(&mutex_);
   constraints_.emplace_back();
@@ -97,10 +98,10 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
       [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_id, submap, node_id,
+        ComputeConstraint(
-                          true, /* match_full_submap */
+            submap_id, submap, node_id, true, /* match_full_submap */
             trajectory_connectivity, compressed_point_cloud,
-                          transform::Rigid3d::Identity(), constraint);
+            transform::Rigid3d::Rotation(gravity_alignment), constraint);
         FinishComputation(current_computation);
       });
 }

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -84,7 +84,9 @@ class ConstraintBuilder {
   // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
   // This performs full-submap matching.
   //
-  // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
+  // The 'gravity_alignment' is the rotation to apply to the point cloud data
+  // to make it approximately gravity aligned. The 'trajectory_connectivity' is
+  // updated if the full-submap match succeeds.
   //
   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
@@ -93,6 +95,7 @@ class ConstraintBuilder {
       const mapping::NodeId& node_id,
       const sensor::CompressedPointCloud* compressed_point_cloud,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
+      const Eigen::Quaterniond& gravity_alignment,
       mapping::TrajectoryConnectivity* trajectory_connectivity);
 
   // Must be called after all computations related to one node have been added.

cartographer/mapping_3d/submaps.cc

@@ -217,17 +217,20 @@ proto::SubmapsOptions CreateSubmapsOptions(
 }
 
 Submap::Submap(const float high_resolution, const float low_resolution,
-               const Eigen::Vector3f& origin)
+               const transform::Rigid3d& local_pose)
-    : mapping::Submap(origin),
+    : mapping::Submap(local_pose),
       high_resolution_hybrid_grid(high_resolution),
       low_resolution_hybrid_grid(low_resolution) {}
 
 Submaps::Submaps(const proto::SubmapsOptions& options)
     : options_(options),
       range_data_inserter_(options.range_data_inserter_options()) {
-  // We always want to have at least one likelihood field which we can return,
+  // We always want to have at least one submap which we can return and will
-  // and will create it at the origin in absence of a better choice.
+  // create it at the origin in absence of a better choice.
-  AddSubmap(Eigen::Vector3f::Zero());
+  //
+  // TODO(whess): Start with no submaps, so that all of them can be
+  // approximately gravity aligned.
+  AddSubmap(transform::Rigid3d::Identity());
 }
 
 const Submap* Submaps::Get(int index) const {
@@ -271,11 +274,12 @@ void Submaps::SubmapToProto(
                              global_submap_pose.translation().z())));
 }
 
-void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
+void Submaps::InsertRangeData(const sensor::RangeData& range_data,
+                              const Eigen::Quaterniond& gravity_alignment) {
   for (const int index : insertion_indices()) {
     Submap* submap = submaps_[index].get();
     const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
-        range_data, submap->local_pose().inverse().cast<float>());
+        range_data, submap->local_pose.inverse().cast<float>());
     range_data_inserter_.Insert(
         FilterRangeDataByMaxRange(transformed_range_data,
                                   options_.high_resolution_max_range()),
@@ -286,18 +290,19 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
   }
   const Submap* const last_submap = Get(size() - 1);
   if (last_submap->num_range_data == options_.num_range_data()) {
-    AddSubmap(range_data.origin);
+    AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
+                                 gravity_alignment));
   }
 }
 
-void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
+void Submaps::AddSubmap(const transform::Rigid3d& local_pose) {
   if (size() > 1) {
     Submap* submap = submaps_[size() - 2].get();
     CHECK(!submap->finished);
     submap->finished = true;
   }
   submaps_.emplace_back(new Submap(options_.high_resolution(),
-                                   options_.low_resolution(), origin));
+                                   options_.low_resolution(), local_pose));
   LOG(INFO) << "Added submap " << size();
 }
 

cartographer/mapping_3d/submaps.h

@@ -32,6 +32,7 @@
 #include "cartographer/mapping_3d/proto/submaps_options.pb.h"
 #include "cartographer/mapping_3d/range_data_inserter.h"
 #include "cartographer/sensor/range_data.h"
+#include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 
 namespace cartographer {
@@ -48,7 +49,7 @@ proto::SubmapsOptions CreateSubmapsOptions(
 
 struct Submap : public mapping::Submap {
   Submap(float high_resolution, float low_resolution,
-         const Eigen::Vector3f& origin);
+         const transform::Rigid3d& local_pose);
 
   HybridGrid high_resolution_hybrid_grid;
   HybridGrid low_resolution_hybrid_grid;
@@ -69,8 +70,11 @@ class Submaps : public mapping::Submaps {
       int index, const transform::Rigid3d& global_submap_pose,
       mapping::proto::SubmapQuery::Response* response) const override;
 
-  // Inserts 'range_data' into the Submap collection.
+  // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
-  void InsertRangeData(const sensor::RangeData& range_data);
+  // used for the orientation of new submaps so that the z axis approximately
+  // aligns with gravity.
+  void InsertRangeData(const sensor::RangeData& range_data,
+                       const Eigen::Quaterniond& gravity_alignment);
 
  private:
   struct PixelData {
@@ -81,7 +85,7 @@ class Submaps : public mapping::Submaps {
     float max_probability = 0.5f;
   };
 
-  void AddSubmap(const Eigen::Vector3f& origin);
+  void AddSubmap(const transform::Rigid3d& local_pose);
 
   std::vector<PixelData> AccumulatePixelData(
       const int width, const int height, const Eigen::Array2i& min_index,