激光slam学习日记——激光的前端配准算法

激光slam学习日记——激光的前端配准算法

一、ICP匹配方法
简单回顾一下：ICP算法实际就是两个位置观测到了同一块点云，然后把其中一个位置观测到的点云用我们估计的R 、t 投影到第二个位置观测的地方，因为R 、t 是不准确的，所以会与第二个位置的观测产生误差，ICP要做的就是是这个误差尽可能的小，使得R、t 变得很准确。

这里就是先求出两块点云的中心点坐标，分别是每个点与中心点做差（这样做可使得计算量变小，同时也方便计算），然后X与P的转置想乘实际就是两个点云的位姿关系：X = T * P；然后SVD分解就可以得到 R t 的值。下面会介绍怎么得到的R、t。

这部分通过简单的计算，转化为点云与中心点差的形式。此过程中分离了R、t，因为我们想使得式子的值最小，所以另第二项等于0，于是我们就得到了 t 的值t = ux - Rup。。就是上图那个

接下来我们计算R，先是简单的替换，然后展开，我们想要的是最小值，所以舍弃常数项，只保留第三项；因为第三项是负的，所以也就是要求去其绝对值的最大值；接下来是一个算是公式吧，简单点想，把R当成单位矩阵忽略不看，然后两个向量的乘积自然等于转置乘积的迹。。。整理一下我们就是相求Trace（RH）的最大值；
看右边公式：意思只要B是正交矩阵，A是正定矩阵就满足，然后看一下原公式，发现R就是正交矩阵（RRT = I，转过去转回来当然没动啊，哈哈 ），另外正交矩阵与正交矩阵相乘还是正交矩阵。看最后一个式子，BX= R的，那么我们如果想让这个迹的值最大，那是不是说我们让这个B等于单位矩阵就好，那么那么那么R是不是就等于X了，所以R的值我们也知道了，上面的结论可以安心用了。


二、PL-ICP（点到线）
pp-ICP表示点到点之间距离，如果匹配出现问题，匹配误差会干扰后面的优化。
PL-ICP表示点到线之间的距离，可以相对减少匹配误差的影响，通俗点就是投影过来两个点，然后求参考点到线段的距离。
右边公式中括号内就是相当于两点误差，然后外边乘一个法向量就可以当做相对于误差在法向量的投影，ab = a* cos,b是单位向量（法向量），a是误差向量。


三、基于优化的方法
优化的方法就是；首先将激光雷达数据进行T转换，然后计算坐标X的地图占用概率（也就是这个点位于当前地图中为真实坐标的概率，越大说明我们测量的越准），我们想让其最大，然后得到一个最小二乘问题，非线性的，所以为梯度求解也就是迭代的方式，由于地图中都是栅格化的（也就离散的），所以我们需要对其进行插值求解。

将非线性函数线性化（泰勒展开）：需要注意的是就是M对S的求导就是M的梯度
用deltaM表示了；得到式子之后，在对deltaT求导=0，就可以解deltaT，然后不断迭代求解。

简单计算就能得到deltaT。



这里注意：就是Li = Yi 就行，直接带入最后公式看看




四、相关方法

暴力匹配的方法计算量过于大，所以需要采取一些策略来进行加速。
CSM受限于分辨率

五、分支定界法（重点）

类似于四叉树，每个节点表这片区域的得分最高的解；太难了，我害得学学。
学完了，来更新一下。我就叫图像了，差不多意思都。
步骤一：根据我们估计的机器人位姿在上一帧图像中投影，我们选出一块区域。
步骤二：在这个区域我们分成四块（分枝），每块左上角对应一个位姿。
步骤三：分别计算这四个位姿在地图中的映射，把这些激光点映射到栅格地图中。
步骤四：计算每片栅格的得分（即这片中栅格的最大值：定界）。在这里我们会先设定一个初始的得分。
步骤五：分别与初始得分比较，得分低的直接砍掉（相当于砍枝），因为最佳位姿肯定不在这里。
步骤六：然后在进行搜索比较下面的直至找到最大得分那个值，对应的就是最佳位姿。