Fix gravity alignment of submaps in local SLAM (#1398)

We were passing the gravity estimate of the current tracking frame
to intialize the local submap pose. Fixing this improves the alignment
of submaps in the global (and approx. gravity-aligned) frame.

cartographer/mapping/3d/submap_3d.cc

@@ -253,12 +253,13 @@ void Submap3D::ToResponseProto(
                     response->add_textures());
 }
 
-void Submap3D::InsertRangeData(const sensor::RangeData& range_data,
+void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local,
                                const RangeDataInserter3D& range_data_inserter,
                                const float high_resolution_max_range) {
   CHECK(!insertion_finished());
+  // Transform range data into submap frame.
   const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
-      range_data, local_pose().inverse().cast<float>());
+      range_data_in_local, local_pose().inverse().cast<float>());
   range_data_inserter.Insert(
       FilterRangeDataByMaxRange(transformed_range_data,
                                 high_resolution_max_range),
@@ -284,11 +285,11 @@ std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::submaps() const {
 
 std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData(
     const sensor::RangeData& range_data,
-    const Eigen::Quaterniond& gravity_alignment) {
+    const Eigen::Quaterniond& local_from_gravity_aligned) {
   if (submaps_.empty() ||
       submaps_.back()->num_range_data() == options_.num_range_data()) {
     AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
-                                 gravity_alignment));
+                                 local_from_gravity_aligned));
   }
   for (auto& submap : submaps_) {
     submap->InsertRangeData(range_data, range_data_inserter_,

cartographer/mapping/3d/submap_3d.h

@@ -90,12 +90,12 @@ class ActiveSubmaps3D {
   ActiveSubmaps3D(const ActiveSubmaps3D&) = delete;
   ActiveSubmaps3D& operator=(const ActiveSubmaps3D&) = delete;
 
-  // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
+  // Inserts 'range_data_in_local' into the Submap collection.
-  // used for the orientation of new submaps so that the z axis approximately
+  // 'local_from_gravity_aligned' is used for the orientation of new submaps so
-  // aligns with gravity.
+  // that the z axis approximately aligns with gravity.
   std::vector<std::shared_ptr<const Submap3D>> InsertRangeData(
-      const sensor::RangeData& range_data,
+      const sensor::RangeData& range_data_in_local,
-      const Eigen::Quaterniond& gravity_alignment);
+      const Eigen::Quaterniond& local_from_gravity_aligned);
 
   std::vector<std::shared_ptr<const Submap3D>> submaps() const;
 

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -352,9 +352,13 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
   if (motion_filter_.IsSimilar(time, pose_estimate)) {
     return nullptr;
   }
+  const auto local_from_gravity_aligned =
+      pose_estimate.rotation() * gravity_alignment.inverse();
+
   std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
       active_submaps_.InsertRangeData(filtered_range_data_in_local,
-                                      gravity_alignment);
+                                      local_from_gravity_aligned);
+
   const Eigen::VectorXf rotational_scan_matcher_histogram =
       scan_matching::RotationalScanMatcher::ComputeHistogram(
           sensor::TransformPointCloud(

cartographer/mapping/pose_extrapolator.h

@@ -54,7 +54,8 @@ class PoseExtrapolator {
   void AddOdometryData(const sensor::OdometryData& odometry_data);
   transform::Rigid3d ExtrapolatePose(common::Time time);
 
-  // Gravity alignment estimate.
+  // Returns the current gravity alignment estimate as a rotation from
+  // the tracking frame into a gravity aligned frame.
   Eigen::Quaterniond EstimateGravityOrientation(common::Time time);
 
  private: