0. 简介

前段时间海龙老哥找我梳理。LIO-SAM中点云配准之角点面点的残差及梯度构建的算法，本人之前都是从算法层面来理解残差问题的。所以这里结合海龙老哥讨论的图形层面来分别看待LIO-SAM残差问题。阅读LIO-SAM源码的时候，发现点线残差和点面残差和雅克比构建采用了LOAM的表示方法。这里我们以电线残差的构建完成从图形和算法层面来看LIO-SAM残差问题。

1. 算法层面代码推导                                                                        

点线残差的定义为点到线的距离，在该直线上取两个点 A=(x1,y1,z1), B=(x2,y2,z2)，设当前点为 P=(x0,y0,z0)，如下

其中我们要计算$d$的话其实只需要计算\triangle{PAB}的面积，除以$AB$的长度即可，即：
\mathrm{d}=\frac{|\mathbf{PA} \times \mathbf{PB}|}{|\mathbf{AB}|}

从这里来看，我们就会明白代码中的操作了

// 计算 PA x PB 的模长
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))     
            + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))    
            + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); 

// 计算 AB 模长
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

// P 到 AB 的距离
float ld2 = a012 / l12;

然后下面就是雅克比的推导计算点线雅可比计算。步骤主要分为两步，先对点求导，再对位姿求导：
J=\frac{\partial d}{\partial T_{w b}}=\frac{\partial d}{\partial \boldsymbol{p}^{w}} \frac{\partial \boldsymbol{p}^{w}}{\partial \boldsymbol{T}_{w b}}

其中$d$代表了上式中的三角形高度，可以用上式的模长表示

为了更方便表示，可以设置\boldsymbol{p}_{m}=\overrightarrow{P M}=\overrightarrow{P A} \times \overrightarrow{P B}，这样既可得：

然后下面就是对不同轴进行偏导，并得到\boldsymbol{p}^w=(x0,y0,z0)偏导结果

对应代码中，代码中对向量|AB||PM|除是为了进行单位化

// 计算残差对点 P 的雅可比且进行单位化
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
      + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
       - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
       + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

2. 图像层面代码推导

一开始和我们上面公式层面代码推导一致，其实求的就是\triangle{OAB}面积，以及AB的模长

// 计算 PA x PB 的模长
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))     
            + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))    
            + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); 

// 计算 AB 模长
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

// P 到 AB 的距离
float ld2 = a012 / l12;

然后我们同时明白一点，可以就是|\mathbf{OA} \times \mathbf{OB}|代表了面积，但是\mathbf{OA} \times \mathbf{OB}可以则可以看做一个向量，然后向量与向量的计算可以完成方向上模长的计算

根据上面的代码可以推导出\overrightarrow{B A} \times \overrightarrow{OC}的向量，也就是计算出三个方向上的d残差

3. 外参处理偏导

在做到这一步后，剩下的就是后半部分\frac{\partial \boldsymbol{p}^{w}}{\partial \boldsymbol{T}_{w b}}，这部分主要是点与外参的偏导关系。设 Lidar 相对于世界坐标系旋转矩阵为R ，平移为t ，将 Lidar 坐标系下的点转为世界坐标系的过程为：
\boldsymbol{p}^{w}=\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t}
对平移的推导很简单，即
\frac{\partial \boldsymbol{p}^{w}}{\partial \boldsymbol{t}}=\frac{\partial \boldsymbol{R} \boldsymbol{p}+\boldsymbol{t}}{\partial \boldsymbol{t}}=\boldsymbol{I}

// 用法向量作为雅可比，方向由 pd2 间接决定
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
//..........即得到
matA.at<float>(i, 3) = coeff.z;
matA.at<float>(i, 4) = coeff.x;
matA.at<float>(i, 5) = coeff.y;

然后下面就是对对旋转的雅可比。LOAM 原作者选择的是 ZXY 外旋（沿固定 Z 轴旋转\rightarrow沿固定 X 轴旋转\rightarrow沿固定 Y 轴旋转）表示，由于外旋的旋转矩阵顺序为作乘，因此 ZXY 内旋的旋转矩阵的计算方式为：R=RyRxRz，同时平移对旋转求导为0，忽略即：

点对rx求雅可比有：

点对ry求雅可比有：

点对rz求雅可比有：

至此雅可比推导完毕，对应的代码如下

float arx = (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y - srx * sry * pointOri.z) * coeff.x + (-srx * srz * pointOri.x - crz * srx * pointOri.y - crx * pointOri.z) * coeff.y + (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y - cry * srx * pointOri.z) * coeff.z;

float ary = ((cry * srx * srz - crz * sry) * pointOri.x + (sry * srz + cry * crz * srx) * pointOri.y + crx * cry * pointOri.z) * coeff.x + ((-cry * crz - srx * sry * srz) * pointOri.x + (cry * srz - crz * srx * sry) * pointOri.y - crx * sry * pointOri.z) * coeff.z;

float arz = ((crz * srx * sry - cry * srz) * pointOri.x + (-cry * crz - srx * sry * srz) * pointOri.y) * coeff.x + (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y + ((sry * srz + cry * crz * srx) * pointOri.x + (crz * sry - cry * srx * srz) * pointOri.y) * coeff.z;

4. 参考链接

https://blog.csdn.net/qq_32761549/article/details/126641834
https://zhuanlan.zhihu.com/p/548579394