Add rotational score histograms in 3D. (#403)

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -106,21 +106,23 @@ bool FastCorrelativeScanMatcher::Match(
     const transform::Rigid3d& initial_pose_estimate,
     const sensor::PointCloud& coarse_point_cloud,
     const sensor::PointCloud& fine_point_cloud, const float min_score,
-    float* const score, transform::Rigid3d* const pose_estimate) const {
+    float* const score, transform::Rigid3d* const pose_estimate,
+    float* const rotational_score) const {
   const SearchParameters search_parameters{
       common::RoundToInt(options_.linear_xy_search_window() / resolution_),
       common::RoundToInt(options_.linear_z_search_window() / resolution_),
       options_.angular_search_window()};
-  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
+  return MatchWithSearchParameters(
-                                   coarse_point_cloud, fine_point_cloud,
+      search_parameters, initial_pose_estimate, coarse_point_cloud,
-                                   min_score, score, pose_estimate);
+      fine_point_cloud, min_score, score, pose_estimate, rotational_score);
 }
 
 bool FastCorrelativeScanMatcher::MatchFullSubmap(
     const Eigen::Quaterniond& gravity_alignment,
     const sensor::PointCloud& coarse_point_cloud,
     const sensor::PointCloud& fine_point_cloud, const float min_score,
-    float* const score, transform::Rigid3d* const pose_estimate) const {
+    float* const score, transform::Rigid3d* const pose_estimate,
+    float* const rotational_score) const {
   const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
                                                  gravity_alignment);
   float max_point_distance = 0.f;
@@ -132,9 +134,9 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
       common::RoundToInt(max_point_distance / resolution_ + 0.5f);
   const SearchParameters search_parameters{linear_window_size,
                                            linear_window_size, M_PI};
-  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
+  return MatchWithSearchParameters(
-                                   coarse_point_cloud, fine_point_cloud,
+      search_parameters, initial_pose_estimate, coarse_point_cloud,
-                                   min_score, score, pose_estimate);
+      fine_point_cloud, min_score, score, pose_estimate, rotational_score);
 }
 
 bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
@@ -142,7 +144,8 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
     const transform::Rigid3d& initial_pose_estimate,
     const sensor::PointCloud& coarse_point_cloud,
     const sensor::PointCloud& fine_point_cloud, const float min_score,
-    float* const score, transform::Rigid3d* const pose_estimate) const {
+    float* const score, transform::Rigid3d* const pose_estimate,
+    float* const rotational_score) const {
   CHECK_NOTNULL(score);
   CHECK_NOTNULL(pose_estimate);
 
@@ -158,12 +161,14 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
       precomputation_grid_stack_->max_depth(), min_score);
   if (best_candidate.score > min_score) {
     *score = best_candidate.score;
+    const auto& discrete_scan = discrete_scans[best_candidate.scan_index];
     *pose_estimate =
         (transform::Rigid3f(
              resolution_ * best_candidate.offset.matrix().cast<float>(),
              Eigen::Quaternionf::Identity()) *
-         discrete_scans[best_candidate.scan_index].pose)
+         discrete_scan.pose)
             .cast<double>();
+    *rotational_score = discrete_scan.rotational_score;
     return true;
   }
   return false;
@@ -171,8 +176,8 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
 
 DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
     const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
-    const sensor::PointCloud& point_cloud,
+    const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose,
-    const transform::Rigid3f& pose) const {
+    const float rotational_score) const {
   std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
   const PrecomputationGrid& original_grid = precomputation_grid_stack_->Get(0);
   std::vector<Eigen::Array3i> full_resolution_cell_indices;
@@ -210,7 +215,7 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
           low_resolution_cell_at_start - low_resolution_search_window_start);
     }
   }
-  return DiscreteScan{pose, cell_indices_per_depth};
+  return DiscreteScan{pose, cell_indices_per_depth, rotational_score};
 }
 
 std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
