介绍
回环检测消除累积误差。本文的里程计漂移小，精度高，有实时性。都可以不使用imu。slam分为两个问题
- 高频低精：前端粗糙的配准
- 高精低频：配准的精度很高
两个方法的结合才有好的效果。
雷达对光照和纹理不敏感，视觉里程计需要提取特征点，万一光照不好就不好搞。我们这累积的里程计漂移很小，所以没有加入回环检测
又漂移少又算的快，是因为我们把传统的SLAM拆分为上面的两个问题。这两个问题都需要提出角点，然后进行配准。后端是一个优化问题，过程和ICP比较相似，可以实现一个高精度的实时求解
符号和目的
$X_{k,i}^L$: 雷达坐标系下第k次扫描中的第i个点
$X_{k,i}^W$: 世界坐标系下第k次扫描中的第i个点
$P_k$:第k次扫描的数据，注意时间是不同的
任务：针对每一此扫描的雷达点，计算雷达的运动，建立相应的地图
前端里程计
特征点提取
第一个事情：提取特征点，面点和角点，面点的曲率很小，角点的曲率很大，对同一个scan的所有点进行排序，选择最大的几个点作为角点。特征点要尽可能的均匀化，我们把一个scan分成六等份，每一份中取几个点。即便是在一个小区域中的点，也不可以太集中了
对于不可靠的点
- 面点和当前雷达比较平行，是不能要的，因为当雷达稍微移动可能就检测不到了
- 对于遮挡面比较多的点，也需要干掉
总结：先算曲率，判断是角点还是面点。如果点被选中了，是不是超出了最大的限制，每一个子区域都有一个上线的，周围的点是不是被选取了，保证每一个子区域的分布性。再一次判断点是不是可靠的
寻找匹配对
雷达的处理：把$tk$到$t{k+1}$所有的点到设置为$t{k+1}$上的点。记为$\hat{P}{k}$，下面需要对下一帧点云做处理，即$P_{k+1}$。
下面需要在由$\hat{P}k$的特征点组成的KDTree中，寻找$P{k+1}$的对应特征点。 需要先把$P{k+1}$上的点，转到开始$t{k+1}$处。注意上一帧的点云应该已经转移到扫描的结束时刻
对于新的一帧也是找面点和角点。把新的一帧的所有点都投影到扫描开始的时刻，记录这一帧的角点和面点为$\hat{\mathcal{E}}{k+1},\hat{\mathcal{H}}{k+1}$。通过KDtree我们在$\hat{P}_k$中，找寻与其最近的匹配点。
两个点确定一条直线，三个点确定一个面，计算角点距离线或者面的距离
距离计算：
- 点到线：h = 2S / d。d为两个点的距离，S 为三个点的面积，可以通过叉乘获得
- 点到面：体积 / 底面面积
运动估计
我们把每一帧之间的运动看做是一个匀速运动，因此时间的间隔是比较短的。然后其他的时间点进行插值
假设t为当前时间，$t{k+1}$为开始扫描的时间，$T{k+1}^L$ 为在$[t_{k+1},t]$时间段的雷达坐标系的变换
$ti$表示$\mathcal{P}{k+1}$上的点，$T{(k+1,i)}^L$表示在时间段$[t{k+1},t_i]$上的变换，可以通过简单的插值进行计算
通过改运动估计，可以把在扫描最开始的点 转换 到对应的位置上
通过LM算法，把；匹配对之间的距离设为 残差，因为要匹配，两个最好是完全重合，优化变量为$T_{k+1}^L$
雷达mapping
里程计是帧间匹配的过程，是频率比较高的过程。
雷达mapping是当前帧匹配到地图的过程，这个频率会比较低，但是精度比较高
每十帧进行一次地图优化
前端为了时效性，提取的点会比较少，后端的雷达匹配提取的特征点会比前端的多十倍。特征点越多，精度高，鲁棒性比较好。
mapping是通过栅格，找t周围的栅格寻找匹配对，依然是用过KDtree。会计算一个协方差矩阵，计算特征值和特征向量，如果是一个边缘点，那么一个特征值会明显大于其他两个特性值。
面点的：会有两个比较大的特征值，其他一个是比较小的。第三个特征向量就是面的法向量。仍然构建和前端一样 的优化问题。最后点云会通过滤波进行下采样
其实帧间匹配和地图匹配的原理是一样的，不过帧间匹配是寻找两帧之间的关键点，而地图匹配是寻找地图中的匹配点。