残差构建和雅克比推导

最近在阅读LIO-SAM源码的时候，在它这个后端优化部分这里，scan_to_map里面涉及到了一些数学原理，这部分作者写得和LOAM很像，同样采用了欧拉角来推导残差和雅可比矩阵，最后基于此构建增量方程进行求解。实际上在了解清楚原理之后，再去看这部分的源码的时候也是比较好理解的。因为这一部分相对来说比较多的细节，所以这里单独一节记录下来。这部分对应的代码在 LIO-SAM/src/mapOptmization.cpp的scan2MapOptimization()函数中。

点线残差构建

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

要计算点到直线的距离，只需要先计算三角形的面积，然后除以底边的模长即可

三角形面积计算
S△ABP∝∣PA→×PB→∣PA→×PB→=∣ijkx1−x0y1−y0z1−z0x2−x0y2−y0z2−z0∣=∣(y1−y0)(z2−z0)−(z1−z0)(y2−y0)(z1−z0)(x2−x0)−(x1−x0)(z2−z0)(x1−x0)(y2−y0)−(y1−y0)(x2−x0)∣∣PA→×PB→∣=(PA→×PB→)T(PA→×PB→)=((y1−y0)(z2−z0)−(z1−z0)(y2−y0))2+((z1−z0)(x2−x0)−(x1−x0)(z2−z0))2+((x1−x0)(y2−y0)−(y1−y0)(x2−x0))2\begin{aligned} &S_{\triangle A B P} \propto|\overrightarrow{P A} \times \overrightarrow{P B}|\\ &\overrightarrow{P A} \times \overrightarrow{P B}=\left|\begin{array}{ccc} i & j & k \\ x_1-x_0 & y_1-y_0 & z_1-z_0 \\ x_2-x_0 & y_2-y_0 & z_2-z_0 \end{array}\right|\\ &=\left|\begin{array}{c} \left(y_1-y_0\right)\left(z_2-z_0\right)-\left(z_1-z_0\right)\left(y_2-y_0\right) \\ \left(z_1-z_0\right)\left(x_2-x_0\right)-\left(x_1-x_0\right)\left(z_2-z_0\right) \\ \left(x_1-x_0\right)\left(y_2-y_0\right)-\left(y_1-y_0\right)\left(x_2-x_0\right) \end{array}\right|\\ &\mid\overrightarrow{P A} \times \overrightarrow{P B} \mid=(\overrightarrow{P A} \times \overrightarrow{P B})^T(\overrightarrow{P A} \times \overrightarrow{P B})\\ &=\left(\left(y_1-y_0\right)\left(z_2-z_0\right)-\left(z_1-z_0\right)\left(y_2-y_0\right)\right)^2\\ &+\left(\left(z_1-z_0\right)\left(x_2-x_0\right)-\left(x_1-x_0\right)\left(z_2-z_0\right)\right)^2\\ &+\left(\left(x_1-x_0\right)\left(y_2-y_0\right)-\left(y_1-y_0\right)\left(x_2-x_0\right)\right)^2 \end{aligned} S△ABP∝∣PA×PB∣PA×PB=∣∣ix1−x0x2−x0jy1−y0y2−y0kz1−z0z2−z0∣∣=∣∣(y1−y0)(z2−z0)−(z1−z0)(y2−y0)(z1−z0)(x2−x0)−(x1−x0)(z2−z0)(x1−x0)(y2−y0)−(y1−y0)(x2−x0)∣∣∣PA×PB∣=(PA×PB)T(PA×PB)=((y1−y0)(z2−z0)−(z1−z0)(y2−y0))2+((z1−z0)(x2−x0)−(x1−x0)(z2−z0))2+((x1−x0)(y2−y0)−(y1−y0)(x2−x0))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;

残差对点的雅可比矩阵

分为两步，先对点求导，再对位姿求导
J=∂d∂Twb=∂d∂pw∂pw∂TwbJ=\frac{\partial d}{\partial T_{w b}}=\frac{\partial d}{\partial \boldsymbol{p}^w} \frac{\partial \boldsymbol{p}^w}{\partial \boldsymbol{T}_{w b}} J=∂Twb∂d=∂pw∂d∂Twb∂pw
设 pm=PM→=PA→×PB→\boldsymbol{p}_m=\overrightarrow{P M}=\overrightarrow{P A} \times \overrightarrow{P B}pm=PM=PA×PB ，残差为:
d∝((y1−y0)(z2−z0)−(z1−z0)(y2−y0))2+((z1−z0)(x2−x0)−(x1−x0)(z2−z0))2+((x1−x0)(y2−y0)−(y1−y0)(x2−x0))2=pmTpm=(pmx2+pmy2+pmz2)\begin{aligned} &d \propto \sqrt{\begin{array}{l} \left(\left(y_1-y_0\right)\left(z_2-z_0\right)-\left(z_1-z_0\right)\left(y_2-y_0\right)\right)^2 \\ +\left(\left(z_1-z_0\right)\left(x_2-x_0\right)-\left(x_1-x_0\right)\left(z_2-z_0\right)\right)^2 \\ +\left(\left(x_1-x_0\right)\left(y_2-y_0\right)-\left(y_1-y_0\right)\left(x_2-x_0\right)\right)^2 \end{array}}\\ &=\sqrt{\boldsymbol{p}_m^T \boldsymbol{p}_m}\\ &=\sqrt{\left(p_{m x}^2+p_{m y}^2+p_{m z}^2\right)} \end{aligned} d∝((y1−y0)(z2−z0)−(z1−z0)(y2−y0))2+((z1−z0)(x2−x0)−(x1−x0)(z2−z0))2+((x1−x0)(y2−y0)−(y1−y0)(x2−x0))2=pmTpm=(pmx2+pmy2+pmz2)
得到残差的表达式之后，分别对(x0,y0,z0)求导可得
∂d∂pw=[∂d∂x0∂d∂y0∂d∂z0]T∝12[2pmy((z2−z0)−(z1−z0))+2pmz((y1−y0)−(y2−y0))2pmz((x2−x0)−(x1−x0))+2pmx((z1−z0)−(z2−z0))2pmx((y2−y0)−(y1−y0))+2pmy((x1−x0)−(x2−x0))]T=[pmy(z2−z1)+pmz(y1−y2)pmz(x2−x1)+pmx(z1−z2)pmx(y2−y1)+pmy(x1−x2)]\begin{aligned} \frac{\partial d}{\partial \boldsymbol{p}^w} &=\left[\begin{array}{l} \frac{\partial d}{\partial x_0} \\ \frac{\partial d}{\partial y_0} \\ \frac{\partial d}{\partial z_0} \end{array}\right]^T \\ & \propto \frac{1}{2}\left[\begin{array}{c} 2 p_{m y}\left(\left(z_2-z_0\right)-\left(z_1-z_0\right)\right)+2 p_{m z}\left(\left(y_1-y_0\right)-\left(y_2-y_0\right)\right) \\ 2 p_{m z}\left(\left(x_2-x_0\right)-\left(x_1-x_0\right)\right)+2 p_{m x}\left(\left(z_1-z_0\right)-\left(z_2-z_0\right)\right) \\ 2 p_{m x}\left(\left(y_2-y_0\right)-\left(y_1-y_0\right)\right)+2 p_{m y}\left(\left(x_1-x_0\right)-\left(x_2-x_0\right)\right) \end{array}\right]^T \\ &=\left[\begin{array}{l} p_{m y}\left(z_2-z_1\right)+p_{m z}\left(y_1-y_2\right) \\ p_{m z}\left(x_2-x_1\right)+p_{m x}\left(z_1-z_2\right) \\ p_{m x}\left(y_2-y_1\right)+p_{m y}\left(x_1-x_2\right) \end{array}\right] \end{aligned} ∂pw∂d=⎣⎡∂x0∂d∂y0∂d∂z0∂d⎦⎤T∝21⎣⎡2pmy((z2−z0)−(z1−z0))+2pmz((y1−y0)−(y2−y0))2pmz((x2−x0)−(x1−x0))+2pmx((z1−z0)−(z2−z0))2pmx((y2−y0)−(y1−y0))+2pmy((x1−x0)−(x2−x0))⎦⎤T=⎣⎡pmy(z2−z1)+pmz(y1−y2)pmz(x2−x1)+pmx(z1−z2)pmx(y2−y1)+pmy(x1−x2)⎦⎤
对应到代码中为

