LOAM误差函数的导数详细推导
前言
LOAM对于激光SLAM的发展起到了举足轻重的作用，他提出了点到线和点到面的误差函数，通过优化的方法，得到了非常不错的激光里程计效果。我猜测作者Zhang Ji很可能是从点到线和点到面的ICP算法中找到的灵感。
在LOAM的论文中以及Zhang Ji早期开源的代码中，对于代价函数求解使用的是欧拉角的方式进行求导解算的。一方面由于未采用矩阵的形式进行推导，导致整个推导过程非常复杂，另一方面在代码实现中，有大量的中间运算过程，实际上对效率也带来了一部分影响。
在后续研究中F LOAM，采用了更加优雅的方式，在SE(3)的理论基础之上推导出了更加规整的雅克比矩阵，并借用ceres进行了实现，也确实对于精度和速度有一定的提升。
LOAM这种使用欧拉角的方式进行优化的方式，一直继承了下来，在LEGO-LOAM、LIO-SAM中均能看到。
在这篇博客中，我将在SO(3)的基础之上进行雅克比矩阵的详细推导，它与F LOAM的SE(3)推导区别并不大，只是我更加喜欢使用SO(3)方式。另外在国内的网站上有较少博客去做这件事，所以我将我的一些理解，向你分享。如果有理解偏颇的地方，烦请斧正。
预备知识
本篇博客会比较偏理论一些，需要你有一些先验知识：
（1）简单的矩阵运算；
（2）向量、矩阵求导；
（3）熟悉最优化理论；
（4）了解LOAM的误差函数的意义，本篇重点是探索误差函数的求解，所以不会去介绍误差函数的由来。
1. 点到直线(Point to Edge)
1.1 点到直线误差函数
d e = ∥ ( R p s + t − p t ) × n ⃗ e ∥ 2 (1) d_e = \|(Rp_s + t -p_t) \times \vec n_e\|_2 \tag{1} de=∥(Rps+t−pt)×n e∥2(1)
1.2 误差函数对R(旋转)和t(平移)的求导结果
∂ d e ∂ R = ( n ⃗ e × ( R p s ) ∧ ) T ∗ ( R p s + t − p t ) × n ⃗ e ∥ ( R p s + t − p t ) × n ⃗ ∥ 2 ∂ d e ∂ t = ( − n ⃗ e × I 3 × 3 ) T ∗ ( R p s + t − p t ) × n ⃗ e ∥ ( R p s + t − p t ) × n ⃗ e ∥ 2 (2) \frac{\partial d_e}{\partial R} = (\vec n_e \times (Rp_s)^{\wedge})^T * \frac{(Rp_s + t -p_t) \times \vec n_e}{\|(Rp_s + t -p_t) \times \vec n\|_2} \\ \frac{\partial d_e}{\partial t} = (-\vec n_e \times I_{3\times3})^T *\frac{(Rp_s + t -p_t) \times \vec n_e}{\|(Rp_s + t -p_t) \times \vec n_e\|_2} \tag{2} ∂R∂de=(n e×(Rps)∧)T∗∥(Rps+t−pt)×n ∥2(Rps+t−pt)×n e∂t∂de=(−n e×I3×3)T∗∥(Rps+t−pt)×n e∥2(Rps+t−pt)×n e(2)
1.3 预备数学公式
在推导公式（1）之前，先解释一下用到的一些数学公式：
二范数的导数
∂ ∥ x ∥ 2 ∂ x = x ∥ x ∥ 2 (3) \frac{ \partial\|x\|_2 } {\partial x} = \frac{x}{\|x\|_2} \tag{3} ∂x∂∥x∥2=∥x∥2x(3)
标量对向量求导的链式法则
例如： z z z是关于 y y y的函数， y y y是关于 x x x的函数，它们的传递过程是： x − > y − > z x -> y->z x−>y−>z. 其中 z z z是标量， y y y是向量， x x x是向量，则求导的链式法则如下：
∂ z ∂ x = ( ∂ y ∂ x ) T ∂ z ∂ y (4) \frac{\partial z}{\partial x} = (\frac{\partial y}{\partial x})^T \frac{\partial z}{\partial y} \tag{4} ∂x∂z=(∂x∂y)T∂y∂z(4)
叉乘的交换性质
u × v = − v × u (5) u \times v = -v\times u \tag{5} u×v=−v×u(5)
旋转矩阵左扰动求导
由于旋转矩阵不满足加法性质，所以采用扰动模型进行求导，这里采用的是对其左乘一个微小转动量 δ R \delta R δR，其对应的李代数 ϕ \phi ϕ，于是得到如下结论：（由于求导推导比较简单，并且资料较多，这里只给出结论）
∂ R p ∂ ϕ = − ( R p ) ∧ (6) \frac{\partial Rp}{\partial \phi} = -(Rp)^{\wedge} \tag{6} ∂ϕ∂Rp=−(Rp)∧(6)
1.4 误差函数对R和t的求导推导
根据以上数学性质，我们开始完成loam代价函数公式的求导，从而得到雅克比矩阵：
关于R的导数
∂ d e ∂ R = ∥ ( R p s + t − p t ) × n ⃗ e ∥ 2 ∂ R (7) \frac{\partial d_e}{\partial R} = \frac{\|(Rp_s + t -p_t) \times \vec n_e\|_2}{\partial R} \tag{7} ∂R∂de=∂R∥(Rps+t−pt)×n e∥2(7)
对（7）式使用（4）式对应的链式求导法则：
∂ d e ∂ R = ∥ ( R p s + t − p t ) × n ⃗ e ∥ 2 ∂ R = ( ∂ ( ( R p s + t − p t ) × n ⃗ e ) ∂ R ) T ∗ ( R p s + t − p t ) × n ⃗ e ∥ ( R p s + t − p t ) × n ⃗ ∥ 2 (8) \frac{\partial d_e}{\partial R} = \frac{\|(Rp_s + t -p_t) \times \vec n_e\|_2}{\partial R} \\ = (\frac{\partial((Rp_s + t -p_t) \times \vec n_e)}{\partial R} )^{T} * \frac{(Rp_s + t -p_t) \times \vec n_e}{\|(Rp_s + t -p_t) \times \vec n\|_2} \tag{8} ∂R∂de=∂R∥(Rps+t−pt)×n e∥2=(∂R∂((Rps+t−pt)×n e))T∗∥(Rps+t−pt)×n ∥2(Rps+t−pt)×n e(8)
对于（8）式中第二行前半部分的偏导数计算：
∂ ( ( R p s + t − p t ) × n ⃗ e ) ∂ R = ∂ ( − n ⃗ e × ( R p s + t − p t ) ) ∂ R = ∂ ( − n ⃗ e × ( R p s ) ) ∂ R = − n ⃗ e × ( − ( R p s ) ∧ ) (9) \frac{\partial((Rp_s + t -p_t) \times \vec n_e)}{\partial R} \\ = \frac{\partial( -\vec n_e \times(Rp_s + t -p_t))}{\partial R} \\ = \frac{\partial( -\vec n_e \times(Rp_s))}{\partial R} \\ = -\vec n_e \times(-(Rp_s)^{\wedge}) \tag{9} ∂R∂((Rps+t−pt)×n e)=∂R∂(−n e×(Rps+t−pt))=∂R∂(−n e×(Rps))=−n e×(−(Rps)∧)(9)
整理之后,得到关于点到线的误差函数关于R的导数：
∂ d e ∂ R = ( n ⃗ e × ( R p s ) ∧ ) T ∗ ( R p s + t − p t ) × n ⃗ e ∥ ( R p s + t − p t ) × n ⃗ ∥ 2 (10) \frac{\partial d_e}{\partial R} = (\vec n_e \times (Rp_s)^{\wedge})^T * \frac{(Rp_s + t -p_t) \times \vec n_e}{\|(Rp_s + t -p_t) \times \vec n\|_2} \tag{10} ∂R∂de=(n e×(Rps)∧)T∗∥(Rps+t−pt)×n ∥2(Rps+t−pt)×n e(10)
关于t的导数
∂ d e ∂ t = ∥ ( R p s + t − p t ) × n ⃗ e ∥ 2 ∂ t = ( ∂ ( ( R p s + t − p t ) × n ⃗ e ) ∂ t ) T ∗ ( R p s + t − p t ) × n ⃗ e ∥ ( R p s + t − p t ) × n ⃗ ∥ 2 (11) \frac{\partial d_e}{\partial t} = \frac{\|(Rp_s + t -p_t) \times \vec n_e\|_2}{\partial t} \\ = (\frac{\partial((Rp_s + t -p_t) \times \vec n_e)}{\partial t} )^{T} * \frac{(Rp_s + t -p_t) \times \vec n_e}{\|(Rp_s + t -p_t) \times \vec n\|_2} \tag{11} ∂t∂de=∂t∥(Rps+t−pt)×n e∥2=(∂t∂((Rps+t−pt)×n e))T∗∥(Rps+t−pt)×n ∥2(Rps+t−pt)×n e(11)
对于（11）式第二行左半部分的偏导数计算：
∂ ( ( R p s + t − p t ) × n ⃗ e ) ∂ t = − n ⃗ e × ∂ ( t ) ∂ t = − n ⃗ e × I 3 × 2 (12) \frac{\partial((Rp_s + t -p_t) \times \vec n_e)}{\partial t} \\ = \frac{ -\vec n_e \times \partial(t)}{\partial t} \\ = -\vec n_e \times I_{3\times 2} \tag{12} ∂t∂((Rps+t−pt)×n e)=∂t−n e×∂(t)=−n e×I3×2(12)
整理之后，得到关于点到线的误差函数关于t的导数：
∂ d e ∂ t = ( − n ⃗ e × I 3 × 3 ) T ∗ ( R p s + t − p t ) × n ⃗ e ∥ ( R p s + t − p t ) × n ⃗ e ∥ 2 (13) \frac{\partial d_e}{\partial t} = (-\vec n_e \times I_{3\times3})^T *\frac{(Rp_s + t -p_t) \times \vec n_e}{\|(Rp_s + t -p_t) \times \vec n_e\|_2} \tag{13} ∂t∂de=(−n e×I3×3)T∗∥(Rps+t−pt)×n e∥2(Rps+t−pt)×n e(13)
2. 点到平面(Point to Surface)
2.1 点到平面误差函数
d p = ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 (14) d_p = \| (Rp_s + t - p_t)^T * \vec n \|_2 \tag{14} dp=∥(Rps+t−pt)T∗n ∥2(14)
2.2 误差函数对R(旋转)和t(平移)的求导结果
∂ d p ∂ R = ( − ( R p s ) ∧ ) T ∗ n ⃗ ∗ ( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 ∂ d p ∂ t = I 3 × 3 ∗ n ⃗ ∗ ( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 (15) \frac{\partial d_p}{\partial R} = (-(Rp_s)^{\wedge})^T*\vec{n}* \frac{ (Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2}\\ \frac{\partial d_p}{\partial t} = I_{3\times3} *\vec{n} * \frac{(Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2} \tag{15} ∂R∂dp=(−(Rps)∧)T∗n ∗∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n ∂t∂dp=I3×3∗n ∗∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n (15)
2.3 误差函数对R和t的求导推导
根据第1节的推导过程，对于点到平面误差函数的推导就非常容易了！
关于R的导数
∂ d p ∂ R = ∂ ( ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 ) ∂ R = ( ∂ ( ( R p s + t − p t ) T ∗ n ⃗ ) ∂ R ) T ∗ ( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 = ( ∂ ( ( R p s ) T ∗ n ⃗ ) ∂ R ) T ∗ ( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 = ( − ( R p s ) ∧ ) T ∗ n ⃗ ∗ ( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 (16) \frac{\partial{d_p}}{\partial R} = \frac{\partial(\|(Rp_s + t - p_t)^T * \vec n \|_2)}{\partial R} \\ = ( \frac{\partial((Rp_s + t - p_t)^T * \vec n)}{\partial R})^T * \frac{(Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2} \\ = ( \frac{\partial((Rp_s)^T * \vec n)}{\partial R})^T * \frac{(Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2} \\ =(-(Rp_s)^{\wedge})^T*\vec{n}* \frac{ (Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2} \tag{16} ∂R∂dp=∂R∂(∥(Rps+t−pt)T∗n ∥2)=(∂R∂((Rps+t−pt)T∗n ))T∗∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n =(∂R∂((Rps)T∗n ))T∗∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n =(−(Rps)∧)T∗n ∗∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n (16)
关于t的导数
∂ d p ∂ t = ∂ ( ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 ) ∂ t = ( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 ∗ ∂ ( ( R p s + t − p t ) T ∗ n ⃗ ) ∂ t = ( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 ∗ ∂ ( t ) T ∗ n ⃗ ∂ t = ( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 ∗ I 3 × 3 ∗ n ⃗ (17) \frac{\partial{d_p}}{\partial t} = \frac{\partial(\|(Rp_s + t - p_t)^T * \vec n \|_2)}{\partial t} \\ = \frac{(Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2}* \frac{\partial((Rp_s + t - p_t)^T * \vec n)}{\partial t} \\ = \frac{(Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2} * \frac{\partial(t)^T * \vec n}{\partial t} \\ = \frac{(Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2} * I_{3\times3} *\vec{n} \tag{17} ∂t∂dp=∂t∂(∥(Rps+t−pt)T∗n ∥2)=∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n ∗∂t∂((Rps+t−pt)T∗n )=∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n ∗∂t∂(t)T∗n =∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n ∗I3×3∗n (17)
3. 对于loam代价函数的总结
loam的总体代价函数
如下：
d = ∑ N d e + ∑ M d s = ∑ N ∥ ( R p s + t − p t ) × n ⃗ e ∥ 2 + ∑ M ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 (18) d = \sum_N d_e + \sum_M d_s \\ = \sum_N \|(Rp_s + t -p_t) \times \vec n_e\|_2 + \sum_M \| (Rp_s + t - p_t)^T * \vec n \|_2 \tag{18} d=N∑de+M∑ds=N∑∥(Rps+t−pt)×n e∥2+M∑∥(Rps+t−pt)T∗n ∥2(18)
根据1和2小节的求导，可以得到每一个误差的导数，组成一个大的雅克比矩阵(也有可能是向量)，有了雅克比矩阵之后带入高斯牛顿或者LM算法即可以求解最优的R和t。该过程是属于最优化理论相关的内容，是比较成熟的理论，不是本篇博客要探索的，不在此做细致介绍。
是否有更好的代价函数形式？
我在复现loam的过程中发现，其中点到线的误差项，也就是 ∑ d e \sum d_e ∑de，它实际优化过程中对于R和t求解的贡献比较小，其主要原因是点到面的误差项 ∑ d s \sum d_s ∑ds中M的数量比较庞大，在16线激光雷达中，经过我的验证M几乎是N的20倍以上，所以实际过程中，如果我们省略掉点到线的误差项，对于最终的精度并未产生明显影响。也许在插满了细长柱子的环境中点到线的误差项才会有明显作用。否则我认为多数真实环境下，点到面的误差项实际上已经涵盖住了点到线的误差。所以在后来一些开源项目中，例如r3live、fast-lio都只计算点到面的误差。
另外，我也尝试对loam的代价函数做了一些改变，如下式，构建成两个最小二乘项的求和：
d = ∑ N ( d e ) 2 + ∑ M ( d s ) 2 = ∑ N ∥ ( R p s + t − p t ) × n ⃗ e ∥ 2 2 + ∑ M ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 2 (19) d = \sum_N (d_e)^2 + \sum_M (d_s)^2 \\ = \sum_N \|(Rp_s + t -p_t) \times \vec n_e\|_2^2 + \sum_M \| (Rp_s + t - p_t)^T * \vec n \|_2^2 \tag{19} d=N∑(de)2+M∑(ds)2=N∑∥(Rps+t−pt)×n e∥22+M∑∥(Rps+t−pt)T∗n ∥22(19)
也就是说，对代价函数计算平方二范数误差。在我的意识中，平方二范数会有更好的收敛性，上式应该会比loam的二范数收敛的更好。但是后来的实验中证明我的想法是错误的，上式的求解得到的R和t与真值差距较大。大家可以去思考一下是什么原因导致的？如果有想法的话，可以在评论区留下你的看法。
算法实现过程中的一些小细节
对于式（16）和（17），它们均包含下式：
( R p s + t − p t ) T ∗ n ⃗ ∥ ( R p s + t − p t ) T ∗ n ⃗ ∥ 2 (20) \frac{(Rp_s + t - p_t)^T * \vec n}{\| (Rp_s + t - p_t)^T * \vec n \|_2} \tag{20} ∥(Rps+t−pt)T∗n ∥2(Rps+t−pt)T∗n (20)
观察可以发现，它的值是-1或者1，如果你没看出来，可以花些时间想一想，并不难！