LeGO-LOAM是一种在LOAM之上进行改进的激光雷达建图方法，建图效果比LOAM要好，但是建图较为稀疏，计算量也更小了。
本文原地址：wykxwyc的博客
github注释后LeGO-LOAM源码：LeGO-LOAM_NOTED
关于代码的详细理解，建议阅读：1.地图优化代码理解
3.特征关联代码理解
以上博客会随时更新，如果对你有帮助，请点击注释代码的github仓库右上角star按钮，你的鼓励将给我更多动力。
本文 记录重新读LeGO-LOAM的代码时看到的数学运算，记录这些数学运算背后的原理，会随时更新。
To be continued.
随机变量的协方差是什么?
1.在概率论和统计中，协方差是对两个随机变量联合分布线性相关程度的一种度量。两个随机变量越线性相关，协方差越大，完全线性无关，协方差为零。（线性无关并不代表完全无关，更不代表相互独立。）
2.两个随机变量
X
X
X与
Y
Y
Y协方差的定义如下：
(CO-1)
cov
(
X
,
Y
)
=
E
[
(
X
−
E
[
X
]
)
(
Y
−
E
[
Y
]
)
]
\operatorname{cov}(X, Y)=\mathrm{E}[(X-\mathrm{E}[X])(Y-\mathrm{E}[Y])] \tag{CO-1}
cov(X,Y)=E[(X−E[X])(Y−E[Y])](CO-1)
如何判断两个随机变量的相关程度？
1.通过定义这两个变量之间的相关系数
η
\eta
η进行判断：
(CO-2)
η
=
cov
(
X
,
Y
)
var
(
X
)
⋅
var
(
Y
)
\eta=\frac{\operatorname{cov}(X, Y)}{\sqrt{\operatorname{var}(X) \cdot \operatorname{var}(Y)}} \tag{CO-2}
η=var(X)⋅var(Y)
cov(X,Y)(CO-2)
1表示完全线性相关，−1表示完全线性负相关，0表示线性无关。线性无关并不代表完全无关，更不代表相互独立。
样本的协方差矩阵
1.设多维随机变量
X
=
[
X
1
,
X
2
,
X
3
,
…
,
X
n
]
T
\mathbf{X}=\left[X_{1}, X_{2}, X_{3}, \dots, X_{n}\right]^{T}
X=[X1,X2,X3,…,Xn]T的协方差矩阵为
Σ
\Sigma
Σ，则协方差矩阵中的每个元素为:
(CO-3)
Σ
i
j
=
cov
(
X
i
,
X
j
)
=
E
[
(
X
i
−
E
[
X
i
]
)
(
X
j
−
E
[
X
j
]
)
]
\Sigma_{i j}=\operatorname{cov}\left(X_{i}, X_{j}\right)=\mathrm{E}\left[\left(X_{i}-\mathrm{E}\left[X_{i}\right]\right)\left(X_{j}-\mathrm{E}\left[X_{j}\right]\right)\right] \tag{CO-3}
Σij=cov(Xi,Xj)=E[(Xi−E[Xi])(Xj−E[Xj])](CO-3)
上式表示的是
X
i
X_{i}
Xi和
X
j
X_{j}
Xj之间的协方差。
公式CO-3也揭示了协方差矩阵中每个元素的计算过程，整个矩阵为：
Σ
=
E
[
(
X
−
E
[
X
]
)
(
X
−
E
[
X
]
)
T
]
=
[
cov
(
X
1
,
X
1
)
cov
(
X
1
,
X
2
)
…
cov
(
X
1
,
X
n
)
cov
(
X
2
,
X
1
)
cov
(
X
2
,
X
2
)
…
cov
(
X
2
,
X
n
)
⋮
⋮
⋱
⋮
cov
(
X
n
,
X
1
)
cov
(
X
n
,
X
2
)
⋯
cov
(
X
n
,
X
n
)
]
=
[
E
[
(
X
1
−
E
[
X
1
]
)
(
X
1
−
E
[
X
1
]
)
]
E
[
(
X
1
−
E
[
X
1
]
)
(
X
2
−
E
[
X
2
]
)
]
⋯
E
[
(
X
1
−
E
[
X
1
]
)
(
X
n
−
E
[
X
n
]
)
]
E
[
(
X
2
−
E
[
X
2
]
)
(
X
1
−
E
[
X
1
]
)
]
E
[
(
X
2
−
E
[
X
2
]
)
(
X
2
−
E
[
X
2
]
)
]
⋯
E
[
(
X
2
−
E
[
X
2
]
)
(
X
n
−
E
[
X
n
]
)
]
⋮
⋮
⋱
⋮
E
[
(
X
n
−
E
[
X
n
]
)
(
X
1
−
E
[
X
1
]
)
]
E
[
(
X
n
−
E
[
X
n
]
)
(
X
2
−
E
[
X
2
]
)
]
⋯
E
[
(
X
n
−
E
[
X
n
]
)
(
X
n
−
E
[
X
n
]
)
]
]
\begin{aligned} &\Sigma =\mathrm{E}\left[(\mathbf{X}-\mathrm{E}[\mathbf{X}])(\mathbf{X}-\mathrm{E}[\mathbf{X}])^{T}\right] \\ &=\left[\begin{array}{cccc}{\operatorname{cov}\left(X_{1}, X_{1}\right)} & {\operatorname{cov}\left(X_{1}, X_{2}\right)} & {\dots} & {\operatorname{cov}\left(X_{1}, X_{n}\right)} \\ {\operatorname{cov}\left(X_{2}, X_{1}\right)} & {\operatorname{cov}\left(X_{2}, X_{2}\right)} & {\dots} & {\operatorname{cov}\left(X_{2}, X_{n}\right)} \\ {\vdots} & {\vdots} & {\ddots} & {\vdots} \\ {\operatorname{cov}\left(X_{n}, X_{1}\right)} & {\operatorname{cov}\left(X_{n}, X_{2}\right)} & {\cdots} & {\operatorname{cov}\left(X_{n}, X_{n}\right)}\end{array}\right] \\ &=\left[\begin{array}{cccc}{\mathrm{E}\left[\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\right]} & {\mathrm{E}\left[\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\right]} & {\cdots} & {\mathrm{E}\left[\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\right]} \\ {\mathrm{E}\left[\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\right]} & {\mathrm{E}\left[\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\right]} & {\cdots} & {\mathrm{E}\left[\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\right]} \\ {\vdots} & {\vdots} & {\ddots} & {\vdots} \\ {\mathrm{E}\left[\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\right]} & {\mathrm{E}\left[\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\right]} & {\cdots} & {\mathrm{E}\left[\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\right]}\end{array}\right] \end{aligned}
Σ=E[(X−E[X])(X−E[X])T]=⎣⎢⎢⎢⎡cov(X1,X1)cov(X2,X1)⋮cov(Xn,X1)cov(X1,X2)cov(X2,X2)⋮cov(Xn,X2)……⋱⋯cov(X1,Xn)cov(X2,Xn)⋮cov(Xn,Xn)⎦⎥⎥⎥⎤=⎣⎢⎢⎢⎡E[(X1−E[X1])(X1−E[X1])]E[(X2−E[X2])(X1−E[X1])]⋮E[(Xn−E[Xn])(X1−E[X1])]E[(X1−E[X1])(X2−E[X2])]E[(X2−E[X2])(X2−E[X2])]⋮E[(Xn−E[Xn])(X2−E[X2])]⋯⋯⋱⋯E[(X1−E[X1])(Xn−E[Xn])]E[(X2−E[X2])(Xn−E[Xn])]⋮E[(Xn−E[Xn])(Xn−E[Xn])]⎦⎥⎥⎥⎤
记上面公式为(CO-4)。
2.样本的协方差矩阵的计算
样本集合为
{
x
⋅
j
=
[
x
1
j
,
x
2
j
,
…
,
x
n
j
]
T
∣
1
⩽
j
⩽
m
}
\left\{\mathbf{x}_{\cdot j}=\left[x_{1 j}, x_{2 j}, \ldots, x_{n j}\right]^{T} | 1 \leqslant j \leqslant m\right\}
{x⋅j=[x1j,x2j,…,xnj]T∣1⩽j⩽m}，m表示样本数量。
整个样本的协方差矩阵：
Σ
^
=
[
q
11
q
12
⋯
q
1
n
q
21
q
21
⋯
q
2
n
⋮
⋮
⋱
⋮
q
n
1
q
n
2
⋯
q
n
n
]
=
1
m
−
1
[
∑
j
=
1
m
(
x
1
j
−
x
‾
1
)
(
x
1
j
−
x
‾
1
)
∑
j
=
1
m
(
x
1
j
−
x
‾
1
)
(
x
2
j
−
x
‾
2
)
⋯
∑
j
=
1
m
(
x
1
j
−
x
‾
1
)
(
x
n
j
−
x
‾
n
)
∑
j
=
1
m
(
x
2
j
−
x
‾
2
)
(
x
1
j
−
x
‾
1
)
∑
j
=
1
m
(
x
2
j
−
x
‾
2
)
(
x
2
j
−
x
‾
2
)
⋯
∑
j
=
1
m
(
x
2
j
−
x
‾
2
)
(
x
n
j
−
x
‾
n
)
⋮
⋮
⋱
⋮
∑
j
=
1
m
(
x
n
j
−
x
‾
n
)
(
x
1
j
−
x
‾
1
)
∑
j
=
1
m
(
x
n
j
−
x
‾
n
)
(
x
2
j
−
x
‾
2
)
⋯
∑
j
=
1
m
(
x
n
j
−
x
‾
n
)
(
x
n
j
−
x
‾
n
)
]
=
1
m
−
1
∑
j
=
1
m
(
x
.
j
−
x
‾
)
(
x
:
j
−
x
‾
)
T
\begin{aligned} &\hat{\Sigma} =\left[\begin{array}{cccc}{q_{11}} & {q_{12}} & {\cdots} & {q_{1 n}} \\ {q_{21}} & {q_{21}} & {\cdots} & {q_{2 n}} \\ {\vdots} & {\vdots} & {\ddots} & {\vdots} \\ {q_{n 1}} & {q_{n 2}} & {\cdots} & {q_{n n}}\end{array}\right] \\ &=\frac{1}{m-1}\left[\begin{array}{cccc}{\sum_{j=1}^{m}\left(x_{1 j}-\overline{x}_{1}\right)\left(x_{1 j}-\overline{x}_{1}\right)} & {\sum_{j=1}^{m}\left(x_{1 j}-\overline{x}_{1}\right)\left(x_{2 j}-\overline{x}_{2}\right)} & {\cdots} & {\sum_{j=1}^{m}\left(x_{1 j}-\overline{x}_{1}\right)\left(x_{n j}-\overline{x}_{n}\right)} \\ {\sum_{j=1}^{m}\left(x_{2 j}-\overline{x}_{2}\right)\left(x_{1 j}-\overline{x}_{1}\right)} & {\sum_{j=1}^{m}\left(x_{2 j}-\overline{x}_{2}\right)\left(x_{2 j}-\overline{x}_{2}\right)} & {\cdots} & {\sum_{j=1}^{m}\left(x_{2 j}-\overline{x}_{2}\right)\left(x_{n j}-\overline{x}_{n}\right)} \\ {\vdots} & {\vdots} & {\ddots} & {\vdots} \\ {\sum_{j=1}^{m}\left(x_{n j}-\overline{x}_{n}\right)\left(x_{1 j}-\overline{x}_{1}\right)} & {\sum_{j=1}^{m}\left(x_{n j}-\overline{x}_{n}\right)\left(x_{2 j}-\overline{x}_{2}\right)} & {\cdots} & {\sum_{j=1}^{m}\left(x_{n j}-\overline{x}_{n}\right)\left(x_{n j}-\overline{x}_{n}\right)}\end{array}\right] \\ &=\frac{1}{m-1} \sum_{j=1}^{m}\left(\mathbf{x} ._{j}-\overline{\mathbf{x}}\right)\left(\mathbf{x}_{ : j}-\overline{\mathbf{x}}\right)^{T} \end{aligned}
Σ^=⎣⎢⎢⎢⎡q11q21⋮qn1q12q21⋮qn2⋯⋯⋱⋯q1nq2n⋮qnn⎦⎥⎥⎥⎤=m−11⎣⎢⎢⎢⎡∑j=1m(x1j−x1)(x1j−x1)∑j=1m(x2j−x2)(x1j−x1)⋮∑j=1m(xnj−xn)(x1j−x1)∑j=1m(x1j−x1)(x2j−x2)∑j=1m(x2j−x2)(x2j−x2)⋮∑j=1m(xnj−xn)(x2j−x2)⋯⋯⋱⋯∑j=1m(x1j−x1)(xnj−xn)∑j=1m(x2j−x2)(xnj−xn)⋮∑j=1m(xnj−xn)(xnj−xn)⎦⎥⎥⎥⎤=m−11j=1∑m(x.j−x)(x:j−x)T
记上面公式为CO-5。
LeGO-LOAM的代码中的协方差计算过程
1.LeGO-LOAM中的协方差采用公式OC-5中的计算方法,每个点云的维度为n=3,XYZ三个距离,一共有m=5个样本;
2.有一个区别就是代码中最后直接除以m=5，而公式OC-5中是除以m-1=4。
求方向向量的过程？
1.对识别为特征边缘的点计算它和它周围点的协方差矩阵，
2.然后用opencv库函数求解特征值和特征向量
3.会有一个大的特征值和两个小的特征值，大的特征值对应于方向向量。
为什么这个协方差矩阵的较大特征值对应的就是边缘的方向？
To be continued.
点指的是被认为是特征边缘上的点，直线就是特征边缘所在的这条直线。
简单来看就是计算点到直线的距离，用面积法进行计算。
在上图中，就是求点 X 0 X_{0} X0到紫色直线 0.2 V 0 0.2V_{0} 0.2V0的距离。
点 X 0 X_{0} X0对应于点pointSel，点 C C C对应于点 [ c x , c y , c z ] [c_{x},c_{y},c_{z}] [cx,cy,cz]，先求绿色，蓝色线段构成的平行四边形面积，然后除以紫色的长度，得到距离值。
To be continued
这个函数中通过乘以旋转矩阵的方式，对坐标进行了变换，由局部坐标系变换到地图的全局坐标系。
变换是先进行yaw角的变换，然后是roll角的变换，最后是pitch的变换(即z->x->y),按照坐标变换左乘矩阵的规则，有：
(PA-1)
p
⃗
o
=
R
y
R
x
R
z
p
⃗
i
+
T
\vec{p}_{o}=\mathbf{R}_{y}\mathbf{R}_{x}\mathbf{R}_{z} \vec{p}_{i}+\mathbf{T} \tag{PA-1}
p
o=RyRxRzp
i+T(PA-1)
所以整体的旋转变换可以写成一个矩阵形式：
(PA-2)
R
t
o
t
a
l
=
R
y
R
x
R
z
=
Y
1
X
2
Z
3
=
[
c
1
c
3
+
s
1
s
2
s
3
c
3
s
1
s
2
−
c
1
s
3
c
2
s
1
c
2
s
3
c
2
c
3
−
s
2
c
1
s
2
s
3
−
c
3
s
1
c
1
c
3
s
2
+
s
1
s
3
c
1
c
2
]
\mathbf{R}_{total}=\mathbf{R}_{y}\mathbf{R}_{x}\mathbf{R}_{z} \\ =Y_{1} X_{2} Z_{3}=\left[\begin{array}{ccc}{c_{1} c_{3}+s_{1} s_{2} s_{3}} & {c_{3} s_{1} s_{2}-c_{1} s_{3}} & {c_{2} s_{1}} \\ {c_{2} s_{3}} & {c_{2} c_{3}} & {-s_{2}} \\ {c_{1} s_{2} s_{3}-c_{3} s_{1}} & {c_{1} c_{3} s_{2}+s_{1} s_{3}} & {c_{1} c_{2}}\end{array}\right] \tag{PA-2}
Rtotal=RyRxRz=Y1X2Z3=⎣⎡c1c3+s1s2s3c2s3c1s2s3−c3s1c3s1s2−c1s3c2c3c1c3s2+s1s3c2s1−s2c1c2⎦⎤(PA-2)
注意：上面公式简写了 s i n , c o s sin,cos sin,cos函数的符号。
旋转有两种，一种是向量旋转，另一张是坐标系旋转。如下图所示：
向量旋转
图R-1是向量旋转，通过平面几何公式，很容易就能得到旋转后的向量坐标是:
(RO-1)
v
′
=
R
θ
v
0
=
[
cos
θ
−
sin
θ
sin
θ
cos
θ
]
×
v
0
\mathbf{v}^{\prime}=\mathbf{R}_{\theta} \mathbf{v}_{0}= \left[\begin{array}{cc}{\cos \theta} & {-\sin \theta} \\ {\sin \theta} & {\cos \theta}\end{array}\right]\times \mathbf{v}_{0} \tag{RO-1}
v′=Rθv0=[cosθsinθ−sinθcosθ]×v0(RO-1)
其中有：
(RO-2)
R
θ
=
[
cos
θ
−
sin
θ
sin
θ
cos
θ
]
\mathrm{R}_{\theta}=\left[\begin{array}{cc}{\cos \theta} & {-\sin \theta} \\ {\sin \theta} & {\cos \theta}\end{array}\right] \tag{RO-2}
Rθ=[cosθsinθ−sinθcosθ](RO-2)
坐标系旋转
坐标系旋转就是相当于向量的逆旋转，所以有：
(RO-3)
R
θ
′
=
[
cos
θ
sin
θ
−
sin
θ
cos
θ
]
\mathrm{R}_{\theta}^{\prime}=\left[\begin{array}{cc}{\cos \theta} & {\sin \theta} \\ {-\sin \theta} & {\cos \theta}\end{array}\right] \tag{RO-3}
Rθ′=[cosθ−sinθsinθcosθ](RO-3)
三维空间中的坐标旋转
公式RO-2可以看做是绕z轴正向旋转
θ
\theta
θ的角度，只有
x
,
y
x,y
x,y坐标变化，
z
z
z坐标不变，对应于yaw角的旋转，按照这种方式我们可以分别得到绕roll，pitch和yaw角的旋转公式：
(RO-4)
R
x
(
α
)
=
[
1
0
0
0
cos
α
sin
α
0
−
sin
α
cos
α
]
R
y
(
β
)
=
[
cos
β
0
−
sin
β
0
1
0
sin
β
0
cos
β
]
R
z
(
γ
)
=
[
cos
γ
sin
γ
0
−
sin
γ
cos
γ
0
0
0
1
]
\begin{array}{l}{\mathrm{R}_{x}(\alpha)=\left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \alpha} & {\sin \alpha} \\ {0} & {-\sin \alpha} & {\cos \alpha}\end{array}\right]} \\ {\mathrm{R}_{y}(\beta)=\left[\begin{array}{ccc}{\cos \beta} & {0} & {-\sin \beta} \\ {0} & {1} & {0} \\ {\sin \beta} & {0} & {\cos \beta}\end{array}\right]} \\ {\mathrm{R}_{z}(\gamma)=\left[\begin{array}{ccc}{\cos \gamma} & {\sin \gamma} & {0} \\ {-\sin \gamma} & {\cos \gamma} & {0} \\ {0} & {0} & {1}\end{array}\right]}\end{array} \tag{RO-4}
Rx(α)=⎣⎡1000cosα−sinα0sinαcosα⎦⎤Ry(β)=⎣⎡cosβ0sinβ010−sinβ0cosβ⎦⎤Rz(γ)=⎣⎡cosγ−sinγ0sinγcosγ0001⎦⎤(RO-4)
注意上式中的绕y轴的pitch旋转和另外两个有点不同。
LeGO中的坐标旋转问题
LeGO中的局部坐标系下的点转换到全局坐标系中去的过程在pointAssociateToMap中对坐标变换的数学表达中以及说明,根据公式PA-2，我们可以得到pointAssociateToMap对点进行的坐标变换：
旋转(记为公式RO-5)：
R
=
[
c
o
s
e
x
c
o
s
e
z
+
s
i
n
e
x
s
i
n
e
y
s
i
n
e
z
c
o
s
e
z
s
i
n
e
x
s
i
n
e
y
−
c
o
s
e
x
s
i
n
e
z
c
o
s
e
y
s
i
n
e
x
c
o
s
e
y
s
i
n
e
z
c
o
s
e
y
c
o
s
e
z
−
s
i
n
e
y
c
o
s
e
x
s
i
n
e
y
s
i
n
e
z
−
c
o
s
e
z
s
i
n
e
x
c
o
s
e
x
c
o
s
e
z
s
i
n
e
y
+
s
i
n
e
x
s
i
n
e
z
c
o
s
e
x
c
o
s
e
y
]
R=\left[\begin{array}{ccc}{cos_{ex} cos_{ez}+sin_{ex} sin_{ey} sin_{ez}} & {cos_{ez} sin_{ex} sin_{ey}-cos_{ex} sin_{ez}} & {cos_{ey} sin_{ex}} \\ {cos_{ey} sin_{ez}} & {cos_{ey} cos_{ez}} & {-sin_{ey}} \\ {cos_{ex} sin_{ey} sin_{ez}-cos_{ez} sin_{ex}} & {cos_{ex} cos_{ez} sin_{ey}+sin_{ex} sin_{ez}} & {cos_{ex} cos_{ey}}\end{array}\right]
R=⎣⎡cosexcosez+sinexsineysinezcoseysinezcosexsineysinez−cosezsinexcosezsinexsiney−cosexsinezcoseycosezcosexcosezsiney+sinexsinezcoseysinex−sineycosexcosey⎦⎤
平移：
t
=
[
t
x
t
y
t
z
]
t=\left[ \begin{array}{ccc} t_{x} \\ t_{y} \\ t_{z}\end{array} \right]
t=⎣⎡txtytz⎦⎤
先明确优化函数中的优化目标
优化函数优化的量是特征点与对应直线的距离(或者特征点与特征平面的距离)。
按照最优的情况下来说，特征点应该是在特征直线（或者特征平面）上的，所以距离应该为0.
但是当特征点到直线或者平面的距离不为0的时候，说明这个点由于运动产生了畸变，我们一开始估计的旋转 R R R和平移 T T T估计得不准确。所以对这个旋转和平移要进行调整。也就是每次优化后进行的如下操作：
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);
其中的变换涉及到对旋转矩阵求雅克比，对距离求导等。
先明确一下优化中涉及到的定义
1.局部坐标系中的点
(LM-a)
X
(
k
+
1
,
i
)
L
=
(
p
x
,
p
y
,
p
z
)
T
X_{(k+1, i)}^{L}=(p x, p y, p z)^{T} \tag{LM-a}
X(k+1,i)L=(px,py,pz)T(LM-a)
这里的点是特征点，边缘上的点或者平面上的点，点i在LiDAR坐标系下,k+1时刻的坐标.
2.变换 T ( k + 1 ) w T_{(k+1)}^{w} T(k+1)w表示特征点经过这个变换内包含的3个旋转个3个平移，可以准确地变换到全局地图中，与地图中特征边缘和平面的距离为0；
3.将从局部坐标系变换到全局地图坐标系的变换定义成一个函数
G
(
.
)
G( .)
G(.)这个函数将局部点
X
(
k
+
1
,
i
)
L
X^{L}_{(k+1, i)}
X(k+1,i)L转换到全局点
X
(
k
+
1
,
i
)
w
X_{(k+1, i)}^{w}
X(k+1,i)w。
(LM-b)
X
(
k
+
1
,
i
)
w
=
G
(
X
(
k
+
1
,
i
)
L
,
T
(
k
+
1
)
w
)
=
R
⋅
X
(
k
+
1
,
i
)
L
+
t
X_{(k+1, i)}^{w}=G\left(X_{(k+1, i)}^{L}, T_{(k+1)}^{w}\right)=R \cdot X_{(k+1, i)}^{L}+t \tag{LM-b}
X(k+1,i)w=G(X(k+1,i)L,T(k+1)w)=R⋅X(k+1,i)L+t(LM-b)
4.定义误差
(LM-c)
loss
=
d
=
D
(
X
(
k
+
1
,
i
)
w
,
m
a
p
)
\text {loss}=d=D\left(X_{(k+1, i)}^{w}, map\right) \tag{LM-c}
loss=d=D(X(k+1,i)w,map)(LM-c)
结合公式LM-b，LM-c
(LM-d)
l
o
s
s
=
d
=
D
(
X
(
k
+
1
,
i
)
w
,
m
a
p
)
=
D
(
G
(
X
(
k
+
1
,
i
)
L
,
T
(
k
+
1
)
w
)
,
m
a
p
)
=
D
(
R
⋅
X
(
k
+
1
,
i
)
L
+
t
,
m
a
p
)
\begin{aligned} & loss=d=D\left(X_{(k+1, i)}^{w}, map\right) \\ & =D\left(G\left(X_{(k+1, i)}^{L}, T_{(k+1)}^{w}\right), map\right) \\ & =D\left(R \cdot X_{(k+1, i)}^{L}+t, map\right) \\ \end{aligned} \tag{LM-d}
loss=d=D(X(k+1,i)w,map)=D(G(X(k+1,i)L,T(k+1)w),map)=D(R⋅X(k+1,i)L+t,map)(LM-d)
5.误差对旋转求偏导的过程
(LM-e) ∂ loss ∂ e x = ∂ D ( G ( X ( k + 1 , i ) L , T ( k + 1 ) w ) , m a p ) ∂ e x = ∂ D ( . ) ∂ G ( . ) ⋅ ∂ G ( . ) ∂ e x = ∂ D ( . ) ∂ G ( . ) ⋅ ∂ ( R ⋅ X ( k + 1 , 1 ) L + t ) ∂ e x = ∂ D ( . ) ∂ G ( . ) ⋅ ∂ ( R ⋅ X ( k + 1 , i ) L ) ∂ e x + ∂ D ( . ) ∂ G ( . ) ⋅ ∂ ( t ) ∂ e x = ∂ D ( . ) ∂ G ( . ) ⋅ ∂ ( R ⋅ X ( k + 1 , i ) L ) ∂ e x \begin{array}{c} {\frac{\partial \operatorname{loss}}{\partial e x}=\frac{\partial D\left(G\left(X_{(k+1, i)}^{L}, T_{(k+1)}^{w}\right), map\right)}{\partial e x}} \\ {=\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial G( .)}{\partial e x}}\\ {=\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial\left(R \cdot X_{(k+1,1)}^{L}+t\right)}{\partial e x}}\\ {=\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial\left(R \cdot X_{(k+1, i)}^{L}\right)}{\partial e x}+\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial(t)}{\partial e x}} \\ {=\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial\left(R \cdot X_{(k+1, i)}^{L}\right)}{\partial e x}} \end{array} \tag{LM-e} ∂ex∂loss=∂ex∂D(G(X(k+1,i)L,T(k+1)w),map)=∂G(.)∂D(.)⋅∂ex∂G(.)=∂G(.)∂D(.)⋅∂ex∂(R⋅X(k+1,1)L+t)=∂G(.)∂D(.)⋅∂ex∂(R⋅X(k+1,i)L)+∂G(.)∂D(.)⋅∂ex∂(t)=∂G(.)∂D(.)⋅∂ex∂(R⋅X(k+1,i)L)(LM-e)
6.误差对平移求偏导的过程
(LM-f) ∂ loss ∂ x = ∂ D ( G ( X ( k + 1 , i ) L , T ( k + 1 ) w ) , m a p ) ∂ x = ∂ D ( . ) ∂ G ( . ) ⋅ ∂ G ( . ) ∂ x = ∂ D ( . ) ∂ G ( . ) ⋅ ∂ ( R ⋅ X ( k + 1 , 1 ) L + t ) ∂ x = ∂ D ( . ) ∂ G ( . ) ⋅ ∂ ( R ⋅ X ( k + 1 , i ) L ) ∂ x + ∂ D ( . ) ∂ G ( . ) ⋅ ∂ ( t ) ∂ x = 0 + ∂ D ( . ) ∂ G ( . ) ⋅ ∂ ( t ) ∂ x = ∂ D ( . ) ∂ G ( . ) \begin{array}{c} {\frac{\partial \operatorname{loss}}{\partial x}=\frac{\partial D\left(G\left(X_{(k+1, i)}^{L}, T_{(k+1)}^{w}\right), map\right)}{\partial x}} \\ {=\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial G( .)}{\partial x}}\\ {=\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial\left(R \cdot X_{(k+1,1)}^{L}+t\right)}{\partial x}}\\ {=\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial\left(R \cdot X_{(k+1, i)}^{L}\right)}{\partial x}+\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial(t)}{\partial x}} \\ {=0+\frac{\partial D( .)}{\partial G( .)} \cdot \frac{\partial(t)}{\partial x}} \\ {=\frac{\partial D( .)}{\partial G( .)}} \end{array} \tag{LM-f} ∂x∂loss=∂x∂D(G(X(k+1,i)L,T(k+1)w),map)=∂G(.)∂D(.)⋅∂x∂G(.)=∂G(.)∂D(.)⋅∂x∂(R⋅X(k+1,1)L+t)=∂G(.)∂D(.)⋅∂x∂(R⋅X(k+1,i)L)+∂G(.)∂D(.)⋅∂x∂(t)=0+∂G(.)∂D(.)⋅∂x∂(t)=∂G(.)∂D(.)(LM-f)
6.
∂
D
(
⋅
)
∂
G
(
⋅
)
\frac{\partial D(\cdot)}{\partial G(\cdot)}
∂G(⋅)∂D(⋅)的求解
(LM-g)
∂
D
(
.
)
∂
G
(
.
)
=
∂
d
(
∂
X
(
(
k
+
1
,
i
)
)
w
)
\frac{\partial D( .)}{\partial G( .)}=\frac{\partial d}{\left(\partial X_{((k+1, i))}^{w}\right)} \tag{LM-g}
∂G(.)∂D(.)=(∂X((k+1,i))w)∂d(LM-g)
公式LM-g中对全局坐标系中的点求导，可以理解成求一个全局点的移动方向，点在这个方向上移动， d d d减小得最快。
所以这个方向就是沿着垂线的方向。
所以点到直线的方向就是底边的垂线方向。
点到平面的方向就是平面的法线方向。
∂ D ( . ) ∂ G ( . ) = ∂ d ( ∂ X ( ( k + 1 , i ) ) w ) = ( ∂ d ∂ x , ∂ d ∂ y , ∂ d ∂ z ) = ( l a , l b , l c ) \frac{\partial D( .)}{\partial G( .)}=\frac{\partial d}{\left(\partial X_{((k+1, i))}^{w}\right)}=\left(\frac{\partial d}{\partial x},\frac{\partial d}{\partial y},\frac{\partial d}{\partial z}\right)=(la,lb,lc) ∂G(.)∂D(.)=(∂X((k+1,i))w)∂d=(∂x∂d,∂y∂d,∂z∂d)=(la,lb,lc)
所以总结一下上面求解的两个部分：
(LM-h)
∂
(
R
∗
X
(
k
+
1
,
i
)
L
)
∂
e
x
=
∂
(
R
)
∂
e
x
⋅
X
(
k
+
1
,
i
)
L
=
∂
(
R
)
∂
e
x
⋅
(
p
x
,
p
y
,
p
z
)
T
=
[
s
y
⋅
c
x
⋅
s
z
c
z
⋅
s
y
⋅
c
x
−
s
x
⋅
s
y
−
s
x
⋅
s
z
−
s
x
⋅
c
z
−
c
x
c
y
⋅
c
x
⋅
s
z
c
y
⋅
c
z
⋅
c
x
−
c
y
⋅
s
x
]
⋅
(
p
x
,
p
y
,
p
z
)
T
\begin{aligned} &\frac{\partial\left(R * X_{(k+1, i)}^{L}\right)}{\partial e x}=\frac{\partial\left(R \right)}{\partial e x}\cdot X_{(k+1, i)}^{L} \\ & =\frac{\partial\left(R \right)}{\partial e x}\cdot \left( p_x,p_y,p_z \right)^{T} \\ & =\left[\begin{array}{ccc}{sy \cdot cx \cdot sz} & {cz \cdot sy \cdot cx} & {-sx \cdot sy} \\ {-sx \cdot sz} & {-sx \cdot cz} & {-cx} \\ {cy \cdot cx \cdot sz} & {cy \cdot cz \cdot cx} & {-cy \cdot sx}\end{array}\right] \cdot \left( p_x,p_y,p_z\right)^T \end{aligned} \tag{LM-h}
∂ex∂(R∗X(k+1,i)L)=∂ex∂(R)⋅X(k+1,i)L=∂ex∂(R)⋅(px,py,pz)T=⎣⎡sy⋅cx⋅sz−sx⋅szcy⋅cx⋅szcz⋅sy⋅cx−sx⋅czcy⋅cz⋅cx−sx⋅sy−cx−cy⋅sx⎦⎤⋅(px,py,pz)T(LM-h)
同样的做法可以求得
∂
(
R
×
X
(
k
+
1
,
i
)
L
)
∂
e
y
\frac{\partial\left(R \times X_{(k+1, i)}^{L}\right)}{\partial e y}
∂ey∂(R×X(k+1,i)L)
以及
∂
(
R
×
X
(
k
+
1
,
i
)
L
)
∂
e
z
\frac{\partial\left(R \times X_{(k+1, i)}^{L}\right)}{\partial e z}
∂ez∂(R×X(k+1,i)L)
,分别对应于代码中的ary和arz。
迭代计算的过程
代码中用的是高斯-牛顿法进行迭代计算求解的。
高斯-牛顿法的基本形式为：
J
T
J
Δ
x
=
−
J
⋅
f
(
x
)
J^{T}J \Delta{x}=-J \cdot f(x)
JTJΔx=−J⋅f(x)
代码中，雅克比矩阵
J
J
J为matA,
(matA)
m
a
t
A
=
[
∂
d
∂
r
o
l
l
∂
d
∂
p
i
t
c
h
∂
d
∂
y
a
w
∂
d
∂
t
x
∂
d
∂
t
y
∂
d
∂
t
z
]
matA=\left[\begin{array}{ccc} {\frac{\partial{d}}{\partial{roll}}} \\ {\frac{\partial{d}}{\partial{pitch}}} \\ {\frac{\partial{d}}{\partial{yaw}}} \\ {\frac{\partial{d}}{\partial{t_x}}} \\ {\frac{\partial{d}}{\partial{t_y}}} \\ {\frac{\partial{d}}{\partial{t_z}}} \\ \end{array} \right] \tag{matA}
matA=⎣⎢⎢⎢⎢⎢⎢⎢⎡∂roll∂d∂pitch∂d∂yaw∂d∂tx∂d∂ty∂d∂tz∂d⎦⎥⎥⎥⎥⎥⎥⎥⎤(matA)
To be continued.
1.苦力笨笨的博客:https://www.cnblogs.com/terencezhou/p/6235974.html
2.知乎SLAM专栏-「能儿」的回答：https://zhuanlan.zhihu.com/p/57351961
3.mathworld.wolfram.com：http://mathworld.wolfram.com/RotationMatrix.html
4.维基百科旋转矩阵解释：https://en.wikipedia.org/wiki/Euler_angles#Definition_by_intrinsic_rotations