LEGO-LOAM（LOAM）部分公式推导---未完待续

一、featureAssociation相关推导

1）帧间匹配雅可比矩阵推导

首先明确LEGO-LOAM中，运动坐标系（符合右手系）的设置为：

因此对于平面运动来说，影响最大的三个分量为tx,tz,ryt_x,t_z,r_ytx,tz,ry，因此仅考虑这三个分量对雅可比矩阵，也就是对非线性优化问题的贡献。首先让我们考虑整个非线性优化过程，首先将本帧点ppp转换至上一帧坐标系中，得到p′p'p′，即

p′=Tpp' = Tpp′=Tp

通过计算p′p'p′点和其近邻线（以corner为例，存在于上一帧点云系中）的点线间距离，得到残差fff，即

f=dist(p′,line)=dist(Tp,line)f = dist(p', line) = dist(Tp, line)f=dist(p′,line)=dist(Tp,line)

于是雅可比矩阵可以写为

J=∂f∂T=∂(dist(Tp,line))∂TJ =\frac{{\partial f}}{{\partial T}} = \frac{{\partial (dist(Tp,line))}}{{\partial T}}J=∂T∂f=∂T∂(dist(Tp,line))

由于我们只关心三个分量，且每一帧包含多个点线配对，可以计算许多个残差，则雅可比矩阵写开以后是这个类型: J=J=J=
fff对这三个位姿量的求导，使用链式法则来计算，首先将残差对使用这个位姿量转换过去的点p′=(x,y,z)p'=(x,y,z)p′=(x,y,z)进行求导（将本帧点转换至上一帧坐标系中的点），然后再使用被转换过去的点p′=(x,y,z)p'=(x,y,z)p′=(x,y,z)对位姿量求导。也就是：

∂fk∂tx=∂fk∂x∂x∂tx+∂fk∂y∂y∂tx+∂fk∂z∂z∂tx\frac{{\partial {f_k}}}{{\partial {t_x}}} = \frac{{\partial {f_k}}}{{\partial x}}\frac{{\partial x}}{{\partial {t_x}}} + \frac{{\partial {f_k}}}{{\partial y}}\frac{{\partial y}}{{\partial {t_x}}} + \frac{{\partial {f_k}}}{{\partial z}}\frac{{\partial z}}{{\partial {t_x}}}∂tx∂fk=∂x∂fk∂tx∂x+∂y∂fk∂tx∂y+∂z∂fk∂tx∂z

每一项链式法则的求导分为两部分，在程序中分别在findCorrespondingCornerFeatures函数和calculateTransformationCorner函数中完成。之后我们再细说，在这里先推导∂fk∂x\frac{{\partial {f_k}}}{{\partial x}}∂x∂fk和∂x∂tx\frac{{\partial x}}{{\partial {t_x}}}∂tx∂x的具体形式，首先推导前半部分∂fk∂x\frac{{\partial {f_k}}}{{\partial x}}∂x∂fk，根据公式（这里的公式复制自知乎某回答），点线距离为：

即

其中，

令分母为：

接下来求∂fk∂x\frac{{\partial {f_k}}}{{\partial x}}∂x∂fk，也就是这里的∂dε∂x\frac{{\partial {d_\varepsilon }}}{{\partial x}}∂x∂dε。xxx即为公式里面的x0x_0x0。

∂dε∂x0=12(m112+m222+m332)(2m11∂m11∂x0+2m22∂m22∂x0)l12=m11∂m11∂x0+m22∂m22∂x0l12m112+m222+m332\frac{{\partial {d_\varepsilon }}}{{\partial {x_0}}}{\rm{ = }}\frac{{\frac{1}{2}(\sqrt {m_{11}^2 + m_{22}^2 + m_{33}^2} )(2{m_{11}}\frac{{\partial {m_{11}}}}{{\partial {x_0}}} + 2{m_{22}}\frac{{\partial {m_{22}}}}{{\partial {x_0}}})}}{{{l_{12}}}}{\rm{ = }}\frac{{{m_{11}}\frac{{\partial {m_{11}}}}{{\partial {x_0}}} + {m_{22}}\frac{{\partial {m_{22}}}}{{\partial {x_0}}}}}{{{l_{12}}\sqrt {m_{11}^2 + m_{22}^2 + m_{33}^2} }}∂x0∂dε=l1221(m112+m222+m332)(2m11∂x0∂m11+2m22∂x0∂m22)=l12m112+m222+m332m11∂x0∂m11+m22∂x0∂m22

对另两个位姿分量的就省略了，是同理的，这一块对应源码中的：

                tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];float x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = tripod1.x;float y1 = tripod1.y;float z1 = tripod1.z;float x2 = tripod2.x;float y2 = tripod2.y;float z2 = tripod2.z;float m11 = ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));float m22 = ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));float m33 = ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));float a012 = sqrt(m11 * m11  + m22 * m22 + m33 * m33);float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));float la =  ((y1 - y2)*m11 + (z1 - z2)*m22) / a012 / l12;float lb = -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;float lc = -((x1 - x2)*m22 + (y1 - y2)*m33) / a012 / l12;float ld2 = a012 / l12;

接下来求链式法则的后面那部分，也就是∂x∂tx\frac{{\partial x}}{{\partial {t_x}}}∂tx∂x。xxx指代的是经过变换矩阵变换，将本帧点转换至上一帧坐标系中的点的坐标。因此，首先需要明确旋转变换关系。LEGO-LOAM(LOAM)中的帧间坐标变换关系如下：

