Add 'gravity_alignment' rotation to each node. (#500)

In 2D this replaces the 'tracking_to_tracking_2d' transform.
Also changes the 2D SparsePoseGraph to get the full 3D pose.

cartographer/mapping/trajectory_node.h

@@ -35,16 +35,17 @@ struct TrajectoryNode {
     // Range data in 'tracking' frame. Only used in 3D.
     sensor::CompressedRangeData range_data;
 
-    // Used for loop closure in 2D: voxel filtered returns in 'pose' frame.
+    // Transform to approximately gravity align the tracking frame as
-    sensor::PointCloud filtered_point_cloud;
+    // determined by local SLAM.
+    Eigen::Quaterniond gravity_alignment;
+
+    // Used for loop closure in 2D: voxel filtered returns in the
+    // 'gravity_alignment' frame.
+    sensor::PointCloud filtered_gravity_aligned_point_cloud;
 
     // Used for loop closure in 3D.
     sensor::PointCloud high_resolution_point_cloud;
     sensor::PointCloud low_resolution_point_cloud;
-
-    // Transforms the 'tracking' frame into a gravity-aligned 'tracking_2d'
-    // frame. Only used in 2D.
-    transform::Rigid3d tracking_to_tracking_2d;
   };
 
   common::Time time() const { return constant_data->time; }

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -181,11 +181,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
           mapping::TrajectoryNode::Data{
               time,
               {},  // 'range_data' is only used in 3D.
+              gravity_alignment.rotation(),
               filtered_gravity_aligned_point_cloud,
               {},  // 'high_resolution_point_cloud' is only used in 3D.
               {},  // 'low_resolution_point_cloud' is only used in 3D.
-              gravity_alignment}),
+          }),
-      pose_estimate_2d, std::move(insertion_submaps)});
+      pose_estimate, std::move(insertion_submaps)});
 }
 
 const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {

cartographer/mapping_2d/local_trajectory_builder.h

@@ -42,7 +42,7 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
-    transform::Rigid2d pose_observation;
+    transform::Rigid3d pose_observation;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -95,11 +95,10 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-    const transform::Rigid2d& pose, const int trajectory_id,
+    const transform::Rigid3d& pose, const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose) *
+      GetLocalToGlobalTransform(trajectory_id) * pose);
-      constant_data->tracking_to_tracking_2d);
 
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
@@ -132,8 +131,11 @@ void SparsePoseGraph::AddScan(
   // execute the lambda.
   const bool newly_finished_submap = insertion_submaps.front()->finished();
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(trajectory_id, insertion_submaps,
+    ComputeConstraintsForScan(
-                              newly_finished_submap, pose);
+        trajectory_id, insertion_submaps, newly_finished_submap,
+        transform::Project2D(pose *
+                             transform::Rigid3d::Rotation(
+                                 constant_data->gravity_alignment.inverse())));
   });
 }
 
