Fix 3D yaw rotation in GenerateDiscreteScans. (#640)

Correctly generates discrete scans rotating around yaw in the gravity-aligned global map frame.
Fixes #639.
PAIR=wohe

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -157,7 +157,8 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
 FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
 
 bool FastCorrelativeScanMatcher::Match(
-    const transform::Rigid3d& initial_pose_estimate,
+    const transform::Rigid3d& global_node_pose,
+    const transform::Rigid3d& global_submap_pose,
     const mapping::TrajectoryNode::Data& constant_data, const float min_score,
     float* const score, transform::Rigid3d* const pose_estimate,
     float* const rotational_score, float* const low_resolution_score) const {
@@ -168,7 +169,8 @@ bool FastCorrelativeScanMatcher::Match(
       common::RoundToInt(options_.linear_z_search_window() / resolution_),
       options_.angular_search_window(), &low_resolution_matcher};
   return MatchWithSearchParameters(
-      search_parameters, initial_pose_estimate,
+      search_parameters, global_node_pose.cast<float>(),
+      global_submap_pose.cast<float>(),
       constant_data.high_resolution_point_cloud,
       constant_data.rotational_scan_matcher_histogram,
       constant_data.gravity_alignment, min_score, score, pose_estimate,
@@ -176,12 +178,11 @@ bool FastCorrelativeScanMatcher::Match(
 }
 
 bool FastCorrelativeScanMatcher::MatchFullSubmap(
-    const Eigen::Quaterniond& gravity_alignment,
+    const Eigen::Quaterniond& global_node_rotation,
+    const Eigen::Quaterniond& global_submap_rotation,
     const mapping::TrajectoryNode::Data& constant_data, const float min_score,
     float* const score, transform::Rigid3d* const pose_estimate,
     float* const rotational_score, float* const low_resolution_score) const {
-  const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
-                                                 gravity_alignment);
   float max_point_distance = 0.f;
   for (const Eigen::Vector3f& point :
        constant_data.high_resolution_point_cloud) {
@@ -195,7 +196,9 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
   const SearchParameters search_parameters{
       linear_window_size, linear_window_size, M_PI, &low_resolution_matcher};
   return MatchWithSearchParameters(
-      search_parameters, initial_pose_estimate,
+      search_parameters,
+      transform::Rigid3f::Rotation(global_node_rotation.cast<float>()),
+      transform::Rigid3f::Rotation(global_submap_rotation.cast<float>()),
       constant_data.high_resolution_point_cloud,
       constant_data.rotational_scan_matcher_histogram,
       constant_data.gravity_alignment, min_score, score, pose_estimate,
@@ -204,7 +207,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
 
 bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
     const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
-    const transform::Rigid3d& initial_pose_estimate,
+    const transform::Rigid3f& global_node_pose,
+    const transform::Rigid3f& global_submap_pose,
     const sensor::PointCloud& point_cloud,
     const Eigen::VectorXf& rotational_scan_matcher_histogram,
     const Eigen::Quaterniond& gravity_alignment, const float min_score,
@@ -215,7 +219,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
 
   const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
       search_parameters, point_cloud, rotational_scan_matcher_histogram,
-      gravity_alignment, initial_pose_estimate.cast<float>());
+      gravity_alignment, global_node_pose, global_submap_pose);
 
   const std::vector<Candidate> lowest_resolution_candidates =
       ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
@@ -284,7 +288,8 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
     const sensor::PointCloud& point_cloud,
     const Eigen::VectorXf& rotational_scan_matcher_histogram,
     const Eigen::Quaterniond& gravity_alignment,
-    const transform::Rigid3f& initial_pose) const {
+    const transform::Rigid3f& global_node_pose,
+    const transform::Rigid3f& global_submap_pose) const {
   std::vector<DiscreteScan> result;
   // We set this value to something on the order of resolution to make sure that
   // the std::acos() below is defined.
@@ -299,15 +304,15 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
                                           (2.f * common::Pow2(max_scan_range)));
   const int angular_window_size = common::RoundToInt(
       search_parameters.angular_search_window / angular_step_size);
-  // TODO(whess): Should there be a small search window for rotations around
-  // x and y?
   std::vector<float> angles;
   for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) {
     angles.push_back(rz * angular_step_size);
   }
+  const transform::Rigid3f node_to_submap =
+      global_submap_pose.inverse() * global_node_pose;
   const std::vector<float> scores = rotational_scan_matcher_.Match(
       rotational_scan_matcher_histogram,
-      transform::GetYaw(initial_pose.rotation() *
+      transform::GetYaw(node_to_submap.rotation() *
                         gravity_alignment.inverse().cast<float>()),
       angles);
   for (size_t i = 0; i != angles.size(); ++i) {
@@ -319,9 +324,10 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
     // and rotation of the 'initial_pose', so that the rotation is around the
     // origin of the range data, and yaw is in map frame.
     const transform::Rigid3f pose(
-        initial_pose.translation(),
+        node_to_submap.translation(),
+        global_submap_pose.rotation().inverse() *
             transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
-            initial_pose.rotation());
+            global_node_pose.rotation());
     result.push_back(
         DiscretizeScan(search_parameters, point_cloud, pose, scores[i]));
   }

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -61,20 +61,23 @@ class FastCorrelativeScanMatcher {
       delete;
 
   // Aligns the node with the given 'constant_data' within the 'hybrid_grid'
-  // given an 'initial_pose_estimate'. If a score above 'min_score' (excluding
+  // given 'global_node_pose' and 'global_submap_pose'. If a score above
-  // equality) is possible, true is returned, and 'score', 'pose_estimate',
+  // 'min_score' (excluding equality) is possible, true is returned, and
-  // 'rotational_score', and 'low_resolution_score' are updated with the result.
+  // 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score'
-  bool Match(const transform::Rigid3d& initial_pose_estimate,
+  // are updated with the result.
+  bool Match(const transform::Rigid3d& global_node_pose,
+             const transform::Rigid3d& global_submap_pose,
              const mapping::TrajectoryNode::Data& constant_data,
              float min_score, float* score, transform::Rigid3d* pose_estimate,
              float* rotational_score, float* low_resolution_score) const;
 
   // Aligns the node with the given 'constant_data' within the 'hybrid_grid'
-  // given a rotation which is expected to be approximately gravity aligned.
+  // given rotations which are expected to be approximately gravity aligned.
   // If a score above 'min_score' (excluding equality) is possible, true is
   // returned, and 'score', 'pose_estimate', 'rotational_score', and
   // 'low_resolution_score' are updated with the result.
-  bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
+  bool MatchFullSubmap(const Eigen::Quaterniond& global_node_rotation,
+                       const Eigen::Quaterniond& global_submap_rotation,
                        const mapping::TrajectoryNode::Data& constant_data,
                        float min_score, float* score,
                        transform::Rigid3d* pose_estimate,
@@ -91,7 +94,8 @@ class FastCorrelativeScanMatcher {
 
   bool MatchWithSearchParameters(
       const SearchParameters& search_parameters,
-      const transform::Rigid3d& initial_pose_estimate,
+      const transform::Rigid3f& global_node_pose,
+      const transform::Rigid3f& global_submap_pose,
       const sensor::PointCloud& point_cloud,
       const Eigen::VectorXf& rotational_scan_matcher_histogram,
       const Eigen::Quaterniond& gravity_alignment, float min_score,
@@ -106,7 +110,8 @@ class FastCorrelativeScanMatcher {
       const sensor::PointCloud& point_cloud,
       const Eigen::VectorXf& rotational_scan_matcher_histogram,
       const Eigen::Quaterniond& gravity_alignment,
-      const transform::Rigid3f& initial_pose) const;
+      const transform::Rigid3f& global_node_pose,
+      const transform::Rigid3f& global_submap_pose) const;
   std::vector<Candidate> GenerateLowestResolutionCandidates(
       const SearchParameters& search_parameters, int num_discrete_scans) const;
   void ScoreCandidates(int depth,

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -145,9 +145,9 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
     float rotational_score = 0.f;
     float low_resolution_score = 0.f;
     EXPECT_TRUE(fast_correlative_scan_matcher->Match(
-        transform::Rigid3d::Identity(), CreateConstantData(point_cloud_),
+        transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
-        kMinScore, &score, &pose_estimate, &rotational_score,
+        CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate,
-        &low_resolution_score));
+        &rotational_score, &low_resolution_score));
     EXPECT_LT(kMinScore, score);
     EXPECT_LT(0.09f, rotational_score);
     EXPECT_LT(0.14f, low_resolution_score);
@@ -156,7 +156,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
         << "Actual: " << transform::ToProto(pose_estimate).DebugString()
         << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
     EXPECT_FALSE(fast_correlative_scan_matcher->Match(
-        transform::Rigid3d::Identity(),
+        transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
         CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
         &score, &pose_estimate, &rotational_score, &low_resolution_score))
         << low_resolution_score;
@@ -174,9 +174,9 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
   float rotational_score = 0.f;
   float low_resolution_score = 0.f;
   EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap(
-      Eigen::Quaterniond::Identity(), CreateConstantData(point_cloud_),
+      Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
-      kMinScore, &score, &pose_estimate, &rotational_score,
+      CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate,
-      &low_resolution_score));
+      &rotational_score, &low_resolution_score));
   EXPECT_LT(kMinScore, score);
   EXPECT_LT(0.09f, rotational_score);
   EXPECT_LT(0.14f, low_resolution_score);
@@ -185,7 +185,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
       << "Actual: " << transform::ToProto(pose_estimate).DebugString()
       << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
   EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap(
-      Eigen::Quaterniond::Identity(),
+      Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
       CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
       &score, &pose_estimate, &rotational_score, &low_resolution_score))
       << low_resolution_score;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -179,19 +179,21 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                         const mapping::SubmapId& submap_id) {
   CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
 
-  const transform::Rigid3d inverse_submap_pose =
+  const transform::Rigid3d global_node_pose =
-      optimization_problem_.submap_data().at(submap_id).pose.inverse();
-
-  const transform::Rigid3d initial_relative_pose =
-      inverse_submap_pose *
       optimization_problem_.node_data().at(node_id).global_pose;
 
+  const transform::Rigid3d global_submap_pose =
+      optimization_problem_.submap_data().at(submap_id).pose;
+
+  const transform::Rigid3d global_submap_pose_inverse =
+      global_submap_pose.inverse();
+
   std::vector<mapping::TrajectoryNode> submap_nodes;
   for (const mapping::NodeId& submap_node_id :
        submap_data_.at(submap_id).node_ids) {
     submap_nodes.push_back(mapping::TrajectoryNode{
         trajectory_nodes_.at(submap_node_id).constant_data,
-        inverse_submap_pose *
+        global_submap_pose_inverse *
             trajectory_nodes_.at(submap_node_id).global_pose});
   }
 
@@ -211,24 +213,17 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
     constraint_builder_.MaybeAddConstraint(
         submap_id, submap_data_.at(submap_id).submap.get(), node_id,
         trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
-        initial_relative_pose);
+        global_node_pose, global_submap_pose);
   } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
