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)×ne∥2(1)
d
e
d_e
de:点到直线的距离(标量);
p
t
p_t
pt:目标(地图)点云中的角点;
p
s
p_s
ps:源(当前帧)点云中的角点;
n
⃗
e
\vec n_e
ne:近邻角点组成的直线对应的单位向量。
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=(ne×(Rps)∧)T∗∥(Rps+t−pt)×n∥2(Rps+t−pt)×ne∂t∂de=(−ne×I3×3)T∗∥(Rps+t−pt)×ne∥2(Rps+t−pt)×ne(2)
二范数的导数
∂
∥
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)
其中:
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)×ne∥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)×ne∥2=(∂R∂((Rps+t−pt)×ne))T∗∥(Rps+t−pt)×n∥2(Rps+t−pt)×ne(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)×ne)=∂R∂(−ne×(Rps+t−pt))=∂R∂(−ne×(Rps))=−ne×(−(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=(ne×(Rps)∧)T∗∥(Rps+t−pt)×n∥2(Rps+t−pt)×ne(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)×ne∥2=(∂t∂((Rps+t−pt)×ne))T∗∥(Rps+t−pt)×n∥2(Rps+t−pt)×ne(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)×ne)=∂t−ne×∂(t)=−ne×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=(−ne×I3×3)T∗∥(Rps+t−pt)×ne∥2(Rps+t−pt)×ne(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
ns:近邻平面点组成的平面对应的法向量。
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)×ne∥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)×ne∥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,如果你没看出来,可以花些时间想一想,并不难!