关于LOAM中的点到面/点到线距离的系数S的解析；以及最简单的LOAM雅克比矩阵的计算详解

一、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(dis_o)

fabs(pd2)/sqrt(dis_o)，这就是0.9后面的值，命为td2

其中dis_o=sqrt(point_sel.x * point_sel.x + point_sel.y * point_sel.y + point_sel.z * point_sel.z)，为点到原点(若为第一帧则为到激光雷达)的距离。

假设pd2恒为0.2，也就是说这个点(平面最近临点)也就是平面越远这个dis_o会越大，td2越小，s越接近1。

平面越近这个dis_o会越小，td2越大，s越小。可以理解为对平面的置信度，远处的平面对位姿求解起主要作用，这也符合我们的常规认知，远处的平面对位姿约束会好一些。

二、雅克比的求解

求解雅克比，首先要计算残差(还是以点到面为例)

残差为：

coeff.intensity = s * pd2

pd2为对(Rp_target+T)变换后的点云的距离，记Rp_target+T = T_c

记(pa, pb, pc, pd)^T为Vd

则dis =tr(T_c * 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/高斯牛顿等方法求解 J^TJ x = J^Te 便可以了