激光slam课程学习笔记－－第６课：激光的前端配准算法

前言：这系列笔记是学习曾书格老师的激光slam课程所得，这里分享只是个人理解，有误之处，望大佬们赐教。这节课主要介绍激光点云的前端配准算法。

１．帧间匹配算法

［帧间匹配算法，不一定指的是前后帧情况，也可能是好几帧情况］

1.1 ICP匹配算法

已知点的求解方法

求解证明：

［没理解，上图左侧的底部式子为零］

[正定对称举矩阵：矩阵对称，并且元素都是大于０]［正交矩阵，其逆矩阵和转置矩阵相等］
［右侧的Track拼写有误，应该写成Ｔrace］
[老师说，slam更偏向于一个工程问题而不是一个理论问题；基础知识都是那些固定的内容，只要静下心来都是可以推导出来的，都是固定套路]
［个人，老师说得或许没错，但是我目前连套路都没懂，怎么推导呢？］

未知对应点的求解方法

［ＥＭ算法是非线MLE的工具］
［slam,里面优化，常用到的方法LM,GS,Dosles］

1.2 PL-ICP匹配算法

数学描述

［由于激光数据散点都是离散的，导致了PP-ICP存在固有的缺陷；或者说激光雷达散点打到同一物体的不同实体部分，强行匹配是存在误差］［还是很迷］

算法流程：

［PL-ICP的开源代码案例，ros-kinect-csm］

1.3 基于优化的匹配方法

１）
[根据梯度进行的，例如hector-slam]

[T表示机器人位姿]［Ｓi(T)表示将激光数据转为世界坐标系或者地图坐标系下］［Ｍ(x)表示的占用概率，可以理解为似然场．（个人表示似然场没有理解）］

［上面公式是一个非线性，这是由其变换矩阵决定的；基本的逻辑思路：变换矩阵－－－＞梯度－－－＞似然场－－－＞地图栅格化－－－＞离散处理－－－＞地图进行插值］

２）优化的方法的求解

［不断迭代，Ｔ收敛后，就得到了机器人的位姿］［这里求解过程涉及非线性优化以及高斯牛顿知识］

［上图最下面的公式Ｈ，没有三角Ｔ的．老师又写错公式了］

［个人表示，这里的坐标点对变换矩阵求导没有理解］［老师的意思是，Ｔ表示ｘｙ＋角度变量三者［Ｔx,Ty,T角度］，对Ｔ求导，也就是Ｓi(T)依次对xy+角度三个变量进行求导，最后得到的是２ｘ３矩阵］

3)地图双性插值

插值方法：
使用拉格朗日插值法，对Ｘ，Ｙ两个方向进行插值，该方法属于一维线性插值的推广

［注意Ｌn(x) = yi的关系，从而得到基函数的条件］
拉格朗日插值方法－－基函数构造

［上图的第一句话，可以根据一维线性插值的基函数满足的条件进行理解］

［注意，四个点坐标的获取，并不是表示直接等号关系，而是一个－－－>转换关系，表示当x=x0，y=y0带入上面式子u,v时所计算出的结果］[但是构造基函数步骤，个人没有理解]

地图插值

［地图Ｍ对点ｐ的求导，等于（Ｌ(x,y)对ｘ的偏导数,L(x,y)对ｙ的偏导数）］

［基于优化方法，对初始值敏感，但是精度高.hector-slam，cartographer均使用到该方法］

1.4 相关匹配方法及分枝定界加速

1.4.1帧间匹配似然场

帧间匹配似然场（CSM）匹配特点：

１）高度非凸，存在很多的局部极值；
２）对初值非常敏感
３）进行暴力匹配，排除初值影响

４）通过加速策略，减低计算量
５）计算位姿匹配方差[老师认为这个方差计算非常重要]

算法流程：

算法流程：
１）构造似然场
２）在指定的搜索空间内，进行搜索，计算每一个位姿得分［每一个，表示的是暴力搜索；如何对其优化进行加速计算是算法的重点］
３）根据步骤２）中的位姿得分，计算本次位姿匹配的方差

1.4.2位姿搜索方法介绍

［１，２，３是更新策略的方式，３或者不是最优解，但是也是次优解，计算速率也可以加速；其中３是CSM常用的方法］

1.4.3分枝定界算法

１）常用的树形搜索剪枝算法
２）求解整数规划问题
３）解的数量为有限个

４）把最优解求解问题转换为树形搜索问题，根节点表示整个解空间，叶子节点表示最优解，中间的节点表示解空间的某一部分子空间

５）分枝：即根节点表示整个解空间，深度为１的节点表示解空间的子空间，深度为２的节点表示深度１空间的子空间，这样层层划分，直到划分到真实解，也就是叶子节点为止．

６）定界：对于搜索树中的每一个节点，确定以该节点为根节点的子树的界．对于最小值问题，确定下界；对于最大值问题，确定上界．（slam中为上界）［定界，可以类似大／小顶堆树结构理解］

［对于某一子树空间，其子树的根节点得分就是子树界，如果整树获取了当前最优解，而讨论的子树空间，其根节点得分也是比最优解得分低的，那么该子树空间也没有继续搜索的必要，也就是实现了不搜索操作，也就是分（剪）枝操作］

[分枝定界方法，跟前面的多分辨率搜索方法想相比，其能保证取到最优解]

［分支定界方法，会检索到叶子节点时，才会更新最优解；但是跟分枝操作是没有冲突的，因为第一次得到最优解时，必定检索到叶子节点才得到的，后面的子树空间也是根据当前已有的最优解进行确定是否进行检索的］

分枝定界算法的算法实现：

［bound表示分（剪）枝操作］

1.4.4分枝定界在相关方法的加速作用

［四个子节点，第一个叉乘表示四个节点意思，后面表示角度，最后的ch表示深度］

［图中左上的Ｗc是不考虑地图形状的；上图右下式子左侧Ｗ是考虑了地图形状的，式子最右侧的Ｗ是表示考虑地图形状的，如果画框超出了地图覆盖范围，自动删除超出部分；这也表明了双杆Ｗ画格范围比单杆Ｗ范围大的原因］

［从图中的右侧的关系式可以发现，粗分（画格）获得的分数比下面细分（画格）获得分数大；也可以认为，父节点得分比子节点得分要大］

［上面内容很多，很多没有理解］

分支定界方法的算法流程：

［右侧表示分枝定界在slam加速的全过程］［老师测试，分支定界方法加速效果是多分辨率搜索方法六倍左右］

#####################
图片版权归原作者所有
致谢曾老师的付出

不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