Karto 前端

karto slam 包含前端 回环检测 后端图优化 建图几个部分组成。本篇从端的数据处理 子图匹配开始分析。

整个过程是相关匹配算法，采用了暴力搜索匹配

搜先计算角度变化后的激光帧情况。

用始终动态维护的滑动窗口建立一个子图。这个子图是一个正方形，以机器人为中心，原点是这个正方形左下角的点, 源码中默认大小如下

 //相当于一个子图，是位姿微小变化搜索框+2*激光测距最大值，

    //roi的宽和高都是 0.3 / 0.01 + 2 * 12 / 0.01

    Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();

建立一个二维数组，对当前雷达的角度根据角度变化的分辨率进行变化，每一个角度就对应一个二维数组表中的一维数组，这个一维数组就放激光帧，是经过旋转变换后的激光帧，最后保存的是旋转变换后的激光点在子图系下的索引。

这样角度变换后每一个雷达航向角对应的激光点在子图系下的索引就好了

然后计算平移，分为x y方向，先根据平移的距离和平移分辨率得到一个平移集合

然后开始了三层for循环的暴力搜索

将y x 角度顺序循环

先得到平移后的雷达在子图索引对应的栅格的指针

然后得到经过平移和角度变换后的雷达下的激光帧中激光点对应的索引子图下的

索引知道了就可以到到该栅格的占用情况，累加这写占用值

pByte是雷达平移后所对应的栅格索引位置指针，pAngleIndexPointer[i]是某一个角度下的一帧雷达中的一个激光点对应的在雷达系下的索引。 pByte[pAngleIndexPointer[i]]就是以平移后的雷达索引为起点，再移动pAngleIndexPointer[i]这么多索引后的栅格占用情况。

      //本质就是平移的雷达+角度变换化对应的雷达系下的激光点后的情况，就是经过雷达平移和角度变换作用后的激光点在子图中对应的栅格索引(以子图为原点)

      response += pByte[pAngleIndexPointer[i]];//激光点对应的子图栅格索引的占用情况，

最后得到

  response /= (nPoints * GridStates_Occupied);

这个response=1就说明该帧激光与子图完全匹配了

为了后面查询每个雷达位姿对应的匹配值 建立std::pair 匹配值-雷达位姿

每个雷达变化后的位姿对应的与子图的匹配值已经得到并构成了std::pair类型

开始计算最优位姿

先得到最大的匹配值，然后得到在这个与这个最大值在一定范围内的匹配值，

就是比较接近这个匹配值，默认是相差在1e-6内， 将这些匹配值对应的雷达位姿求和在平均下就是最后得到的前端匹配的最优位姿。

整个前端匹配就得到了本次前端匹配的最优位姿和最优的匹配值

还有该位姿的位置协方差和角度协方差

后面还有一次精匹配，子图的分辨率变高了 ，位置变化的范围变小的了，其他一样

 //精匹配时 角度变化是0.2度， 角度分辨率是粗匹配时的0.5倍即1度, 调用的函数与粗匹配相同，过程与粗匹配基本相同