Add a histogram of low resolution scores for 3D. (#473)

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -91,6 +91,39 @@ class PrecomputationGridStack {
   std::vector<PrecomputationGrid> precomputation_grids_;
 };
 
+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 {
+  Candidate(const int scan_index, const Eigen::Array3i& offset)
+      : scan_index(scan_index), offset(offset) {}
+
+  static Candidate Unsuccessful() {
+    return Candidate(0, Eigen::Array3i::Zero());
+  }
+
+  // Index into the discrete scans vectors.
+  int scan_index;
+
+  // Linear offset from the initial pose in cell indices. For lower resolution
+  // candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth
+  // block of possibilities.
+  Eigen::Array3i offset;
+
+  // Score, higher is better.
+  float score = -std::numeric_limits<float>::infinity();
+
+  // Score of the low resolution matcher.
+  float low_resolution_score = 0.f;
+
+  bool operator<(const Candidate& other) const { return score < other.score; }
+  bool operator>(const Candidate& other) const { return score > other.score; }
+};
+
 FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
     const HybridGrid& hybrid_grid,
     const std::vector<mapping::TrajectoryNode>& nodes,
@@ -108,25 +141,26 @@ 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,
-    const MatchingFunction& matching_function, float* const score,
+    const MatchingFunction& low_resolution_matcher, float* const score,
-    transform::Rigid3d* const pose_estimate,
+    transform::Rigid3d* const pose_estimate, float* const rotational_score,
-    float* const rotational_score) const {
+    float* const low_resolution_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(), &matching_function};
+      options_.angular_search_window(), &low_resolution_matcher};
-  return MatchWithSearchParameters(
+  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
-      search_parameters, initial_pose_estimate, coarse_point_cloud,
+                                   coarse_point_cloud, fine_point_cloud,
-      fine_point_cloud, min_score, score, pose_estimate, rotational_score);
+                                   min_score, score, pose_estimate,
+                                   rotational_score, low_resolution_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,
-    const MatchingFunction& matching_function, float* const score,
+    const MatchingFunction& low_resolution_matcher, float* const score,
-    transform::Rigid3d* const pose_estimate,
+    transform::Rigid3d* const pose_estimate, float* const rotational_score,
-    float* const rotational_score) const {
+    float* const low_resolution_score) const {
   const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
                                                  gravity_alignment);
   float max_point_distance = 0.f;
@@ -137,10 +171,11 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
       (width_in_voxels_ + 1) / 2 +
       common::RoundToInt(max_point_distance / resolution_ + 0.5f);
   const SearchParameters search_parameters{
-      linear_window_size, linear_window_size, M_PI, &matching_function};
+      linear_window_size, linear_window_size, M_PI, &low_resolution_matcher};
-  return MatchWithSearchParameters(
+  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
-      search_parameters, initial_pose_estimate, coarse_point_cloud,
+                                   coarse_point_cloud, fine_point_cloud,
-      fine_point_cloud, min_score, score, pose_estimate, rotational_score);
+                                   min_score, score, pose_estimate,
+                                   rotational_score, low_resolution_score);
 }
 
 bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
@@ -149,7 +184,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
     const sensor::PointCloud& coarse_point_cloud,
     const sensor::PointCloud& fine_point_cloud, const float min_score,
     float* const score, transform::Rigid3d* const pose_estimate,
-    float* const rotational_score) const {
+    float* const rotational_score, float* const low_resolution_score) const {
   CHECK_NOTNULL(score);
   CHECK_NOTNULL(pose_estimate);
 
@@ -169,6 +204,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
         GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>();
     *rotational_score =
         discrete_scans[best_candidate.scan_index].rotational_score;
+    *low_resolution_score = best_candidate.low_resolution_score;
     return true;
   }
   return false;