// 计算残差对点的雅可比矩阵并进行单位化
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;

点面残差构建

点到的平面的距离作为残差，对系数进行归一化处理，则计算方式为：
dh=∣pa⋅x0+pb⋅y0+pc⋅z0+pd∣pa2+pb2+pc2=∣pa⋅x0+pb⋅y0+pc⋅z0+pd∣d_h=\frac{\left|p a \cdot x_0+p b \cdot y_0+p c \cdot z_0+p d\right|}{\sqrt{p a^2+p b^2+p c^2}}=\left|p a \cdot x_0+p b \cdot y_0+p c \cdot z_0+p d\right| dh=pa2+pb2+pc2∣pa⋅x0+pb⋅y0+pc⋅z0+pd∣=∣pa⋅x0+pb⋅y0+pc⋅z0+pd∣

// 五个点组成5个约束，三个变量
Eigen::Matrix<float, 5, 3> matA0;
Eigen::Matrix<float, 5, 1> matB0;
Eigen::Vector3f matX0;matA0.setZero();
matB0.fill(-1);
matX0.setZero();// 要求距离都小于1m
if (pointSearchSqDis[4] < 1.0) {for (int j = 0; j < 5; j++) {  // 构建超定方程组matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;}// 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1matX0 = matA0.colPivHouseholderQr().solve(matB0);// 平面方程的系数，也是法向量的分量float pa = matX0(0, 0);float pb = matX0(1, 0);float pc = matX0(2, 0);float pd = 1;// 单位法向量float ps = sqrt(pa * pa + pb * pb + pc * pc);pa /= ps; pb /= ps; pc /= ps; pd /= ps;// 检查平面是否合格，如果5个点中有点到平面的距离超过0.2m，那么认为这些点太分散了，不构成平面bool planeValid = true;for (int j = 0; j < 5; j++) {if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {planeValid = false;break;}}// 平面合格if (planeValid) {// 当前激光帧点到平面距离float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;// 惩罚系数float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));// 点到平面垂线单位法向量（其实等价于平面法向量）coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;// 点到平面距离coeff.intensity = s * pd2;if (s > 0.1) {// 当前激光帧平面点，加入匹配集合中laserCloudOriSurfVec[i] = pointOri;// 参数coeffSelSurfVec[i] = coeff;laserCloudOriSurfFlag[i] = true;}}

残差对点的雅可比矩阵

根据残差的表达式
dh=∣pa⋅x0+pb⋅y0+pc⋅z0+pd∣pa2+pb2+pc2=∣pa⋅x0+pb⋅y0+pc⋅z0+pd∣d_h=\frac{\left|p a \cdot x_0+p b \cdot y_0+p c \cdot z_0+p d\right|}{\sqrt{p a^2+p b^2+p c^2}}=\left|p a \cdot x_0+p b \cdot y_0+p c \cdot z_0+p d\right| dh=pa2+pb2+pc2∣pa⋅x0+pb⋅y0+pc⋅z0+pd∣=∣pa⋅x0+pb⋅y0+pc⋅z0+pd∣
残差表达式分别对（x0,y0,z0）求导可以看出，雅可比矩阵就是平面的法向量，所以

// 用法向量作为雅可比，方向由 pd2 间接决定
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;

残差对位移的雅可比

因为
J=∂d∂Twb=∂d∂pw∂pw∂TwbJ=\frac{\partial d}{\partial T_{w b}}=\frac{\partial d}{\partial \boldsymbol{p}^w} \frac{\partial \boldsymbol{p}^w}{\partial \boldsymbol{T}_{w b}} J=∂Twb∂d=∂pw∂d∂Twb∂pw
实际上我们要求的是
mat⁡A=[∂d∂roll ∂d∂pitch∂d∂yaw∂d∂tx∂d∂ty∂d∂tz]\operatorname{mat} A=\left[\begin{array}{c} \frac{\partial d}{\partial \text { roll }} \\ \frac{\partial d}{\partial p i t c h} \\ \frac{\partial d}{\partial y a w} \\ \frac{\partial d}{\partial t_x} \\ \frac{\partial d}{\partial t_y} \\ \frac{\partial d}{\partial t_z} \end{array}\right] matA=⎣⎡∂ roll ∂d∂pitch∂d∂yaw∂d∂tx∂d∂ty∂d∂tz∂d⎦⎤
通过上面的几步求得了残差对点的雅可比，下面求点对位姿的雅可比即可

设 Lidar 相对于世界坐标系旋转矩阵为R ，平移为t ，将 Lidar 坐标系下的点转为世界坐标系的过程为：
pw=Rp+t\boldsymbol{p}^w=\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t} pw=Rp+t
因为对平移的雅可比矩阵即为：
∂pw∂t=∂Rp+t∂t=I\frac{\partial p^w}{\partial t}=\frac{\partial \boldsymbol{R} p+t}{\partial t}=\boldsymbol{I} ∂t∂pw=∂t∂Rp+t=I
所以
Jt=∂d∂twb=∂d∂pw∂pw∂t=∂d∂pwJ_t=\frac{\partial d}{\partial t_{w b}}=\frac{\partial d}{\partial \boldsymbol{p}^w} \frac{\partial \boldsymbol{p}^w}{\partial \boldsymbol{t}} = \frac{\partial d}{\partial \boldsymbol{p}^w} Jt=∂twb∂d=∂pw∂d∂t∂pw=∂pw∂d
对应到代码中即为：

// 残差对位移的求导
matA.at<float>(i, 3) = coeff.z;
matA.at<float>(i, 4) = coeff.x;
matA.at<float>(i, 5) = coeff.y;

残差对旋转的雅可比

LOAM 原作者选择的是 ZXY 外旋（沿固定 Z 轴旋转-> 沿固定 X 轴旋转 -> 沿固定 Y 轴旋转）表示，这里作者也沿用了这种旋转方式，因此 ZXY 外旋的旋转矩阵的计算方式为：R=RyRxRz，即：
R=RyRxRz=[cos⁡(ry)0sin⁡(ry)010−sin⁡(ry)0cos⁡(ry)][1000cos⁡(rx)−sin⁡(rx)0sin⁡(rx)cos⁡(rx)][cos⁡(rz)−sin⁡(rz)0sin⁡(rz)cos⁡(rz)0001]=[cos⁡(ry)cos⁡(rz)+sin⁡(ry)sin⁡(rx)sin⁡(rz)cos⁡(rz)sin⁡(ry)sin⁡(rx)−cos⁡(ry)sin⁡(rz)cos⁡(rx)sin⁡(ry)cos⁡(rx)sin⁡(rz)cos⁡(rx)cos⁡(rz)−sin⁡(rx)cos⁡(ry)sin⁡(rx)sin⁡(rz)−cos⁡(rz)sin⁡(ry)cos⁡(ry)cos⁡(rz)sin⁡(rx)+sin⁡(ry)sin⁡(rz)cos⁡(ry)cos⁡(rx)]\begin{aligned} \boldsymbol{R} &=\boldsymbol{R}_y \boldsymbol{R}_x \boldsymbol{R}_z \\ &=\left[\begin{array}{ccc} \cos (r y) & 0 & \sin (r y) \\ 0 & 1 & 0 \\ -\sin (r y) & 0 & \cos (r y) \end{array}\right]\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos (r x) & -\sin (r x) \\ 0 & \sin (r x) & \cos (r x) \end{array}\right]\left[\begin{array}{ccc} \cos (r z) & -\sin (r z) & 0 \\ \sin (r z) & \cos (r z) & 0 \\ 0 & 0 & 1 \end{array}\right] \\ &=\left[\begin{array}{cccc} \cos (r y) \cos (r z)+\sin (r y) \sin (r x) \sin (r z) & \cos (r z) \sin (r y) \sin (r x)-\cos (r y) \sin (r z) & \cos (r x) \sin (r y) \\ \cos (r x) \sin (r z) & \cos (r x) \cos (r z) & -\sin (r x) \\ \cos (r y) \sin (r x) \sin (r z)-\cos (r z) \sin (r y) & \cos (r y) \cos (r z) \sin (r x)+\sin (r y) \sin (r z) & \cos (r y) \cos (r x) \end{array}\right] \end{aligned} R=RyRxRz=⎣⎡cos(ry)0−sin(ry)010sin(ry)0cos(ry)⎦⎤⎣⎡1000cos(rx)sin(rx)0−sin(rx)cos(rx)⎦⎤⎣⎡cos(rz)sin(rz)0−sin(rz)cos(rz)0001⎦⎤=⎣⎡cos(ry)cos(rz)+sin(ry)sin(rx)sin(rz)cos(rx)sin(rz)cos(ry)sin(rx)sin(rz)−cos(rz)sin(ry)cos(rz)sin(ry)sin(rx)−cos(ry)sin(rz)cos(rx)cos(rz)cos(ry)cos(rz)sin(rx)+sin(ry)sin(rz)cos(rx)sin(ry)−sin(rx)cos(ry)cos(rx)⎦⎤
又因为平移对旋转的求导为0，所以可以得到：