-    // In this situation, 'initial_relative_pose' is:
+    // In this situation, 'global_node_pose' and 'global_submap_pose' have
-    //
+    // orientations agreeing on gravity. Their relationship regarding yaw is
-    // submap <- global map 2 <- global map 1 <- tracking
+    // arbitrary. Finding the correct yaw component will be handled by the
-    //               (agreeing on gravity)
+    // matching procedure in the FastCorrelativeScanMatcher, and the given yaw
-    //
+    // is essentially ignored.
-    // 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_data_.at(submap_id).submap.get(), node_id,
         trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
-        initial_relative_pose.rotation());
+        global_node_pose.rotation(), global_submap_pose.rotation());
   }
 }
 

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -59,8 +59,10 @@ void ConstraintBuilder::MaybeAddConstraint(
     const mapping::NodeId& node_id,
     const mapping::TrajectoryNode::Data* const constant_data,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
-    const transform::Rigid3d& initial_pose) {
+    const transform::Rigid3d& global_node_pose,
-  if (initial_pose.translation().norm() > options_.max_constraint_distance()) {
+    const transform::Rigid3d& global_submap_pose) {
+  if ((global_node_pose.translation() - global_submap_pose.translation())
+          .norm() > options_.max_constraint_distance()) {
     return;
   }
   if (sampler_.Pulse()) {
@@ -72,7 +74,8 @@ void ConstraintBuilder::MaybeAddConstraint(
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
         submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
           ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
-                            constant_data, initial_pose, constraint);
+                            constant_data, global_node_pose, global_submap_pose,
+                            constraint);
           FinishComputation(current_computation);
         });
   }
