在学习栅格地图的时候，我们知道在栅格更新前会先进行扫描匹配获取当前机器人最有可能所在的位姿：

  // local map frame <- gravity-aligned frame// 扫描匹配, 进行点云与submap的匹配std::unique_ptr<transform::Rigid2d> pose_estimate_2d =ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);if (pose_estimate_2d == nullptr) {LOG(WARNING) << "Scan matching failed.";return nullptr;}

这个函数同样在LocalTrajectoryBuilder2D里面，它主要执行了两个操作：包里匹配以及基于ceres的LM匹配。这里我们首先看一下暴力匹配部分。

参数配置

在扫描匹配中，是否启用相关性扫描匹配（暴力匹配）根据参数use_online_correlative_scan_matching决定：

  // 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准if (options_.use_online_correlative_scan_matching()) {const double score = real_time_correlative_scan_matcher_.Match(pose_prediction, filtered_gravity_aligned_point_cloud,*matching_submap->grid(), &initial_ceres_pose);kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);}

其决定了是否启动该扫描搜索器。同时，如果要使用的话，还需要配置下述一些参数：

real_time_correlative_scan_matcher = {linear_search_window = 0.1,             -- 线性搜索窗口的大小angular_search_window = math.rad(20.),  -- 角度搜索窗口的大小translation_delta_cost_weight = 1e-1,   -- 用于计算各部分score的权重rotation_delta_cost_weight = 1e-1,},

进一步，我们看一下match这个函数的具体实现。

函数实现

1、函数输入与输出

对于相关性扫描匹配函数而言，其输入输出主要包含了以下几个：

/*** @param[in] initial_pose_estimate 预测出来的先验位姿* @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点* @param[in] grid 用于匹配的栅格地图* @param[out] pose_estimate 校正后的位姿* @return double 匹配得分*/

在调用相关性扫描匹配函数时，前面我们看到它传递了一些参数：

const double score = real_time_correlative_scan_matcher_.Match(pose_prediction, filtered_gravity_aligned_point_cloud,*matching_submap->grid(), &initial_ceres_pose);

上述参数中，第一项pose_prediction为位姿推测器计算出来的预测位姿，作为当前先验的存在；filtered_gravity_aligned_point_cloud是用于扫描匹配的当前点云。注意到这里的点云是位于local坐标系原点的点云。在cartographer中，似乎它的位姿跟随不是跟随的全局坐标系而是一直跟随的local坐标系，所以前期处理点云的时候都是将点云转换到local坐标系原点然后用于后期匹配以及插入；*matching_submap->grid()则是一个指针，我们知道在cartographer中地图都是采用智能指针的形式处理的，所以这里也就是相当于使用了第一张激活的地图进行匹配（cartographer中正常运行时存在两张激活的子图，它们之间差90帧）。而最后面的initial_ceres_pose则是一个根据相关性扫描匹配函数得到的新的位姿。

2、函数具体流程

看完输入输出，接下来展开看看这个函数的具体内容，它主要包含了以下几个部分：

2.1、激光数据的位姿旋转

前面我们知道，输入的点云是位于local坐标系原点的，但是传入的初始姿态一版都不是原点，我们的搜索是基于给定姿态进行前后左右的搜索的，所以需要先进行位姿旋转：

// Step: 1 将点云旋转到预测的方向上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())));

注意到这里进行的是位姿的旋转而不是直接投影到给定坐标系下的。然后我们往下看第二步：

2.2、生成按照不同角度旋转后的点云集合

这一步的操作是根据前面我们初始化时给定次参数生成一系列点云：

// 根据配置参数初始化 SearchParametersconst SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(),rotated_point_cloud, grid.limits().resolution());// Step: 2 生成按照不同角度旋转后的点云集合const std::vector<sensor::PointCloud> rotated_scans =GenerateRotatedScans(rotated_point_cloud, search_parameters);

注意到上面部分的search_parameters里面的就是我们开始时传递的lua参数：

linear_search_window = 0.1,             -- 线性搜索窗口的大小
angular_search_window = math.rad(20.),  -- 角度搜索窗口的大小

这个地方的作用是根据传递给过来的参数生成具体的线性搜索框数量以及角度搜索框数量。其准循以下原则：首先对于旋转而言，传入的角度搜索窗为上下限（-20-20度）然后根据获取的点云的最远距离与栅格分辨率决定每次角度旋转的具体大小。即一次旋转时最远处点云不要平移超过一个分辨率的距离。其次对于平移而言，这个就比较简单了，直接一次平移一个分辨率的距离。
然后下面的GenerateRotatedScans负责按照计算出来的分辨率生成不同角度下的点云。

2.3、获取平移后的点在地图中的索引

到这里我们已经得到了一系列旋转后的点云的集合，既然有旋转自然也要有平移，所以下一步自然要进行平移操作：

  // Step: 3 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(grid.limits(), rotated_scans,Eigen::Translation2f(initial_pose_estimate.translation().x(),initial_pose_estimate.translation().y()));

DiscretizeScans函数本身传参为之前旋转后的所有点云，然后在这里对每一组点云按照之前设置的平移阈值以及分辨率生成一系列平移后的点云，并返回这些点云的索引。

2.4、生成候选解的数量

它的作用是计算有多少个候选解得数量。总数量为：角度步长总数乘以X方向平移步长总数乘以Y方向平移步长总数。然后再生成一系列的候选解，以结构体Candidate2D的形式存在。

  // Step: 4 生成所有的候选解std::vector<Candidate2D> candidates =GenerateExhaustiveSearchCandidates(search_parameters);