这里首先对rx进行求导
∂(Rp)∂rx=[(sin⁡(ry)cos⁡(rx)sin⁡(rz))px+(cos⁡(rz)sin⁡(ry)cos⁡(rx))py−(sin⁡(rx)sin⁡(ry))pz−(sin⁡(rx)sin⁡(rz))px−(sin⁡(rx)cos⁡(rz))py−(cos⁡(rx))pz(cos⁡(ry)cos⁡(rx)sin⁡(rz))px+(cos⁡(ry)cos⁡(rz)cos⁡(rx))py−(cos⁡(ry)sin⁡(rx))pz]\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial r x}=\left[\begin{array}{c} (\sin (r y) \cos (r x) \sin (r z)) p_x+(\cos (r z) \sin (r y) \cos (r x)) p_y-(\sin (r x) \sin (r y)) p_z \\ -(\sin (r x) \sin (r z)) p_x-(\sin (r x) \cos (r z)) p_y-(\cos (r x)) p_z \\ (\cos (r y) \cos (r x) \sin (r z)) p_x+(\cos (r y) \cos (r z) \cos (r x)) p_y-(\cos (r y) \sin (r x)) p_z \end{array}\right] ∂rx∂(Rp)=⎣⎡(sin(ry)cos(rx)sin(rz))px+(cos(rz)sin(ry)cos(rx))py−(sin(rx)sin(ry))pz−(sin(rx)sin(rz))px−(sin(rx)cos(rz))py−(cos(rx))pz(cos(ry)cos(rx)sin(rz))px+(cos(ry)cos(rz)cos(rx))py−(cos(ry)sin(rx))pz⎦⎤
这里得到的是点对旋转的雅可比，再右乘以残差对点的雅可比即（coeff.x，coeff.y，coeff.z）即可得到残差对旋转的雅可比，在代码中就是：

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;