@@ -83,7 +86,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::NodeId& node_id,
     const mapping::TrajectoryNode::Data* const constant_data,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
-    const Eigen::Quaterniond& gravity_alignment) {
+    const Eigen::Quaterniond& global_node_rotation,
+    const Eigen::Quaterniond& global_submap_rotation) {
   common::MutexLocker locker(&mutex_);
   constraints_.emplace_back();
   auto* const constraint = &constraints_.back();
@@ -91,10 +95,10 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   const int current_computation = current_computation_;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
+        ComputeConstraint(
-                          constant_data,
+            submap_id, node_id, true, /* match_full_submap */
-                          transform::Rigid3d::Rotation(gravity_alignment),
+            constant_data, transform::Rigid3d::Rotation(global_node_rotation),
-                          constraint);
+            transform::Rigid3d::Rotation(global_submap_rotation), constraint);
         FinishComputation(current_computation);
       });
 }
@@ -166,7 +170,8 @@ void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
     bool match_full_submap,
     const mapping::TrajectoryNode::Data* const constant_data,
-    const transform::Rigid3d& initial_pose,
+    const transform::Rigid3d& global_node_pose,
+    const transform::Rigid3d& global_submap_pose,
     std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_id);
@@ -178,6 +183,7 @@ void ConstraintBuilder::ComputeConstraint(
   transform::Rigid3d pose_estimate;
   float rotational_score = 0.f;
   float low_resolution_score = 0.f;
+  // TODO(gaschler): Match methods should return unique_ptr<struct>.
 
   // Compute 'pose_estimate' in three stages:
   // 1. Fast estimate using the fast correlative scan matcher.
@@ -185,9 +191,9 @@ void ConstraintBuilder::ComputeConstraint(
   // 3. Refine.
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
-            initial_pose.rotation(), *constant_data,
+            global_node_pose.rotation(), global_submap_pose.rotation(),
-            options_.global_localization_min_score(), &score, &pose_estimate,
+            *constant_data, options_.global_localization_min_score(), &score,
-            &rotational_score, &low_resolution_score)) {
+            &pose_estimate, &rotational_score, &low_resolution_score)) {
       CHECK_GT(score, options_.global_localization_min_score());
       CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
@@ -196,8 +202,9 @@ void ConstraintBuilder::ComputeConstraint(
     }
   } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
