参考一点儿也不萌的萌萌的文章

代价函数使用的是欧拉角的方式进行求导解算的。一方面由于未采用矩阵的形式进行推导，导致整个推导过程非常复杂. 在LEGO-LOAM、LIO-SAM中还能看到欧拉角，另外还有旋转矩阵等表示方法。

FLOAM使用SE(3)推导雅格比。

点到直线的误差函数 $d_e =||(Rp_s+t-p_t) \times \overrightarrow {n_e}\ ||_2$

$d_e$ 点到直线的距离

$p_t$ 目标（地图）点云中的角点

$p_s$ 源（当前帧）点云中的角点

$\overrightarrow {n_e}$ 近邻角点组成的直线对应的单位向量

这里我没太明白，LOAM中应该是两个目标点云中的角点，也就是 $\overrightarrow {PA} \times \overrightarrow {PB}$， 不明白为什么叉乘两个角点对应的单位向量。

作者讲的计算有点绕远，根据叉乘的求导公式，范数部分的求导如下

完整2范数的对R求导，直接套用范数求雅格比的公式

这样直接把上面的计算结果带入 $J_f(X)$，就得到最终结果。 关于t的导数同样计算。

点到平面误差函数就是 $d_p =||(Rp_s+t-p_t) * \overrightarrow {n_e}\ ||_2$ 没有疑问

$p_t$ 目标（地图）点云中的平面点；

$\overrightarrow {n_e}$ 近邻平面点组成的平面对应的法向量。

推导过程还是按我上面讲的更容易理解。

$\frac{(Rp_s+t-p_t) * \overrightarrow {n_e}}{||(Rp_s+t-p_t) * \overrightarrow {n_e}\ ||_2 }$ 的值是-1或者1

得到每一个误差的导数，组成一个大的雅克比矩阵(也有可能是向量)，有了雅克比矩阵之后带入高斯牛顿或者LM算法即可以求解最优的R和t。
点到线的误差项在优化过程中的贡献比较小，在r3live、fast-lio都只计算点到面的误差。