同理，对ry的求导也是如法炮制：
∂(Rp)∂ry=[(−sin⁡(ry)cos⁡(rz)+cos⁡(ry)sin⁡(rx)sin⁡(rz))px+(cos⁡(rz)cos⁡(ry)sin⁡(rx)+sin⁡(ry)sin⁡(rz))py+(cos⁡(rx)cos⁡(ry))pz0(−sin⁡(ry)sin⁡(rx)sin⁡(rz)−cos⁡(rz)cos⁡(ry))px+(−sin⁡(ry)cos⁡(rz)sin⁡(rx)+cos⁡(ry)sin⁡(rz))py−(sin⁡(ry)cos⁡(rx))pz]\begin{aligned} \frac{\partial(\boldsymbol{R} p)}{\partial r y} =\left[\begin{array}{c} (-\sin (r y) \cos (r z)+\cos (r y) \sin (r x) \sin (r z)) p_x +(\cos (r z) \cos (r y) \sin (r x)+\sin (r y) \sin (r z)) p_y+(\cos (r x) \cos (r y)) p_z \\ 0 \\ (-\sin (r y) \sin (r x) \sin (r z)-\cos (r z) \cos (r y)) p_x+(-\sin (r y) \cos (r z) \sin (r x)+\cos(r y) \sin (r z)) p_y-(\sin (r y) \cos (r x)) p_z \end{array}\right] \end{aligned} ∂ry∂(Rp)=⎣⎡(−sin(ry)cos(rz)+cos(ry)sin(rx)sin(rz))px+(cos(rz)cos(ry)sin(rx)+sin(ry)sin(rz))py+(cos(rx)cos(ry))pz0(−sin(ry)sin(rx)sin(rz)−cos(rz)cos(ry))px+(−sin(ry)cos(rz)sin(rx)+cos(ry)sin(rz))py−(sin(ry)cos(rx))pz⎦⎤
同样地，再右乘以残差对点的雅可比即（coeff.x，coeff.y，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;

