激光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受限于分辨率
在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

五、分支定界法(重点)

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

  • 0
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值