数学推导
1、位姿变换
在LOAM中，odom和后端优化都是类似的问题，简单来说都是将当前点云提取特征，corner和surface，将当前帧特征点与map点云进行匹配，找到最有的位姿变换。
假设函数G(T)就是这个最优变化的变换。R为旋转矩阵，t为位移
G
(
T
)
=
R
∗
X
+
t
G(T)=R*X+t
G(T)=R∗X+t
T
=
(
R
,
t
)
=
(
r
x
,
r
y
,
r
z
,
t
x
,
t
y
,
t
z
)
T
T=(R,t)=(r_{x},r_{y},r_{z},t_{x},t_{y},t_{z})^{T}
T=(R,t)=(rx,ry,rz,tx,ty,tz)T
R旋转：
R=R(r_{x},r_{y},r_{z})=\begin{bmatrix}
cr_{x}*cr_{z}+sr_{y}*sr_{x}*sr_{z}& cr_{z}*sr_{y}*sr_{x}-cr_{y}*sr_{z} &cr_{x}*sr_{y} \\
cr_{x}*sr_{z}& cr_{x}*cr_{z} &-sr_{x} \\
cr_{y}*sr_{x}*sr_{z}-cr_{z}*sr_{y}& cr_{y}*cr_{z}*sr_{x}+sr_{y}*sr_{z} &cr_{y}*cr_{x}
\end{bmatrix}
t平移：
t
=
(
t
x
,
t
y
,
t
z
)
T
t=(t_{x},t_{y},t_{z})^T
t=(tx,ty,tz)T
2、损失函数
为了对位姿变换G(.)进行优化,我们需要定义损失函数，也就是当前点到上一帧点的距离。
定义D()
l
o
s
s
=
d
=
D
(
G
(
T
)
,
X
m
a
p
)
=
D
(
R
∗
X
c
u
r
+
t
,
X
m
a
p
)
loss=d=D(G(T),X_{map})=D(R*X_{cur}+t,X_{map})
loss=d=D(G(T),Xmap)=D(R∗Xcur+t,Xmap)
雅克比分量
-
位姿求导
∂ l o s s ∂ r x = ∂ D ∂ G ∗ ∂ G ∂ r x = ∂ D ∂ G ∗ ∂ ( R ∗ X + t ) ∂ r x \frac{\partial loss}{\partial r_{x}}=\frac{\partial D}{\partial G} *\frac{\partial G}{\partial r_{x}} =\frac{\partial D}{\partial G} *\frac{\partial (R*X+t)}{\partial r_{x}} ∂rx∂loss=∂G∂D∗∂rx∂G=∂G∂D∗∂rx∂(R∗X+t)
= ∂ D ∂ G ∗ ∂ ( R ∗ X ) ∂ r x + ∂ D ∂ G ∗ ∂ ( t ) ∂ r x = ∂ D ∂ G ∗ ∂ ( R ∗ X ) ∂ r x =\frac{\partial D}{\partial G} *\frac{\partial (R*X)}{\partial r_{x}} + \frac{\partial D}{\partial G} *\frac{\partial (t)}{\partial r_{x}} =\frac{\partial D}{\partial G} *\frac{\partial (R*X)}{\partial r_{x}} =∂G∂D∗∂rx∂(R∗X)+∂G∂D∗∂rx∂(t)=∂G∂D∗∂rx∂(R∗X) -
位移求导
∂ l o s s ∂ t x = ∂ D ∂ G ∗ ∂ G ∂ t x = ∂ D ∂ G ∗ ∂ ( R ∗ X + t ) ∂ t x \frac{\partial loss}{\partial t_{x}}=\frac{\partial D}{\partial G} *\frac{\partial G}{\partial t_{x}} =\frac{\partial D}{\partial G} *\frac{\partial (R*X+t)}{\partial t_{x}} ∂tx∂loss=∂G∂D∗∂tx∂G=∂G∂D∗∂tx∂(R∗X+t)
= ∂ D ∂ G ∗ ∂ ( R ∗ X ) ∂ t x + ∂ D ∂ G ∗ ∂ ( t ) ∂ t x = ∂ D ∂ G =\frac{\partial D}{\partial G} *\frac{\partial (R*X)}{\partial t_{x}} + \frac{\partial D}{\partial G} *\frac{\partial (t)}{\partial t_{x}} =\frac{\partial D}{\partial G} =∂G∂D∗∂tx∂(R∗X)+∂G∂D∗∂tx∂(t)=∂G∂D
综上所诉：
∂
D
∂
G
=
∂
d
∂
X
m
a
p
\frac{\partial D}{\partial G} =\frac{\partial d}{\partial X_{map}}
∂G∂D=∂Xmap∂d
d为点到map的距离，Xmap是一个点，距离对点求导，可疑理解为一个点的移动方向。
针对corner点，我们可以直接用点到与之匹配的直线之间的垂线方向对应的向量表示，针对surface点，我们可以直接用点到与之匹配的平面之间的垂线方向对应的向量(也是平面的法向量)表示。假设垂线对应的向量为（la, lb, lc），则有：
∂ D ∂ G = ∂ d ∂ X m a p = ( ∂ d ∂ x , ∂ d ∂ y , ∂ d ∂ z ) \frac{\partial D}{\partial G} =\frac{\partial d}{\partial X_{map}} =(\frac{\partial d}{\partial x}, \frac{\partial d}{\partial y},\frac{\partial d}{\partial z}) ∂G∂D=∂Xmap∂d=(∂x∂d,∂y∂d,∂z∂d)
高斯牛顿
J T J Δ x = − J T f ( x ) J^{T} J\Delta x=-J^{T}f(x) JTJΔx=−JTf(x)
其中J：
J
=
[
a
r
x
0
a
r
y
0
a
r
z
0
l
a
0
l
b
0
l
c
0
a
r
x
1
a
r
y
1
a
r
z
1
l
a
1
l
b
1
l
c
1
a
r
x
2
a
r
y
2
a
r
z
2
l
a
2
l
b
2
l
c
2
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
a
r
x
m
a
r
y
m
a
r
z
m
l
a
m
l
b
m
l
c
m
]
J=\begin{bmatrix} ar_{x_{0}} & ar_{y_{0}} & ar_{z_{0}} & l_{a_{0}} & l_{b_{0}} & l_{c_{0}}\\ ar_{x_{1}} & ar_{y_{1}} & ar_{z_{1}} & l_{a_{1}} & l_{b_{1}} & l_{c_{1}}\\ ar_{x_{2}} & ar_{y_{2}} & ar_{z_{2}} & l_{a_{2}} & l_{b_{2}} & l_{c_{2}}\\ ... & ... & ... & ... & ... & ...\\ ar_{x_{m}} & ar_{y_{m}} & ar_{z_{m}} & l_{a_{m}} & l_{b_{m}} & l_{c_{m}} \end{bmatrix}
J=
arx0arx1arx2...arxmary0ary1ary2...arymarz0arz1arz2...arzmla0la1la2...lamlb0lb1lb2...lbmlc0lc1lc2...lcm
f(x)：
f
(
x
)
=
[
l
o
s
s
0
l
o
s
s
1
l
o
s
s
2
.
.
.
l
o
s
s
m
]
f(x)=\begin{bmatrix} loss_{0}\\ loss_{1}\\ loss_{2}\\ ...\\ loss_{m} \end{bmatrix}
f(x)=
loss0loss1loss2...lossm
迭代优化
- 基于前一状态的位姿，以及IMU积分的结果，对位姿T进行估计，获得初始值；
- 基于位姿T的前一状态值，带入特征点云的每一个点的坐标，求得雅克比矩阵J和f(x)；
- 计算出雅可比矩阵的转置J^T，计算出正规方程AX=B中的A和B；
- 求解线性方程AX=B中的X，即Δx；
- 更新位姿T：T=T+Δx；
- 重复2~5，直至Δx中的旋转和平移增量小于某个阈值。
代码实现
先mark下。。。。。。