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)

d e d_e de​：点到直线的距离（标量）；
p t p_t pt​：目标（地图）点云中的角点；
p s p_s ps​：源（当前帧）点云中的角点；
n ⃗ e \vec n_e n e​：近邻角点组成的直线对应的单位向量。

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)

I 3 × 3 I_{3\times 3} I3×3​：单位矩阵；
× \times ×：该运算符表示向量叉乘；
∗ * ∗：该运算符表示一般的向量、矩阵之间的乘法。

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∥2​x​(3)

其中： x = [ x 0 , x 1 , x 2 , ⋅ ] T x = [x_0,x_1,x_2,\cdot]^{T} x=[x0​,x1​,x2​,⋅]T， ∥ x ∥ 2 = x 0 2 + x 1 2 + ⋯ \|x\|_2 = \sqrt{x_0^2+x_1^2+\cdots} ∥x∥2​=x02​+x12​+⋯ ​

标量对向量求导的链式法则

例如： 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)

∧ \wedge ∧：该符号表示向量的反对称矩阵

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)

p t p_t pt​：目标（地图）点云中的平面点；
n ⃗ s \vec n_s n s​：近邻平面点组成的平面对应的法向量。

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，如果你没看出来，可以花些时间想一想，并不难！