-            initial_pose, *constant_data, options_.min_score(), &score,
+            global_node_pose, global_submap_pose, *constant_data,
-            &pose_estimate, &rotational_score, &low_resolution_score)) {
+            options_.min_score(), &score, &pose_estimate, &rotational_score,
+            &low_resolution_score)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
     } else {
@@ -238,8 +245,9 @@ void ConstraintBuilder::ComputeConstraint(
     if (match_full_submap) {
       info << " matches";
     } else {
-      const transform::Rigid3d difference =
+      const transform::Rigid3d difference = global_submap_pose.inverse() *
-          initial_pose.inverse() * constraint_transform;
+                                            global_node_pose *
+                                            constraint_transform;
       info << " differs by translation " << std::setprecision(2)
            << difference.translation().norm() << " rotation "
            << std::setprecision(3) << transform::GetAngle(difference);

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -68,7 +68,9 @@ class ConstraintBuilder {
 
   // Schedules exploring a new constraint between 'submap' identified by
   // 'submap_id', and the 'compressed_point_cloud' for 'node_id'.
-  // The 'initial_pose' is relative to the 'submap'.
+  //
+  // 'global_node_pose' and 'global_submap_pose' are initial estimates of poses
+  // in the global map frame, i.e. both are gravity aligned.
   //
   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
@@ -77,14 +79,15 @@ class ConstraintBuilder {
       const mapping::NodeId& node_id,
       const mapping::TrajectoryNode::Data* const constant_data,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
-      const transform::Rigid3d& initial_pose);
+      const transform::Rigid3d& global_node_pose,
+      const transform::Rigid3d& global_submap_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
   // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
   // This performs full-submap matching.
   //
-  // The 'gravity_alignment' is the rotation to apply to the point cloud data
+  // 'global_node_rotation' and 'global_submap_rotation' are initial estimates
-  // to make it approximately gravity aligned.
+  // of roll and pitch, i.e. their yaw is essentially ignored.
   //
   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
@@ -93,7 +96,8 @@ class ConstraintBuilder {
       const mapping::NodeId& node_id,
       const mapping::TrajectoryNode::Data* const constant_data,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
-      const Eigen::Quaterniond& gravity_alignment);
+      const Eigen::Quaterniond& global_node_rotation,
+      const Eigen::Quaterniond& global_submap_rotation);
 
   // Must be called after all computations related to one node have been added.
   void NotifyEndOfScan();
@@ -141,7 +145,8 @@ class ConstraintBuilder {
       const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
       bool match_full_submap,
       const mapping::TrajectoryNode::Data* const constant_data,
-      const transform::Rigid3d& initial_pose,
+      const transform::Rigid3d& global_node_pose,
+      const transform::Rigid3d& global_submap_pose,
       std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
 
   // Decrements the 'pending_computations_' count. If all computations are done,