@@ -347,26 +383,30 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
     const std::vector<DiscreteScan>& discrete_scans,
     const std::vector<Candidate>& candidates, const int candidate_depth,
     float min_score) const {
-  Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero());
-  best_high_resolution_candidate.score = min_score;
   if (candidate_depth == 0) {
     for (const Candidate& candidate : candidates) {
       if (candidate.score <= min_score) {
         // Return if the candidate is bad because the following candidate will
         // not have better score.
-        return best_high_resolution_candidate;
+        return Candidate::Unsuccessful();
-      } else if ((*search_parameters.matching_function)(
+      }
-                     GetPoseFromCandidate(discrete_scans, candidate)) >=
+      const float low_resolution_score =
-                 options_.min_low_resolution_score()) {
+          (*search_parameters.low_resolution_matcher)(
+              GetPoseFromCandidate(discrete_scans, candidate));
+      if (low_resolution_score >= options_.min_low_resolution_score()) {
         // We found the best candidate that passes the matching function.
-        return candidate;
+        Candidate best_candidate = candidate;
+        best_candidate.low_resolution_score = low_resolution_score;
+        return best_candidate;
       }
     }
 
     // All candidates have good scores but none passes the matching function.
-    return best_high_resolution_candidate;
+    return Candidate::Unsuccessful();
   }
 
+  Candidate best_high_resolution_candidate = Candidate::Unsuccessful();
+  best_high_resolution_candidate.score = min_score;
   for (const Candidate& candidate : candidates) {
     if (candidate.score <= min_score) {
       break;

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -41,32 +41,8 @@ CreateFastCorrelativeScanMatcherOptions(
     common::LuaParameterDictionary* parameter_dictionary);
 
 class PrecomputationGridStack;
-
+struct DiscreteScan;
-struct DiscreteScan {
+struct Candidate;
-  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 {
-  Candidate(const int scan_index, const Eigen::Array3i& offset)
-      : scan_index(scan_index), offset(offset) {}
-
-  // Index into the discrete scans vectors.
-  int scan_index;
-
-  // Linear offset from the initial pose in cell indices. For lower resolution
-  // candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth
-  // block of possibilities.
-  Eigen::Array3i offset;
-
-  // Score, higher is better.
-  float score = 0.f;
-
-  bool operator<(const Candidate& other) const { return score < other.score; }
-  bool operator>(const Candidate& other) const { return score > other.score; }
-};
 
 // Used to compute scores between 0 and 1 how well the given pose matches.
 using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
@@ -85,35 +61,37 @@ 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', 'pose_estimate', and
+  // is possible, true is returned, and 'score', 'pose_estimate',
-  // 'rotational_score' are updated with the result. 'fine_point_cloud' is used
+  // 'rotational_score', and 'low_resolution_score' are updated with the result.
-  // to compute the rotational scan matcher score.
+  // 'fine_point_cloud' is used 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,
-             const MatchingFunction& matching_function, float* score,
+             const MatchingFunction& low_resolution_matcher, float* score,
-             transform::Rigid3d* pose_estimate, float* rotational_score) const;
+             transform::Rigid3d* pose_estimate, float* rotational_score,
+             float* low_resolution_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', 'pose_estimate', and 'rotational_score' are updated with the
+  // 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score'
-  // result. 'fine_point_cloud' is used to compute the rotational scan matcher
+  // are updated with the result. 'fine_point_cloud' is used to compute the
-  // score.
+  // 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,
-                       const MatchingFunction& matching_function, float* score,
+                       const MatchingFunction& low_resolution_matcher,
-                       transform::Rigid3d* pose_estimate,
+                       float* score, transform::Rigid3d* pose_estimate,
-                       float* rotational_score) const;
+                       float* rotational_score,
+                       float* low_resolution_score) const;
 
  private:
   struct SearchParameters {
     const int linear_xy_window_size;     // voxels
     const int linear_z_window_size;      // voxels
     const double angular_search_window;  // radians
-    const MatchingFunction* const matching_function;
+    const MatchingFunction* const low_resolution_matcher;
   };
 
   bool MatchWithSearchParameters(
@@ -121,7 +99,8 @@ 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, float* rotational_score) const;
+      transform::Rigid3d* pose_estimate, float* rotational_score,
+      float* low_resolution_score) const;
   DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
                               const sensor::PointCloud& point_cloud,
                               const transform::Rigid3f& pose,

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -128,12 +128,14 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
     float score = 0.f;
     transform::Rigid3d pose_estimate;
     float rotational_score = 0.f;
