Cartographer源码阅读(9)：图优化的前端——闭环检测

约束计算

闭环检测的策略：搜索闭环，通过匹配检测是否是闭环，采用了分支定界法。

前已经述及PoseGraph的内容，此处继续。位姿图类定义了pose_graph::ConstraintBuilder constraint_builder_对象。

1.ConstraintBuilder类图

定义了SubmapScanMatcher的键值对。

 // Map of already constructed scan matchers by 'submap_id'.
 std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_ GUARDED_BY(mutex_);

SubmapScanMatcher结构体定义如下：

   struct SubmapScanMatcher
  {
     const ProbabilityGrid* probability_grid;
     std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
         fast_correlative_scan_matcher;
   };

注意ConstraintBuilder::ComputeConstraint方法，MaybeAddConstraint和MaybeAddGlobalConstraint都调用了该方法。

 void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
     const mapping::NodeId& node_id, bool match_full_submap,
     const mapping::TrajectoryNode::Data* const constant_data,
     const transform::Rigid2d& initial_relative_pose,
     std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
   const transform::Rigid2d initial_pose =
       ComputeSubmapPose(*submap) * initial_relative_pose;
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_id);

   // The 'constraint_transform' (submap i <- node j) is computed from:
   // - a 'filtered_gravity_aligned_point_cloud' in node j,
   // - the initial guess 'initial_pose' for (map <- node j),
   // - the result 'pose_estimate' of Match() (map <- node j).
   // - the ComputeSubmapPose() (map <- submap i)
   float score = .;
   transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();

   // Compute 'pose_estimate' in three stages:
   // 1. Fast estimate using the fast correlative scan matcher.
   // 2. Prune if the score is too low.
   // 3. Refine.
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
             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, );
       CHECK_GE(submap_id.trajectory_id, );
     } else {
       return;
     }
   } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
             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());
     } else {
       return;
     }
   }
   {
     common::MutexLocker locker(&mutex_);
     score_histogram_.Add(score);
   }

   // Use the CSM estimate as both the initial and previous pose. This has the
   // effect that, in the absence of better information, we prefer the original
   // CSM estimate.
   ceres::Solver::Summary unused_summary;
   ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                             constant_data->filtered_gravity_aligned_point_cloud,
                             *submap_scan_matcher->probability_grid,
                             &pose_estimate, &unused_summary);

   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;
   constraint->reset(new Constraint{submap_id,
                                    node_id,
                                    {transform::Embed3D(constraint_transform),
                                     options_.loop_closure_translation_weight(),
                                     options_.loop_closure_rotation_weight()},
                                    Constraint::INTER_SUBMAP});

   if (options_.log_matches()) {
     std::ostringstream info;
     info << "Node " << node_id << " with "
          << constant_data->filtered_gravity_aligned_point_cloud.size()
          << " points on submap " << submap_id << std::fixed;
     if (match_full_submap) {
       info << " matches";
     } else {
       const transform::Rigid2d difference =
           initial_pose.inverse() * pose_estimate;
       info << " differs by translation " << std::setprecision()
            << difference.translation().norm() << " rotation "
            << std::setprecision() << std::abs(difference.normalized_angle());
     }
     info << " with score " << std::setprecision() << . * score << "%.";
     LOG(INFO) << info.str();
   }
 }

这里出现了scan_matching::FastCorrelativeScanMatcher，另一种扫描匹配的方法。论文中介绍的分支定界法就在这个类中实现。

以上FastCorrelativeScanMatcher::Match和FastCorrelativeScanMatcher::MatchFullSubmap方法都调用了FastCorrelativeScanMatcher::MatchWithSearchParameters方法。

FastCorrelativeScanMatcher::MatchWithSearchParameters调用了FastCorrelativeScanMatcher::BranchAndBound方法。

Tips：总结一下出现的几种扫描匹配的方法？

RealTimeCorrelativeScanMatcher

FastCorrelativeScanMatcher

 bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
     SearchParameters search_parameters,
     const transform::Rigid2d& initial_pose_estimate,
     const sensor::PointCloud& point_cloud, float min_score, float* score,
     transform::Rigid2d* pose_estimate) const
 {
   CHECK_NOTNULL(score);
   CHECK_NOTNULL(pose_estimate);

   const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
   const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
       point_cloud,
       transform::Rigid3f::Rotation(Eigen::AngleAxisf(
           initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
   const std::vector<sensor::PointCloud> rotated_scans =
       GenerateRotatedScans(rotated_point_cloud, search_parameters);
   const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
       limits_, rotated_scans,
       Eigen::Translation2f(initial_pose_estimate.translation().x(),
                            initial_pose_estimate.translation().y()));
   search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());

   const std::vector<Candidate> lowest_resolution_candidates =
       ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
   const Candidate best_candidate = BranchAndBound(
       discrete_scans, search_parameters, lowest_resolution_candidates,
       precomputation_grid_stack_->max_depth(), min_score);//分支定界法
   if (best_candidate.score > min_score) {
     *score = best_candidate.score;
     *pose_estimate = transform::Rigid2d(
         {initial_pose_estimate.translation().x() + best_candidate.x,
          initial_pose_estimate.translation().y() + best_candidate.y},
         initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
     return true;
   }
   return false;
 }

2.分支定界法

FastCorrelativeScanMatcher::BranchAndBound，......

 Candidate FastCorrelativeScanMatcher::BranchAndBound(
     const std::vector<DiscreteScan>& discrete_scans,
     const SearchParameters& search_parameters,
     const std::vector<Candidate>& candidates, const int candidate_depth,
     float min_score) const
 {
   if (candidate_depth == )
   {
     // Return the best candidate.
     return *candidates.begin();
   }

   Candidate best_high_resolution_candidate(, , , search_parameters);
   best_high_resolution_candidate.score = min_score;
   for (const Candidate& candidate : candidates)
   {
     if (candidate.score <= min_score) {  break;  }
     std::vector<Candidate> higher_resolution_candidates;
     const int half_width =  << (candidate_depth - );
     for (int x_offset : {, half_width})
     {
       if (candidate.x_index_offset + x_offset >
           search_parameters.linear_bounds[candidate.scan_index].max_x) {
         break;
       }
       for (int y_offset : {, half_width}) {
         if (candidate.y_index_offset + y_offset >
             search_parameters.linear_bounds[candidate.scan_index].max_y) {
           break;
         }
         higher_resolution_candidates.emplace_back(
             candidate.scan_index, candidate.x_index_offset + x_offset,
             candidate.y_index_offset + y_offset, search_parameters);
       }
     }
     ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - ),
                     discrete_scans, search_parameters,
                     &higher_resolution_candidates);
     best_high_resolution_candidate = std::max(
         best_high_resolution_candidate,
         BranchAndBound(discrete_scans, search_parameters,
                        higher_resolution_candidates, candidate_depth - ,
                        best_high_resolution_candidate.score));
   }
   return best_high_resolution_candidate;
 }