从图形和算法层面来看LIO-SAM残差问题

Posted 2023-02-05

0. 简介

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

1. 算法层面代码推导

点线残差的定义为点到线的距离，在该直线上取两个点 ，设当前点为 ，如下

其中我们要计算的话其实只需要计算的面积，除以的长度即可，即：

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

// 计算 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;

然后下面就是雅克比的推导计算点线雅可比计算。步骤主要分为两步，先对点求导，再对位姿求导：

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

为了更方便表示，可以设置，这样既可得：

然后下面就是对不同轴进行偏导，并得到偏导结果

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

// 计算残差对点 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. 图像层面代码推导

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

// 计算 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;

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

根据上面的代码可以推导出的向量，也就是计算出三个方向上的残差

3. 外参处理偏导

在做到这一步后，剩下的就是后半部分，这部分主要是点与外参的偏导关系。设 Lidar 相对于世界坐标系旋转矩阵为 ，平移为 ，将 Lidar 坐标系下的点转为世界坐标系的过程为：

对平移的推导很简单，即

// 用法向量作为雅可比，方向由 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;

…详情请参照​​古月居​​