+    float low_resolution_score = 0.f;
     EXPECT_TRUE(fast_correlative_scan_matcher->Match(
         transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
         [](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
-        &score, &pose_estimate, &rotational_score));
+        &score, &pose_estimate, &rotational_score, &low_resolution_score));
     EXPECT_LT(kMinScore, score);
     EXPECT_LT(0.09f, rotational_score);
+    EXPECT_LT(0.99f, low_resolution_score);
     EXPECT_THAT(expected_pose,
                 transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
         << "Actual: " << transform::ToProto(pose_estimate).DebugString()
@@ -141,7 +143,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
     EXPECT_FALSE(fast_correlative_scan_matcher->Match(
         transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
         [](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
-        &score, &pose_estimate, &rotational_score));
+        &score, &pose_estimate, &rotational_score, &low_resolution_score));
   }
 }
 
@@ -154,12 +156,14 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
   float score = 0.f;
   transform::Rigid3d pose_estimate;
   float rotational_score = 0.f;
+  float low_resolution_score = 0.f;
   EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap(
       Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
       [](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
-      &score, &pose_estimate, &rotational_score));
+      &score, &pose_estimate, &rotational_score, &low_resolution_score));
   EXPECT_LT(kMinScore, score);
   EXPECT_LT(0.09f, rotational_score);
+  EXPECT_LT(0.99f, low_resolution_score);
   EXPECT_THAT(expected_pose,
               transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
       << "Actual: " << transform::ToProto(pose_estimate).DebugString()
@@ -167,7 +171,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
   EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap(
       Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
       [](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
-      &score, &pose_estimate, &rotational_score));
+      &score, &pose_estimate, &rotational_score, &low_resolution_score));
 }
 
 }  // namespace

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -189,6 +189,7 @@ void ConstraintBuilder::ComputeConstraint(
   float score = 0.f;
   transform::Rigid3d pose_estimate;
   float rotational_score = 0.f;
+  float low_resolution_score = 0.f;
 
   const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
       submap_scan_matcher->low_resolution_hybrid_grid,
@@ -202,7 +203,7 @@ void ConstraintBuilder::ComputeConstraint(
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
             initial_pose.rotation(), high_resolution_point_cloud, point_cloud,
             options_.global_localization_min_score(), low_resolution_matcher,
-            &score, &pose_estimate, &rotational_score)) {
+            &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);
@@ -215,7 +216,7 @@ void ConstraintBuilder::ComputeConstraint(
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
             initial_pose, high_resolution_point_cloud, point_cloud,
             options_.min_score(), low_resolution_matcher, &score,
-            &pose_estimate, &rotational_score)) {
+            &pose_estimate, &rotational_score, &low_resolution_score)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
     } else {
@@ -226,6 +227,7 @@ void ConstraintBuilder::ComputeConstraint(
     common::MutexLocker locker(&mutex_);
     score_histogram_.Add(score);
     rotational_score_histogram_.Add(rotational_score);
+    low_resolution_score_histogram_.Add(low_resolution_score);
   }
 
   // Use the CSM estimate as both the initial and previous pose. This has the
@@ -288,6 +290,8 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
           LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10);
           LOG(INFO) << "Rotational score histogram:\n"
                     << rotational_score_histogram_.ToString(10);
+          LOG(INFO) << "Low resolution score histogram:\n"
+                    << low_resolution_score_histogram_.ToString(10);
         }
         constraints_.clear();
         callback = std::move(when_done_);

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -190,6 +190,7 @@ class ConstraintBuilder {
   // Histograms of scan matcher scores.
   common::Histogram score_histogram_ GUARDED_BY(mutex_);
   common::Histogram rotational_score_histogram_ GUARDED_BY(mutex_);
+  common::Histogram low_resolution_score_histogram_ GUARDED_BY(mutex_);
 };
 
 }  // namespace sparse_pose_graph