LIO_SAM LMOptimization公式推导

点到直线的距离理解

如果点在法线一侧，则点到直线的距离可表示为：
$$\begin{gathered} n^T\times x+D=0 \\ d=n^T\times (x_i-x_0)=n^T\times x_i+D\ge 0 \end{gathered}$$
这样的话就可以直接求导，不需要考虑绝对值的问题。所以在原代码中应该保证距离为正值，cornerOptimization()中使用向量叉乘保证了点在平面法线一侧，但是surfOptimization()却没有，可能是作者的疏忽，pd2为负值用高斯牛顿法迭代是有问题的！

float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
// if(pd2<0)  //* 这里应该加一个改变pd2值和法线方向的代码！！
//     ROS_ERROR("Distance less than 0!!");

cornerOptimization()中求解法线是为了将点到线的距离变为点到面的距离，这样combineOptimizationCoeffs()将所有系数放到一起才有意义，且符合后面的高斯牛顿法求解。

$error=coef{f}^T\times (R\times point+T)+D$

$error$就是点到面的距离，$D$为平面方程中的常数，不影响$Jacobi$矩阵
$$\begin{array}{rl}R& =R_{yaw}\times R_{pitch}\times R_{roll}\\ & =R_{ry}\times R_{rx}\times R_{rz}\\ & =\left[\begin{array}{ccc}cry& 0& sry\\ 0& 1& 0\\ -sry& 0& cry\end{array}\right]\times \left[\begin{array}{ccc}1& 0& 0\\ 0& crx& -srx\\ 0& srx& crx\end{array}\right]\times \left[\begin{array}{ccc}crz& -srz& 0\\ srz& crz& 0\\ 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{ccc}crycrz+srxsrysrz& srxsrycrz-crysrz& crxsry\\ crxsrz& crxcrz& -srx\\ srxcrysrz-srycrz& srysrz+srxcrycrz& crxcry\end{array}\right]\end{array}$$
其中：$crx=cos(rx),srx=sin(rx)$
$T=[tx,ty,tz{]}^T,coeff=[coef{f}_x,coef{f}_y,coef{f}_z{]}^T,point=[poin{t}_x,poin{t}_y,poin{t}_z{]}^T$

$$arx=\frac{\partial error}{\partial rx}=\left[\begin{array}{ccc}coef{f}_x& coef{f}_y& coef{f}_z\end{array}\right]\times \left[\begin{array}{ccc}crxsrysrz& crxsrycrz& -srxsry\\ -srxsrz& -srxcrz& -crx\\ crxcrysrz& crxcrycrz& -srxcry\end{array}\right]\times \left[\begin{array}{c}poin{t}_x\\ poin{t}_y\\ poin{t}_z\end{array}\right]$$
$$ary=\frac{\partial error}{\partial ry}=\left[\begin{array}{ccc}coef{f}_x& coef{f}_y& coef{f}_z\end{array}\right]\times \left[\begin{array}{ccc}srxcrysrz-srycrz& srxcrycrz+srysrz& crxcry\\ 0& 0& 0\\ -srxsrysrz-crycrz& crysrz-srxsrycrz& -crxsry\end{array}\right]\times \left[\begin{array}{c}poin{t}_x\\ poin{t}_y\\ poin{t}_z\end{array}\right]$$
$$arz=\frac{\partial error}{\partial rz}=\left[\begin{array}{ccc}coef{f}_x& coef{f}_y& coef{f}_z\end{array}\right]\times \left[\begin{array}{ccc}srxsrycrz-crysrz& -srxsrysrz-crycrz& 0\\ crxcrz& -crxsrz& 0\\ srxcrycrz+srysrz& srycrz-srxcrysrz& 0\end{array}\right]\times \left[\begin{array}{c}poin{t}_x\\ poin{t}_y\\ poin{t}_z\end{array}\right]$$
$$\begin{gathered} \frac{\partial error}{\partial tx}=coef{f}_x \\ \frac{\partial error}{\partial ty}=coef{f}_y \\ \frac{\partial error}{\partial tz}=coef{f}_z \end{gathered}$$
$$matA=\left[\begin{array}{cccccc}\frac{\partial error}{\partial r{x}_0}& \frac{\partial error}{\partial r{y}_0}& \frac{\partial error}{\partial r{z}_0}& \frac{\partial error}{\partial t{x}_0}& \frac{\partial error}{\partial t{y}_0}& \frac{\partial error}{\partial t{z}_0}\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ \frac{\partial error}{\partial r{x}_n}& \frac{\partial error}{\partial r{y}_n}& \frac{\partial error}{\partial r{z}_n}& \frac{\partial error}{\partial t{x}_n}& \frac{\partial error}{\partial t{y}_n}& \frac{\partial error}{\partial t{z}_n}\end{array}\right]$$
$$matB=\left[\begin{array}{c}-d_0\\ ⋮\\ -d_n\end{array}\right]$$
其中$d_i$为角点到主特征向量所在直线的距离或者面点到平面的距离。
$$\begin{gathered} matAtA=mat{A}^T\times matA \\ matAtB=mat{A}^T\times matB \end{gathered}$$
高斯牛顿法的原型是$J^T\times J\times {\Delta }_x=-J^T\times f(x)$
$J=matA,f(x)=-matB,{\Delta }_x=matX$
通过QR分解的方式，求解$mat{A}^T\times matA\times matX=mat{A}^T\times matB$，得到解${\Delta }_x=matX$