相关匹配Real-Time Correlative Scan Matching 总结

相关匹配算法作为Cartographer的前端算法，属于scan to Map.

这里的Map是Submap子图，即通过几帧激光构成的地图。

首先需要离散化世界坐标系，进入到栅格坐标中，对于通过一帧激光建立的地图，激光束打到的点就是障碍物点被击中，未击中的为空闲或未知，对应于高斯分布曲线，打中的栅格就是这个高斯分布的一阶矩u，其他栅格的概率计算是这样计算，一个栅格，找到距离其最近的障碍物栅格，先通过欧式距离计算出栅格索引的距离，再乘以地图分辨率得到实际距离，这个距离值就是高斯分布中的（x-u）, 有了这个就i可以计算对数似然值了，也就是得分，实际代码中先把每个栅格的这个得分先算法好，像一个表，为后续到来的激光匹配做准备。

过来一帧激光，通过先验给一个初始机器人位姿，将本帧激光根据机器人位姿映射到世界坐标系下，就确定了每个激光束打到了哪个栅格，上面已经计算好了每个栅格得分，查表即可，因为是对数的似然，将每一束激光的得分相加就是这个机器人位姿的状态估计情况。

因为机器人的位姿是根据先验给出的，一般是里程计或IMU积分得到，不准确，但是认为在其周围一定范围内有机器人的最佳位姿估计即后验。那么搜索该先验附近的匹配得分，分别进行 theta， x, y，三个方向的变化，比如theta=10度，x=10cm, y=10cm, 根据论文，三层for循环，将theta放在最外层。那么每个位姿都有一个得分，最大的就是最优估计了。

多分辨率地图分支定界加速。计算得分采用暴力搜索匹配，为了提高效率，采用两种分辨率地图，3cm和30cm， 即地分辨率一个栅格相当于100个高分辨率栅格， 且低分辨率栅格的最大值是其对应高分辨率栅格的上界，注意不是说其叶子节点即高分辨率栅格里的最大值就等于其对应高分辨率栅格的得分，比如说比较低分辨率栅格找到了最大的栅格A，那么查询其对应高分辨率地图中那100个栅格的最高分，比如是score1，不能说这个就是最大了，在上一层，进行回溯，将这个score1跟地分辨率的栅格得分比较，如果有比score1小的栅格，就剪枝这个低分辨率下的栅格，如果有比score1大的栅格，就再次进入这个栅格，看其对应的高分辨率栅格中的得分有没有比score1大的，有就把更大的得分赋值给score1，再继续这样回溯计算，得到最高的高分辨率地图下的得分所对应的位姿就是最佳估计位姿