LOAM中的点到面/点到先距离的S;雅克比计算详解
2022/4/12 6:12:42
本文主要是介绍LOAM中的点到面/点到先距离的S;雅克比计算详解,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!
一、S的意义
以下是某注释对S的解释(以点到面为例)
//这里的s是个权重, 表示s在这个least-square问题中的置信度, 每个点的置信度不一样 //理论上, 这个权重, 与点到面距离负相关, 距离越大, 置信度越低, 这里相当于是一个在loss之外加了一个鲁棒性函数, 用来过减弱离群值的影响 //源代码中"sqrt(sqrt(point_sel.x * point_sel.x + point_sel.y * point_sel.y + point_sel.z * point_sel.z)" 这部分, 并没有什么逻辑性可言 //你可以设计自己的鲁棒性函数来替代这一行代码
点到平面距离公式
(1)
float pa = mat_x0.at<float>(0, 0); float pb = mat_x0.at<float>(1, 0); float pc = mat_x0.at<float>(2, 0); float pd = 1; //pa pb pc pd对应AX+BY+C+1=0 中的A B C和1 float ps = sqrt(pa * pa + pb * pb + pc * pc); pa /= ps; pb /= ps; pc /= ps; pd /= ps; //这里的ps 对应式(1)的分母部分 //由此点到平面距离便变为pa * x +pb * y + pc * z + pd, 在进行平面残差计算前首先计算点到平面距离,以此判断点是否是附近的平面点 fabs(pa * x +pb * y + pc * z + pd) > 0.2 //点到平面距离 float pd2 = pa * x + pb * y + pc * z + pd; float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(point_sel.x * point_sel.x + point_sel.y * point_sel.y + point_sel.z * point_sel.z));
我们来分析以下这个S的变换,显然fabs(pd2)/sqrt(sqrt(point_sel.x * point_sel.x + point_sel.y * point_sel.y + point_sel.z * point_sel.z))
= fabs(pd2)/sqrt(diso)
fabs(pd2)/sqrt(diso),这就是0.9后面的值,命为td2
其中diso=sqrt(point_sel.x * point_sel.x + point_sel.y * point_sel.y + point_sel.z * point_sel.z),为点到原点(若为第一帧则为到激光雷达)的距离。
假设pd2恒为0.2,也就是说这个点(平面最近临点)也就是平面越远这个diso会越大,td2越小,s越接近1。
平面越近这个diso会越小,td2越大,s越小。可以理解为对平面的置信度,远处的平面对位姿求解起主要作用,这也符合我们的常规认知,远处的平面对位姿约束会好一些。
二、雅克比的求解
求解雅克比,首先要计算残差(还是以点到面为例)
残差为:
coeff.intensity = s * pd2
pd2为对(Rptarget+T)变换后的点云的距离,记Rptarget+T = Tc
记(pa, pb, pc, pd)T为Vd
则dis =tr(Tc * Vd)
对其求导得旋转RPY的导数为(-RP)^*Vd
求平移矩阵的导数为Vd
便可得到雅克比,另外记得还要乘以s
coeff.x = s * pa; coeff.y = s * pb; coeff.z = s * pc; float_t arx = - s * pb * jacobin_R_point[2] + s * pc * jacobin_R_point[1]; float_t ary = s * pa * jacobin_R_point[2] - s * pc * jacobin_R_point[0]; float_t arz = - s * pa * jacobin_R_point[1] + s * pb * jacobin_R_point[0];然后用LM/高斯牛顿等方法求解 JTJ x = JTe 便可以了
这篇关于LOAM中的点到面/点到先距离的S;雅克比计算详解的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!
- 2024-05-13TiDB + ES:转转业财系统亿级数据存储优化实践
- 2024-05-09“2024鸿蒙零基础快速实战-仿抖音App开发(ArkTS版)”实战课程已上线
- 2024-05-09聊聊如何通过arthas-tunnel-server来远程管理所有需要arthas监控的应用
- 2024-05-09log4j2这么配就对了
- 2024-05-09nginx修改Content-Type
- 2024-05-09Redis多数据源,看这篇就够了
- 2024-05-09Google Chrome驱动程序 124.0.6367.62(正式版本)去哪下载?
- 2024-05-09有没有大佬知道这种数据应该怎么抓取呀?
- 2024-05-09这种运行结果里的10.100000001,怎么能最快改成10.1?
- 2024-05-09企业src漏洞挖掘-有意思的命令执行