cartographer 代码思想解读（1）- 相关匹配

cartographer在2016年开源后一直在使用，但是一直未仔细阅读并分析其核心代码结构。目前网上可以找到许多博主对其分析和理解。其cartographer的基本思想可参考他人的 博主博客。本博客主要目的根据其框架思想，将其核心算法进行提取，方便后续学习和移植。其google开源的代码量太大，层次太多，大量虚函数，继承关系，功能齐全，并且采用大量的C++11.0新特性等许多原因，导致阅读代码难度加大。因此此次阅读仅对slam核心算法及其思想进行总结。

前端

基本上所有了解slam的人都知道SLAM可分为前端和后端处理，其中前端处理是slam的前提，通常称为帧间匹配，即将新的一帧激光点云和上帧或多帧进行匹配，获取新时刻位置的过程，cartographer也不例外。仔细阅读源码发现，cartographer并非仅一种前端匹配算法，本节仅分析其中一种：采用相关匹配的方法。（注：实际采用的是优化的方法）
帧间匹配主要包括：scan-to-scan、scan-to-map 和优化的方法。
其中cartographer源码中存在一种scan-to-map的相关匹配的算法，其代码目录：cartographer/mapping/internal/2d/scanmathing

栅格地图

Cartographer采用的是当前新的scan激光帧，是否匹配submap并加入。其submap为栅格地图，可以认为submap为一个2维数组，其每个索引下的value值表示概率。为提高运算性能和速度，cartographer将概率表示进行优化，包括概率整数化和对数化。其栅格地图代码目录：cartographer/mapping/2d/grid.h。后续将加入专门代码详解和cartographer的优化。

相关匹配

顶层方法文件目录：cartographer/mapping/internal/2d/scanmathing/real_time_correlative_scan_matcher_2d.cc
调用入口方法为double RealTimeCorrelativeScanMatcher2D::Match

double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate) const {
  CHECK(pose_estimate != nullptr);
  //获取初始位置的角度
  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())));
  // 定义搜索的空间，包括平移的窗口和角度窗口
  // options_ 为配置参数，由上层调用传入
  const SearchParameters search_parameters(
      options_.linear_search_window(), options_.angular_search_window(),
      rotated_point_cloud, grid.limits().resolution());
  // 在一定角度搜索空间中（一定窗口内，一定分辨率，如1度分辨率， 搜索范围100度，共100个角度空间），
  // 对当前点云进行旋转变换，放入vector中
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  // 将点云进行坐标转换到世界坐标系坐标，即根据当前估计位置（可以为里程计的位置）进行转换
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));

  // 初始化在xy平移空间内偏移量，即窗口大小和偏移量
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);

  // 计算当前帧在角度、x、y三层窗口中每个状态在grid地图中的匹配的评分
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

  //采用标准库，迭代获取最大置信度元素
  const Candidate2D& best_candidate =
      *std::max_element(candidates.begin(), candidates.end());
  // 坐标转换为全局坐标
  *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采用的相关匹配算法基本上可以认为是一种暴力搜索匹配算法，即采用当前的激光帧在已知的栅格地图上进行遍历匹配，获取相关性最高的位置，即置信度最高的位置。其思想几乎与KartoSLAM中的帧间匹配方法一致，但对其进行了一定优化。
3重循环暴力匹配伪代码如下：

for（x）
	for（y）
		for（theta）
			point_new= point_old
			bel = sum(point_new)/sum_num

其思想采用了3重循环，在submap中的一定范围内，进行暴力匹配。其步骤如下：

1. 将激光点云先转换至估计坐标的世界坐标系下；
2. 采用了预先定义x、y、yaw三个窗口大小；
3. 将角度进行序列化生成序列，并将点云按照序列后的角度值进行旋转，即可生成N个点云序列。
4. 将x，y按照窗口，分辨率进行序列化，即求出应当遍历的所有索引（因为x，y窗口仅需要加减平移）。
5. 对以上所有点云在栅格地图中获取最大置信度，仅需查表读取概率值和并归一化；

其相对于KartoSLAM优化点，包括两点：
1.采用空间换取了运算性能，将其遍历的x、y层循环不做实际处理，仅保存其索引队列。同时将角度循环提前遍历并保存所有序列。后续可采用vector迭代和库进行批量处理。
2.减少乘除与sin，cos运算。三层循环最先进行角度旋转，因此sin和cos的运算的次数仅为执行角度量化个数。而后点云转换均为加减运算。

疑问点

1.此相关算法在代码实现上没有看出特别出众之处，可能理解不深。
2.由暴力匹配后得到的置信度，即point在栅格图的概率和并归一化后的结果。而cartographer提供的相关算法，在获取的置信度进行了一次基于与初始位置高斯权重处理作为最终的置信度。

    // 此处没看明白????，为什么归一化后的匹配置信度需要加权重，和初始位置距离的高斯分布的概率权重
    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()));