Add full submap scan matching in 3D. (#145)

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -95,10 +95,8 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
     const proto::FastCorrelativeScanMatcherOptions& options)
     : options_(options),
       resolution_(hybrid_grid.resolution()),
-      linear_xy_window_size_(
+      origin_(hybrid_grid.origin()),
-          common::RoundToInt(options_.linear_xy_search_window() / resolution_)),
+      width_in_voxels_(hybrid_grid.grid_size()),
-      linear_z_window_size_(
-          common::RoundToInt(options_.linear_z_search_window() / resolution_)),
       precomputation_grid_stack_(
           common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
       rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {}
@@ -109,19 +107,54 @@ 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* score, transform::Rigid3d* pose_estimate) const {
+    float* const score, transform::Rigid3d* const pose_estimate) 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,
+                                   coarse_point_cloud, fine_point_cloud,
+                                   min_score, score, pose_estimate);
+}
+
+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 {
+  const transform::Rigid3d initial_pose_estimate(origin_.cast<double>(),
+                                                 gravity_alignment);
+  float max_point_distance = 0.f;
+  for (const Eigen::Vector3f& point : coarse_point_cloud) {
+    max_point_distance = std::max(max_point_distance, point.norm());
+  }
+  const int linear_window_size =
+      (width_in_voxels_ + 1) / 2 + std::ceil(max_point_distance);
+  const SearchParameters search_parameters{linear_window_size,
+                                           linear_window_size, M_PI};
+  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
+                                   coarse_point_cloud, fine_point_cloud,
+                                   min_score, score, pose_estimate);
+}
+
+bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
+    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
+    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 {
   CHECK_NOTNULL(score);
   CHECK_NOTNULL(pose_estimate);
 
-  const std::vector<DiscreteScan> discrete_scans =
+  const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
-      GenerateDiscreteScans(coarse_point_cloud, fine_point_cloud,
+      search_parameters, coarse_point_cloud, fine_point_cloud,
       initial_pose_estimate.cast<float>());
 
   const std::vector<Candidate> lowest_resolution_candidates =
-      ComputeLowestResolutionCandidates(discrete_scans);
+      ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
 
-  const Candidate best_candidate =
+  const Candidate best_candidate = BranchAndBound(
-      BranchAndBound(discrete_scans, lowest_resolution_candidates,
+      search_parameters, discrete_scans, lowest_resolution_candidates,
       precomputation_grid_stack_->max_depth(), min_score);
   if (best_candidate.score > min_score) {
     *score = best_candidate.score;
@@ -137,6 +170,7 @@ bool FastCorrelativeScanMatcher::Match(
 }
 
 DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
+    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
     const sensor::PointCloud& point_cloud,
     const transform::Rigid3f& pose) const {
   std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
@@ -156,7 +190,9 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
       options_.branch_and_bound_depth() - full_resolution_depth;
   CHECK_GE(low_resolution_depth, 0);
   const Eigen::Array3i search_window_start(
-      -linear_xy_window_size_, -linear_xy_window_size_, -linear_z_window_size_);
+      -search_parameters.linear_xy_window_size,
+      -search_parameters.linear_xy_window_size,
+      -search_parameters.linear_z_window_size);
   for (int i = 0; i != low_resolution_depth; ++i) {
     const int reduction_exponent = i + 1;
     const Eigen::Array3i low_resolution_search_window_start(
@@ -178,6 +214,7 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
 }
 
 std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
+    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
     const sensor::PointCloud& coarse_point_cloud,
     const sensor::PointCloud& fine_point_cloud,
     const transform::Rigid3f& initial_pose) const {
@@ -194,8 +231,8 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
       kSafetyMargin * std::acos(1.f -
                                 common::Pow2(resolution_) /
                                     (2.f * common::Pow2(max_scan_range)));
-  const int angular_window_size =
+  const int angular_window_size = common::RoundToInt(
-      common::RoundToInt(options_.angular_search_window() / angular_step_size);
+      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;
@@ -216,19 +253,23 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
         Eigen::Translation3f(initial_pose.translation()) *
         transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
         Eigen::Quaternionf(initial_pose.rotation()));
-    result.push_back(DiscretizeScan(coarse_point_cloud, pose));
+    result.push_back(
+        DiscretizeScan(search_parameters, coarse_point_cloud, pose));
   }
   return result;
 }
 
 std::vector<Candidate>
 FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
+    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
     const int num_discrete_scans) const {
   const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
   const int num_lowest_resolution_linear_xy_candidates =
-      (2 * linear_xy_window_size_ + linear_step_size) / linear_step_size;
+      (2 * search_parameters.linear_xy_window_size + linear_step_size) /
+      linear_step_size;
   const int num_lowest_resolution_linear_z_candidates =
-      (2 * linear_z_window_size_ + linear_step_size) / linear_step_size;
+      (2 * search_parameters.linear_z_window_size + linear_step_size) /
+      linear_step_size;
   const int num_candidates =
       num_discrete_scans *
       common::Power(num_lowest_resolution_linear_xy_candidates, 2) *
@@ -236,11 +277,13 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
   std::vector<Candidate> candidates;
   candidates.reserve(num_candidates);
   for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) {
-    for (int z = -linear_z_window_size_; z <= linear_z_window_size_;
+    for (int z = -search_parameters.linear_z_window_size;
-         z += linear_step_size) {
+         z <= search_parameters.linear_z_window_size; z += linear_step_size) {
-      for (int y = -linear_xy_window_size_; y <= linear_xy_window_size_;
+      for (int y = -search_parameters.linear_xy_window_size;
+           y <= search_parameters.linear_xy_window_size;
            y += linear_step_size) {
-        for (int x = -linear_xy_window_size_; x <= linear_xy_window_size_;
+        for (int x = -search_parameters.linear_xy_window_size;
+             x <= search_parameters.linear_xy_window_size;
              x += linear_step_size) {
           candidates.emplace_back(scan_index, Eigen::Array3i(x, y, z));
         }
@@ -277,15 +320,18 @@ void FastCorrelativeScanMatcher::ScoreCandidates(
 
 std::vector<Candidate>
 FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
+    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
     const std::vector<DiscreteScan>& discrete_scans) const {
   std::vector<Candidate> lowest_resolution_candidates =
-      GenerateLowestResolutionCandidates(discrete_scans.size());
+      GenerateLowestResolutionCandidates(search_parameters,
+                                         discrete_scans.size());
   ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans,
                   &lowest_resolution_candidates);
   return lowest_resolution_candidates;
 }
 
 Candidate FastCorrelativeScanMatcher::BranchAndBound(
+    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
     const std::vector<DiscreteScan>& discrete_scans,
     const std::vector<Candidate>& candidates, const int candidate_depth,
     float min_score) const {
@@ -303,15 +349,17 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
     std::vector<Candidate> higher_resolution_candidates;
     const int half_width = 1 << (candidate_depth - 1);
     for (int z : {0, half_width}) {
-      if (candidate.offset.z() + z > linear_z_window_size_) {
+      if (candidate.offset.z() + z > search_parameters.linear_z_window_size) {
         break;
       }
       for (int y : {0, half_width}) {
-        if (candidate.offset.y() + y > linear_xy_window_size_) {
+        if (candidate.offset.y() + y >
+            search_parameters.linear_xy_window_size) {
           break;
         }
         for (int x : {0, half_width}) {
-          if (candidate.offset.x() + x > linear_xy_window_size_) {
+          if (candidate.offset.x() + x >
+              search_parameters.linear_xy_window_size) {
             break;
           }
           higher_resolution_candidates.emplace_back(
@@ -321,10 +369,10 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
     }
     ScoreCandidates(candidate_depth - 1, discrete_scans,
                     &higher_resolution_candidates);
-    best_high_resolution_candidate =
+    best_high_resolution_candidate = std::max(
-        std::max(best_high_resolution_candidate,
+        best_high_resolution_candidate,
-                 BranchAndBound(discrete_scans, higher_resolution_candidates,
+        BranchAndBound(search_parameters, discrete_scans,
-                                candidate_depth - 1,
+                       higher_resolution_candidates, candidate_depth - 1,
                        best_high_resolution_candidate.score));
   }
   return best_high_resolution_candidate;

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -89,28 +89,55 @@ class FastCorrelativeScanMatcher {
              const sensor::PointCloud& fine_point_cloud, float min_score,
              float* score, transform::Rigid3d* pose_estimate) 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'
+  // and 'pose_estimate' are updated with the 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;
+
  private:
-  DiscreteScan DiscretizeScan(const sensor::PointCloud& point_cloud,
+  struct SearchParameters {
+    const int linear_xy_window_size;     // voxels
+    const int linear_z_window_size;      // voxels
+    const double angular_search_window;  // radians
+  };
+
+  bool MatchWithSearchParameters(
+      const SearchParameters& search_parameters,
+      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;
+  DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
+                              const sensor::PointCloud& point_cloud,
                               const transform::Rigid3f& pose) const;
   std::vector<DiscreteScan> GenerateDiscreteScans(
+      const SearchParameters& search_parameters,
       const sensor::PointCloud& coarse_point_cloud,
       const sensor::PointCloud& fine_point_cloud,
       const transform::Rigid3f& initial_pose) const;
   std::vector<Candidate> GenerateLowestResolutionCandidates(
-      int num_discrete_scans) const;
+      const SearchParameters& search_parameters, int num_discrete_scans) const;
   void ScoreCandidates(int depth,
                        const std::vector<DiscreteScan>& discrete_scans,
                        std::vector<Candidate>* const candidates) const;
   std::vector<Candidate> ComputeLowestResolutionCandidates(
+      const SearchParameters& search_parameters,
       const std::vector<DiscreteScan>& discrete_scans) const;
-  Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
+  Candidate BranchAndBound(const SearchParameters& search_parameters,
+                           const std::vector<DiscreteScan>& discrete_scans,
                            const std::vector<Candidate>& candidates,
                            int candidate_depth, float min_score) const;
 
   const proto::FastCorrelativeScanMatcherOptions options_;
   const float resolution_;
-  const int linear_xy_window_size_;
+  const Eigen::Vector3f origin_;
-  const int linear_z_window_size_;
+  const int width_in_voxels_;
   std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
   RotationalScanMatcher rotational_scan_matcher_;
 };