@@ -253,7 +258,7 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
         transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
             initial_pose.rotation());
     result.push_back(
-        DiscretizeScan(search_parameters, coarse_point_cloud, pose));
+        DiscretizeScan(search_parameters, coarse_point_cloud, pose, scores[i]));
   }
   return result;
 }

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -46,6 +46,7 @@ struct DiscreteScan {
   transform::Rigid3f pose;
   // Contains a vector of discretized scans for each 'depth'.
   std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
+  float rotational_score;
 };
 
 struct Candidate {
@@ -81,24 +82,27 @@ class FastCorrelativeScanMatcher {
 
   // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given an
   // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
-  // is possible, true is returned, and 'score' and 'pose_estimate' are updated
+  // is possible, true is returned, and 'score', 'pose_estimate', and
-  // with the result. 'fine_point_cloud' is used to compute the rotational scan
+  // 'rotational_score' are updated with the result. 'fine_point_cloud' is used
-  // matcher score.
+  // to compute the rotational scan matcher score.
   bool Match(const transform::Rigid3d& initial_pose_estimate,
              const sensor::PointCloud& coarse_point_cloud,
              const sensor::PointCloud& fine_point_cloud, float min_score,
-             float* score, transform::Rigid3d* pose_estimate) const;
+             float* score, transform::Rigid3d* pose_estimate,
+             float* rotational_score) const;
 
   // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which
   // is expected to be approximately gravity aligned. If a score above
-  // 'min_score' (excluding equality) is possible, true is returned, and 'score'
+  // 'min_score' (excluding equality) is possible, true is returned, and
-  // and 'pose_estimate' are updated with the result. 'fine_point_cloud' is used
+  // 'score', 'pose_estimate', and 'rotational_score' are updated with the
-  // to compute the rotational scan matcher score.
+  // result. 'fine_point_cloud' is used to compute the rotational scan matcher
+  // score.
   bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
                        const sensor::PointCloud& coarse_point_cloud,
                        const sensor::PointCloud& fine_point_cloud,
                        float min_score, float* score,
-                       transform::Rigid3d* pose_estimate) const;
+                       transform::Rigid3d* pose_estimate,
+                       float* rotational_score) const;
 
  private:
   struct SearchParameters {
@@ -112,10 +116,11 @@ class FastCorrelativeScanMatcher {
       const transform::Rigid3d& initial_pose_estimate,
       const sensor::PointCloud& coarse_point_cloud,
       const sensor::PointCloud& fine_point_cloud, float min_score, float* score,
-      transform::Rigid3d* pose_estimate) const;
+      transform::Rigid3d* pose_estimate, float* rotational_score) const;
   DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
                               const sensor::PointCloud& point_cloud,
-                              const transform::Rigid3f& pose) const;
+                              const transform::Rigid3f& pose,
+                              float rotational_score) const;
   std::vector<DiscreteScan> GenerateDiscreteScans(
       const SearchParameters& search_parameters,
       const sensor::PointCloud& coarse_point_cloud,

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -98,12 +98,14 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
 
     FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {},
                                                              options);
+    float score = 0.f;
     transform::Rigid3d pose_estimate;
-    float score;
+    float rotational_score = 0.f;
     EXPECT_TRUE(fast_correlative_scan_matcher.Match(
         transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore,
-        &score, &pose_estimate));
+        &score, &pose_estimate, &rotational_score));
     EXPECT_LT(kMinScore, score);
+    EXPECT_LT(0.09f, rotational_score);
     EXPECT_THAT(expected_pose,
                 transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
         << "Actual: " << transform::ToProto(pose_estimate).DebugString()

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -181,8 +181,9 @@ void ConstraintBuilder::ComputeConstraint(
   // The 'constraint_transform' (submap i <- scan j) is computed from:
   // - a 'filtered_point_cloud' in scan j and
   // - the initial guess 'initial_pose' (submap i <- scan j).
-  float score = 0.;
+  float score = 0.f;
   transform::Rigid3d pose_estimate;
+  float rotational_score = 0.f;
 
   // Compute 'pose_estimate' in three stages:
   // 1. Fast estimate using the fast correlative scan matcher.
@@ -191,7 +192,8 @@ void ConstraintBuilder::ComputeConstraint(
   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)) {
+            options_.global_localization_min_score(), &score, &pose_estimate,
+            &rotational_score)) {
       CHECK_GT(score, options_.global_localization_min_score());
       CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
@@ -203,7 +205,7 @@ void ConstraintBuilder::ComputeConstraint(
   } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
             initial_pose, filtered_point_cloud, point_cloud,
-            options_.min_score(), &score, &pose_estimate)) {
+            options_.min_score(), &score, &pose_estimate, &rotational_score)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
     } else {
@@ -213,6 +215,7 @@ void ConstraintBuilder::ComputeConstraint(
   {
     common::MutexLocker locker(&mutex_);
     score_histogram_.Add(score);
+    rotational_score_histogram_.Add(rotational_score);
   }
 
   // Use the CSM estimate as both the initial and previous pose. This has the
@@ -282,6 +285,8 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
           LOG(INFO) << constraints_.size() << " computations resulted in "
                     << result.size() << " additional constraints.";
           LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10);
+          LOG(INFO) << "Rotational score histogram:\n"
+                    << rotational_score_histogram_.ToString(10);
         }
         constraints_.clear();
         callback = std::move(when_done_);

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -185,8 +185,9 @@ class ConstraintBuilder {
   const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
   scan_matching::CeresScanMatcher ceres_scan_matcher_;
 
-  // Histogram of scan matcher scores.
+  // Histograms of scan matcher scores.
   common::Histogram score_histogram_ GUARDED_BY(mutex_);
+  common::Histogram rotational_score_histogram_ GUARDED_BY(mutex_);
 };
 
 }  // namespace sparse_pose_graph