注意到这个函数中生成的两个参数x_index_offset与y_index_offset，它们是原始点云在XY方向的平移量，这个后面会用到。

2.5、计算每个候选解的得分

在前面的4个步骤中既然我们已经获取到了所有可能的解，那么下一步自然是将这些可能的解具体分数求出来：

  // Step: 5 计算所有候选解的加权得分ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

这里是直接调用了一个ScoreCandidates函数进行的计算，我们直接看一下这个函数：

// 计算所有候选解的加权得分
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(const Grid2D& grid, const std::vector<DiscreteScan2D>& discrete_scans,const SearchParameters& search_parameters,std::vector<Candidate2D>* const candidates) const {for (Candidate2D& candidate : *candidates) {switch (grid.GetGridType()) {case GridType::PROBABILITY_GRID:candidate.score = ComputeCandidateScore(static_cast<const ProbabilityGrid&>(grid),discrete_scans[candidate.scan_index], candidate.x_index_offset,candidate.y_index_offset);break;case GridType::TSDF:candidate.score = ComputeCandidateScore(static_cast<const TSDF2D&>(grid),discrete_scans[candidate.scan_index], candidate.x_index_offset,candidate.y_index_offset);break;}// 对得分进行加权candidate.score *=std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *options_.translation_delta_cost_weight() +std::abs(candidate.orientation) *options_.rotation_delta_cost_weight()));}
}

这个函数本身分为两个部分，第一部分根据地图选项调用ComputeCandidateScore函数计算每一个候选项的得分，第二部分根据初始化时给定的圈中参数对结果分数进行加权。
首先看一下第一部分：ComputeCandidateScore函数本身也很简单：

// 计算点云在指定像素坐标位置下与ProbabilityGrid地图匹配的得分
float ComputeCandidateScore(const ProbabilityGrid& probability_grid,const DiscreteScan2D& discrete_scan,int x_index_offset, int y_index_offset) {float candidate_score = 0.f;for (const Eigen::Array2i& xy_index : discrete_scan) {// 对每个点都加上像素坐标的offset, 相当于对点云进行平移const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,xy_index.y() + y_index_offset);// 获取占用的概率const float probability =probability_grid.GetProbability(proposed_xy_index);// 以概率为得分candidate_score += probability;}// 计算平均得分candidate_score /= static_cast<float>(discrete_scan.size());CHECK_GT(candidate_score, 0.f);return candidate_score;
}

首先我们看到这个函数的传参包含了四个：probability_grid为一张用于匹配的概率栅格地图；discrete_scan为一帧候选点云。这两个比较明确，但是后续两个x_index_offset与y_index_offset不是很好理解。但是我们看调用函数的传参可以比较清晰的知道，它来自于结构体Candidate2D，而这个东西是在2.4中生成的，所以回看2.4我们知道它代表的是原始点云在XY方向需要的平移量，而实际函数中的使用也体现了这一点：

// 对每个点都加上像素坐标的offset, 相当于对点云进行平移const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,xy_index.y() + y_index_offset);

根据平移量，我们可以获取到该点平移后的栅格，因此也就可以获得它的概率值。然后，我们将每个点的概率值全部加起来：

// 获取占用的概率const float probability =probability_grid.GetProbability(proposed_xy_index);// 以概率为得分candidate_score += probability;

没错，就是直接加起来，不用任何优化的思想就加起来就好了。这就是暴力搜索的暴力所在！那么它的依据在哪里呢？想像两种极端：所有点都与障碍物完全匹配以及一个点都没匹配到障碍物。我们知道，在概率栅格中，1代表完全占用0代表完全空闲。因此对于前面一种，其暴力匹配后的总分就会是点云数量，而对于第二者，约等于0分。因此，暴力匹配的分数越高，也就说明该位置点云与栅格中障碍物所在位置越接近。这也就是暴力匹配的原理。当然这里为了防止点云数量的变化导致有的地方虽然单点概率低但是总分高的情况还做了下均值，但是不影响原理。
然后是第二部分：

candidate.score *=std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *options_.translation_delta_cost_weight() +std::abs(candidate.orientation) *options_.rotation_delta_cost_weight()));

该函数对每个候选项的得分乘以了一个系数，这个系数为它的XY偏移量乘以权重与角度偏移量乘以旋转权重两者和的倒数。也就是说，这个数据离初始姿态的XY平移量越大或者角度分量越大，它的系数也就会越小，所以说在算法中对于位姿推测器的结果的置信度还是很高的。

2.6、获取最高得分

这个函数只用于获取所有解中得分最大值的解：

// Step: 6 获取最优解const Candidate2D& best_candidate =*std::max_element(candidates.begin(), candidates.end());

这里的获取方式主要依靠max_element，可以单独学习一下这个函数的用法。

2.7、更新最优位姿

既然已经得到了分数最高的解，下一步自然是将其作为最优先验位姿，以便后续程序的使用了：

  // Step: 7 将计算出的偏差量加上原始位姿获得校正后的位姿*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 best_candidate.score;

总结

暴力搜索在cartographer中主要用于根据给定的预测位姿在一定范围内寻找最有可能为最优解的大体位置，但是由于它不涉及到优化内容，所以得到的结果是有一定误差的（一个分辨率栅格内），所以根据这个结果还需要调用ceres匹配方式进一步获取最优解。同时，由于它需要不断进行栅格的遍历与计算，所以它的计算量也是想对较高的，如果搜索框给的比较大但是栅格分辨率又给的比较小的话就会需要进行非常久的搜索才能得出结果。