同理，对rz的求导也是如法炮制：
∂(Rp)∂rz=[(−cos⁡(ry)sin⁡(rz)+sin⁡(ry)sin⁡(rx)cos⁡(rz))px+(−sin⁡(rz)sin⁡(ry)sin⁡(rx)−cos⁡(ry)cos⁡(rz))py(cos⁡(rx)cos⁡(rz))px−(cos⁡(rx)sin⁡(rz))py(cos⁡(ry)sin⁡(rx)cos⁡(rz)+sin⁡(rz)sin⁡(ry))px+(−cos⁡(ry)sin⁡(rz)sin⁡(rx)+sin⁡(ry)cos⁡(rz))py]\begin{aligned} \frac{\partial(\boldsymbol{R} p)}{\partial r z} =\left[\begin{array}{c} (-\cos (r y) \sin (r z)+\sin (r y) \sin (r x) \cos (r z)) p_x+(-\sin (r z) \sin (r y) \sin (r x)-\cos (r y) \cos (r z)) p_y \\ (\cos (r x) \cos (r z)) p_x-(\cos (r x) \sin (r z)) p_y \\ (\cos (r y) \sin (r x) \cos (r z)+\sin (r z) \sin (r y)) p_x+(-\cos (r y) \sin (r z) \sin (r x)+\sin (r y) \cos (r z)) p_y \end{array}\right] \end{aligned} ∂rz∂(Rp)=⎣⎡(−cos(ry)sin(rz)+sin(ry)sin(rx)cos(rz))px+(−sin(rz)sin(ry)sin(rx)−cos(ry)cos(rz))py(cos(rx)cos(rz))px−(cos(rx)sin(rz))py(cos(ry)sin(rx)cos(rz)+sin(rz)sin(ry))px+(−cos(ry)sin(rz)sin(rx)+sin(ry)cos(rz))py⎦⎤
同样地，再右乘以残差对点的雅可比即（coeff.x，coeff.y，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;

构建增量方程

在得到残差对位姿的雅可比矩阵之后，就可以构建增量方程了，构建增量方程之后，使用QR分解的方式进行求解。这里还做了一个退化检测，如果检测到信息矩阵发生退化就会进入退化处理，对之前求解出来的解进行一个纠正。如下：

cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
// J^T·J·delta_x = -J^T·f QR分解进行求解
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);// 首次迭代，检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，主要检查信息矩阵的奇异值是否过小
if (iterCount == 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0)); // 存放特征值cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0)); // 存放特征向量cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));cv::eigen(matAtA, matE, matV);  matV.copyTo(matV2);isDegenerate = false;float eignThre[6] = {100, 100, 100, 100, 100, 100};for (int i = 5; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;  // 对于奇异值过小的特征向量直接赋0}isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;
}
if (isDegenerate)
{cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;   // 实际上就是对于奇异值过小的对应的解就直接赋0，其他解按原增量方程解出来是什么就是什么
}

更新位姿

上面求解得到增量位姿之后，最后一步把它叠加到之前我们通过IMU得到的那个初始值上作为一个scan_to_map的结果

// 更新当前关键帧全局位姿估计
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);

总结

scan_to_map的目的是对我们从IMU里程计得到的 transformTobeMapped 当前帧位姿进行进一步优化。首先，我们用当前帧的特征点去局部地图（这个地图由时空上的近邻帧构成）中构建点到线的距离、点到面的距离，以此作为残差以优化当前帧位姿；然后，我们通过链式法则求残差对位移、旋转的雅可比矩阵；最后利用雅可比矩阵和观测（残差）构建增量方程，用QR分解求解方程得到增量解，叠加到transformTobeMapped 即得到了一个比从里程计来更准确的位姿。

LIO-SAM中的scan_to_map原理剖析相关推荐

Golang中context实现原理剖析
转载: Go 并发控制context实现原理剖析 1. 前言 Golang context是Golang应用开发常用的并发控制技术,它与WaitGroup最大的不同点是context对于派生gorou ...
python中遇到循环import即circular import的问题原理剖析及解决方案
在python中常常会遇到循环import即circular import的问题,今天主要给大家介绍了关于Python中循环引用(import)失败的解决方法,文中通过示例代码介绍的非常详细,需要的朋 ...
socket之send和recv原理剖析
socket之send和recv原理剖析 1. 认识TCP socket的发送和接收缓冲区当创建一个TCP socket对象的时候会有一个发送缓冲区和一个接收缓冲区,这个发送和接收缓冲区指的就是内存 ...
fastText的原理剖析
fastText的原理剖析 1. fastText的模型架构 fastText的架构非常简单,有三层:输入层.隐含层.输出层(Hierarchical Softmax) 输入层:是对文档embeddi ...
lua游戏脚本实例源码_Lua与其他宿主语言交互原理剖析
Lua与其他宿主语言交互原理剖析题外话:今天周末,刚好在家有时间就把我这次项目组内部分享的文章贴出来,分享给大家,同时也方便以后自己翻阅. 一. Lua简介目标:Lua语言本身是用C语言来编写开发 ...
彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
视觉三维重建 = 定位定姿 + 稠密重建 + surface reconstruction +纹理贴图.三维重建技术是计算机视觉的重要技术之一,基于视觉的三维重建技术通过深度数据获取.预处理.点云配准 ...
Elasticsearch分布式一致性原理剖析(一)-节点篇
2019独角兽企业重金招聘Python工程师标准>>> 摘要: ES目前是最流行的开源分布式搜索引擎系统,其使用Lucene作为单机存储引擎并提供强大的搜索查询能力.学习其搜索原理, ...
java 反序列化 ysoserial exploit/JRMPListener 原理剖析
目录 0 前言 1 payloads/JRMPClient 1.1 Externalizable 1.2 生成payload 1.3 gadget链分析 2 exploit/JRMPListener ...
统计学习方法|支持向量机(SVM)原理剖析及实现
欢迎直接到我的博客查看最近文章:www.pkudodo.com.更新会比较快,评论回复我也能比较快看见,排版也会更好一点. 原始blog链接: http://www.pkudodo.com/2018/ ...

LIO-SAM中的scan_to_map原理剖析

目录：

残差构建和雅克比推导

点线残差构建

残差对点的雅可比矩阵

点面残差构建

残差对点的雅可比矩阵

残差对位移的雅可比

残差对旋转的雅可比

构建增量方程

更新位姿

总结

LIO-SAM中的scan_to_map原理剖析相关推荐

最新文章

热门文章