@@ -445,9 +447,10 @@ void SparsePoseGraph::RunOptimization() {
     for (; node_data_index != static_cast<int>(node_data[trajectory_id].size());
          ++node_data_index, ++node_index) {
       const mapping::NodeId node_id{trajectory_id, node_index};
-      trajectory_nodes_.at(node_id).pose =
+      auto& node = trajectory_nodes_.at(node_id);
+      node.pose =
           transform::Embed3D(node_data[trajectory_id][node_data_index].pose) *
-          trajectory_nodes_.at(node_id).constant_data->tracking_to_tracking_2d;
+          transform::Rigid3d::Rotation(node.constant_data->gravity_alignment);
     }
     // Extrapolate all point cloud poses that were added later.
     const auto local_to_new_global =
@@ -458,8 +461,8 @@ void SparsePoseGraph::RunOptimization() {
         local_to_new_global * local_to_old_global.inverse();
     for (; node_index < num_nodes; ++node_index) {
       const mapping::NodeId node_id{trajectory_id, node_index};
-      trajectory_nodes_.at(node_id).pose =
+      auto& node_pose = trajectory_nodes_.at(node_id).pose;
-          old_global_to_new_global * trajectory_nodes_.at(node_id).pose;
+      node_pose = old_global_to_new_global * node_pose;
     }
   }
   optimized_submap_transforms_ = submap_data;
@@ -490,8 +493,9 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
     result.push_back(Constraint{
         constraint.submap_id, constraint.node_id,
         Constraint::Pose{constraint.pose.zbar_ij *
+                             transform::Rigid3d::Rotation(
                                  trajectory_nodes_.at(constraint.node_id)
-                                 .constant_data->tracking_to_tracking_2d,
+                                     .constant_data->gravity_alignment),
                          constraint.pose.translation_weight,
                          constraint.pose.rotation_weight},
         constraint.tag});

cartographer/mapping_2d/sparse_pose_graph.h

@@ -71,7 +71,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // 'true', this submap was inserted into for the last time.
   void AddScan(
       std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-      const transform::Rigid2d& pose, int trajectory_id,
+      const transform::Rigid3d& pose, int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -172,7 +172,7 @@ void ConstraintBuilder::ComputeConstraint(
       GetSubmapScanMatcher(submap_id);
 
   // The 'constraint_transform' (submap i <- scan j) is computed from:
-  // - a 'filtered_point_cloud' in scan j,
+  // - a 'filtered_gravity_aligned_point_cloud' in scan j,
   // - the initial guess 'initial_pose' for (map <- scan j),
   // - the result 'pose_estimate' of Match() (map <- scan j).
   // - the ComputeSubmapPose() (map <- submap i)
@@ -185,7 +185,7 @@ void ConstraintBuilder::ComputeConstraint(
   // 3. Refine.
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
-            constant_data->filtered_point_cloud,
+            constant_data->filtered_gravity_aligned_point_cloud,
             options_.global_localization_min_score(), &score, &pose_estimate)) {
       CHECK_GT(score, options_.global_localization_min_score());
       CHECK_GE(node_id.trajectory_id, 0);
@@ -197,7 +197,7 @@ void ConstraintBuilder::ComputeConstraint(
     }
   } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
-            initial_pose, constant_data->filtered_point_cloud,
+            initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
             options_.min_score(), &score, &pose_estimate)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
@@ -214,9 +214,10 @@ void ConstraintBuilder::ComputeConstraint(
   // effect that, in the absence of better information, we prefer the original
   // CSM estimate.
   ceres::Solver::Summary unused_summary;
-  ceres_scan_matcher_.Match(
+  ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
-      pose_estimate, pose_estimate, constant_data->filtered_point_cloud,
+                            constant_data->filtered_gravity_aligned_point_cloud,
-      *submap_scan_matcher->probability_grid, &pose_estimate, &unused_summary);
+                            *submap_scan_matcher->probability_grid,
+                            &pose_estimate, &unused_summary);
 
   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;
@@ -230,8 +231,8 @@ void ConstraintBuilder::ComputeConstraint(
   if (options_.log_matches()) {
     std::ostringstream info;
     info << "Node " << node_id << " with "
-         << constant_data->filtered_point_cloud.size() << " points on submap "
+         << constant_data->filtered_gravity_aligned_point_cloud.size()
-         << submap_id << std::fixed;
+         << " points on submap " << submap_id << std::fixed;
     if (match_full_submap) {
       info << " matches";
     } else {

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -161,11 +161,11 @@ class SparsePoseGraphTest : public ::testing::Test {
         std::make_shared<const mapping::TrajectoryNode::Data>(
             mapping::TrajectoryNode::Data{common::FromUniversal(0),
                                           Compress(range_data),
+                                          Eigen::Quaterniond::Identity(),
                                           range_data.returns,
                                           {},
-                                          {},
+                                          {}}),
-                                          transform::Rigid3d::Identity()}),
+        transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps);
-        pose_estimate, kTrajectoryId, insertion_submaps);
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -210,10 +210,10 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
           mapping::TrajectoryNode::Data{
               time,
               sensor::Compress(range_data_in_tracking),
+              gravity_alignment,
               {},  // 'filtered_point_cloud' is only used in 2D.
               high_resolution_point_cloud,
-              low_resolution_point_cloud,
+              low_resolution_point_cloud}),
-              {}}),  // 'tracking_to_tracking_2d' in only used in 2D.
       pose_observation, std::move(insertion_submaps)});
 }
 

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -114,10 +114,10 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
     return mapping::TrajectoryNode::Data{
         common::FromUniversal(0),
         Compress(sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}),
+        Eigen::Quaterniond::Identity(),
         {},
         point_cloud_,
-        low_resolution_point_cloud,
+        low_resolution_point_cloud};
-        transform::Rigid3d::Identity()};
   }
 
   std::mt19937 prng_ = std::mt19937(42);