其中，p′=(x,y,z)p'=(x,y,z)p′=(x,y,z)表示被转换到上一帧坐标系中的点，p=(xc,yc,zc)p=(x_c,y_c,z_c)p=(xc,yc,zc)表示本帧这个点。RRR和tx,ty,tzt_x,t_y,t_ztx,ty,tz分别代表旋转和平移变换关系，角度为本帧坐标系转了多少欧拉角能转到上一帧坐标系，象征着所有欧拉角加符号带入公式才能获得本帧坐标系点转换到上一帧坐标系中。至于LOAM为什么这样设定不清楚（有点拧巴）。

要求∂x∂tx\frac{{\partial x}}{{\partial {t_x}}}∂tx∂x，必须要明确R是什么形式。根据wiki上给出的公式（上图右边红色框），欧拉角（-rx,-ry,-rz）转换为旋转矩阵的公式为：

这里之所以加负号，与欧拉角设定相关？

因此易求∂x∂tx\frac{{\partial x}}{{\partial {t_x}}}∂tx∂x，仅看第一行第一列乘出来的东西就行，即：

化简可以得到

2）高频率里程计位姿累积函数：integrateTransformation

    void integrateTransformation(){float rx, ry, rz, tx, ty, tz;AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) - sin(rz) * (transformCur[4] - imuShiftFromStartY);float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) + cos(rz) * (transformCur[4] - imuShiftFromStartY);float z1 = transformCur[5] - imuShiftFromStartZ;float x2 = x1;float y2 = cos(rx) * y1 - sin(rx) * z1;float z2 = sin(rx) * y1 + cos(rx) * z1;tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);transformSum[0] = rx;transformSum[1] = ry;transformSum[2] = rz;transformSum[3] = tx;transformSum[4] = ty;transformSum[5] = tz;}

（1）AccumulateRotation函数：

输入：

上一帧粗配准的激光里程计的累积位姿（全局），仅取旋转部分欧拉角：transformSum[0], transformSum[1], transformSum[2]
上一帧和本帧间的位姿变化量（增量），仅取旋转部分欧拉角： -transformCur[0], -transformCur[1], -transformCur[2]。由于transformCur处于本帧坐标系，取负号表示从上一帧变换到本帧的欧拉角变化量。
本帧粗配准的激光里程计的累积位姿，仅取旋转部分欧拉角，由以上两组量求得：rx, ry, rz

首先从位姿变换基础公式入手，根据wiki上给出的公式（上图画黄色框处），这里欧拉角（x,y,z）转换为旋转矩阵的公式为：

设上一帧全局位姿的旋转部分为R1=(x1,y1,z1)R_1=(x_1, y_1, z_1)R1=(x1,y1,z1)，本帧全局位姿的旋转部分为R2=(x2,y2,z2)R_2=(x_2, y_2, z_2)R2=(x2,y2,z2)，上一帧到本帧的旋转变化量δR=(δx,δy,δz)\delta R=(\delta x, \delta y, \delta z)δR=(δx,δy,δz)，于是根据姿态变换公式：

R2=R1⋅δRR_2=R_1·\delta RR2=R1⋅δR

根据上面欧拉角转旋转矩阵的通式，与待求欧拉角相关的R2R_2R2为：

观察到，第二行第三列直接与x2x_2x2相关，因此考虑先求x2x_2x2，即先求R2(2,3)R_2^{(2,3)}R2(2,3)，也就是 (R1R_1R1和δR\delta RδR也按照上面通式转为旋转矩阵) R1R_1R1的第二行乘以δR\delta RδR的第三列，即：

展开就能用来求−sinx2-sinx_2−sinx2，于是就有了AccumulateRotation函数内部的：

float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
ox = -asin(srx);

接下来求y2y_2y2，可以看到：

arctan(R2(1,3)/R2(2,3))=y2arctan(R_2^{(1,3)} / R_2^{(2,3)})=y_2arctan(R2(1,3)/R2(2,3))=y2，同理，按照上述几行几列的思路写出公式，可得函数内部的：

 float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));oy = atan2(srycrx / cos(ox), crycrx / cos(ox));

接下来求z2z_2z2，可以看到：
arctan(R2(2,1)/R2(2,2))=z2arctan(R_2^{(2,1)} / R_2^{(2,2)})=z_2arctan(R2(2,1)/R2(2,2))=z2，同理，按照上述几行几列的思路写出公式，可得函数内部的：

float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));

接下来观察旋转部分，这里我们忽略IMU的使用，即imuShiftFromStartZ = imuShiftFromStartY = 0 （LEGO-LOAM的IMU听说也很鸡肋）。就有如下代码：

        float x1 = cos(rz) * (transformCur[3]) - sin(rz) * (transformCur[4]);float y1 = sin(rz) * (transformCur[3]) + cos(rz) * (transformCur[4]);float z1 = transformCur[5];float x2 = x1;float y2 = cos(rx) * y1 - sin(rx) * z1;float z2 = sin(rx) * y1 + cos(rx) * z1;tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

可以这么理解，先使用rx，ry，rz相关的转换矩阵将参考系与世界坐标系同轴（平行），然后transformSum是位于世界坐标系的，因此可以直接做加法。这个原因主要是因为transformCur是存在于本帧坐标系的，推导公式也能推导出来上面式子。