三维刚体运动
旋转矩阵
点、向量和坐标系
任意向量 a \pmb a aaa在一组基 ( e 1 , e 2 , e 3 ) (\pmb e_1,\pmb e_2,\pmb e_3) (eee1,eee2,eee3)下的坐标
a
=
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
a
1
e
1
+
a
2
e
2
+
a
3
e
3
\pmb a = [\pmb e_1,\pmb e_2,\pmb e_3 ] \begin{bmatrix} a_1\\a_2\\a_3 \end{bmatrix} = a_1 \pmb e_1 + a_2 \pmb e_2 + a_3 \pmb e_3
aaa=[eee1,eee2,eee3]⎣⎡a1a2a3⎦⎤=a1eee1+a2eee2+a3eee3
两个向量的内积
a
⋅
b
=
a
T
b
=
∑
i
=
1
3
a
i
b
i
=
∣
a
∣
∣
b
∣
cos
<
a
,
b
>
(内积)
\pmb a \cdot \pmb b = \pmb a^T \pmb b = \sum^3_{i=1}a_ib_i = \mid \pmb a\mid \mid b \mid \cos<a,b> \tag{内积}
aaa⋅bbb=aaaTbbb=i=1∑3aibi=∣aaa∣∣b∣cos<a,b>(内积)
两个向量的外积
a
×
b
=
∥
e
1
e
2
e
3
a
1
a
2
a
3
b
1
b
2
b
3
∥
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
1
]
=
[
0
−
a
3
a
2
a
3
0
−
a
3
−
a
2
a
1
0
]
b
≜
a
∧
b
(外积)
\pmb a \times \pmb b = \begin{Vmatrix} \pmb e_1 &\pmb e_2 &\pmb e_3\\ a_1 & a_2 & a_3\\ b_1 & b_2 & b_3 \end{Vmatrix}= \begin{bmatrix} a_2b_3-a_3b_2\\ a_3b_1-a_1b_3\\ a_1b_2-a_2b_1 \end{bmatrix}= \begin{bmatrix} 0&-a_3&a_2\\ a_3&0&-a_3\\ -a_2&a_1&0 \end{bmatrix}\pmb b \triangleq a ^\wedge b \tag{外积}
aaa×bbb=∥∥∥∥∥∥eee1a1b1eee2a2b2eee3a3b3∥∥∥∥∥∥=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a30⎦⎤bbb≜a∧b(外积)
外积的结果是一个向量，方向垂直于这两个向量，大小为
∣
a
∣
∣
b
∣
sin
<
a
,
b
>
\mid \pmb a \mid \mid \pmb b \mid \sin<a,b>
∣aaa∣∣bbb∣sin<a,b>，是两个向量张成的四边形的有向面积。通过定义
a
\pmb a
aaa的反对称矩阵
(
S
k
e
w
−
s
y
m
m
e
t
r
i
c
M
a
t
r
i
x
)
(\rm Skew-symmetric \ Matrix)
(Skew−symmetric Matrix)，
a
∧
a^\wedge
a∧把向量外积写成了矩阵和向量的乘法，变成了一个线性运算。
a
∧
=
[
0
−
a
3
a
2
a
3
0
−
a
3
−
a
2
a
1
0
]
(反对称矩阵)
a^\wedge = \begin{bmatrix} 0&-a_3&a_2\\ a_3&0&-a_3\\ -a_2&a_1&0 \end{bmatrix} \tag{反对称矩阵}
a∧=⎣⎡0a3−a2−a30a1a2−a30⎦⎤(反对称矩阵)
坐标系间的欧式变换
两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化，两个坐标系相差了一个欧式变换 E u c l i d e a n T r a n s f o r m \rm Euclidean \ Transform Euclidean Transform。
设某个单位正交基 ( e 1 , e 2 , e 3 ) (\pmb e_1,\pmb e_2,\pmb e_3) (eee1,eee2,eee3)经过一次旋转变成了 ( e 1 ′ , e 2 ′ , e 3 ′ ) (\pmb e_1',\pmb e_2',\pmb e_3') (eee1′,eee2′,eee3′)。那么，对于同一个向量 a \pmb a aaa,它在两个坐标系下的坐标为 [ a 1 , a 2 , a 3 ] T [a_1,a_2,a_3]^T [a1,a2,a3]T和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T [a_1',a_2',a_3']^T [a1′,a2′,a3′]T。由定义可知下式相等：
[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] [\pmb e_1,\pmb e_2,\pmb e_3] \begin{bmatrix} a_1\\a_2\\a_3 \end{bmatrix}= [\pmb e_1',\pmb e_2',\pmb e_3'] \begin{bmatrix} a_1'\\a_2'\\a_3' \end{bmatrix} [eee1,eee2,eee3]⎣⎡a1a2a3⎦⎤=[eee1′,eee2′,eee3′]⎣⎡a1′a2′a3′⎦⎤
两边同时左乘 [ e 1 T e 2 T e 3 T ] \begin{bmatrix}\pmb e_1^T\\\pmb e_2^T\\\pmb e_3^T\end{bmatrix} ⎣⎡eee1Teee2Teee3T⎦⎤,左边就变成了单位矩阵 I I I,得到如下等式：
[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] ≜ R a ′ (旋转矩阵) \begin{bmatrix} a_1\\ a_2\\ a_3 \end{bmatrix}= \begin{bmatrix} \pmb e_1^T \pmb e_1'&\pmb e_1^T \pmb e_2'&\pmb e_1^T \pmb e_3'\\ \pmb e_2^T \pmb e_1'&\pmb e_2^T \pmb e_2'&\pmb e_2^T \pmb e_3'\\ \pmb e_3^T \pmb e_1'&\pmb e_3^T \pmb e_2'&\pmb e_3^T \pmb e_3'\\ \end{bmatrix} \begin{bmatrix} a_1'\\ a_2'\\ a_3' \end{bmatrix} \triangleq \pmb R \pmb a' \tag{旋转矩阵} ⎣⎡a1a2a3⎦⎤=⎣⎡eee1Teee1′eee2Teee1′eee3Teee1′eee1Teee2′eee2Teee2′eee3Teee2′eee1Teee3′eee2Teee3′eee3Teee3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜RRRaaa′(旋转矩阵)
矩阵
R
\pmb R
RRR描述了旋转本身，被称为旋转矩阵
R
o
t
a
t
i
o
n
M
a
t
r
i
x
\rm Rotation \ Matrix
Rotation Matrix，是一个行列式为
1
1
1的正交矩阵。反之，行列式为
1
1
1的正交矩阵也是一个旋转矩阵。可将
n
n
n维旋转矩阵的集合定义如下，这种矩阵是特殊正交群
S
p
e
c
i
a
l
O
r
t
h
o
g
o
n
a
l
G
r
o
u
p
\rm Special \ Orthogonal \ Group
Special Orthogonal Group。
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
(SO)
SO(n) = \{\pmb R \in \mathbb R^{n \times n} \mid \pmb R \pmb R^T = \pmb I,det(\pmb R)=1\} \tag{SO}
SO(n)={RRR∈Rn×n∣RRRRRRT=III,det(RRR)=1}(SO)
它的逆（即转置）描述了一个相反的旋转
a
′
=
R
−
1
a
=
R
T
a
\pmb a' = \pmb R^{-1} \pmb a = \pmb R^T \pmb a
aaa′=RRR−1aaa=RRRTaaa
由于坐标系的平移只需要添加一个平移向量差值
t
\pmb t
ttt,所以得到两个坐标系的欧式变换方程
a
1
=
R
12
a
2
+
t
12
(变换方程)
\pmb a_1 = \pmb R_{12} \pmb a_2 + \pmb t_{12} \tag{变换方程}
aaa1=RRR12aaa2+ttt12(变换方程)
这里的 R 12 \pmb R_{12} RRR12的下标是从右往左读的，因为向量乘在它的右边，所以读作“把坐标系 2 2 2的向量变换到坐标系 1 1 1”中，然后添加一个平移分量。这里的平移分量是坐标系 1 1 1原点指向坐标系 2 2 2原点的向量。
变换矩阵与齐次坐标
由于上述的变换方程不是线性的，当经过多次坐标系变换时，表达或计算时复杂的，为此引入齐次坐标和变换矩阵，重写上述的变换方程
[
a
′
1
]
=
[
R
t
0
T
1
]
[
a
1
]
≜
T
[
a
1
]
\begin{bmatrix} \pmb a'\\1 \end{bmatrix}= \begin{bmatrix} \pmb R &\pmb t\\ \pmb 0^T&1 \end{bmatrix} \begin{bmatrix} \pmb a\\1 \end{bmatrix} \triangleq \pmb T \begin{bmatrix} \pmb a\\1 \end{bmatrix}
[aaa′1]=[RRR000Tttt1][aaa1]≜TTT[aaa1]
在三维向量的末尾添加
1
1
1,将其变成了四维向量，称为齐次坐标。
变换矩阵
T
\pmb T
TTT，左上角为旋转矩阵，右侧为平移向量，左下角为
0
\pmb 0
000向量,右下角为
1
1
1。这种矩阵又称为特殊欧氏群
S
p
e
c
i
a
l
E
u
c
l
i
d
e
a
n
G
r
o
u
p
\rm Special\ Euclidean \ Group
Special Euclidean Group
S
E
(
n
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
(
n
+
1
)
×
(
n
+
1
)
∣
R
∈
S
O
(
n
)
,
t
∈
R
n
}
SE(n) = \left\{ \pmb T = \begin{bmatrix} \pmb R &\pmb t\\ \pmb 0^T&1 \end{bmatrix} \in \mathbb R^{(n+1) \times (n+1)} \mid \pmb R \in SO(n), \pmb t \in \mathbb R^n \right\}
SE(n)={TTT=[RRR000Tttt1]∈R(n+1)×(n+1)∣RRR∈SO(n),ttt∈Rn}
它的逆描述了一个反向的变化
T
−
1
=
[
R
T
−
R
T
t
0
T
1
]
\pmb T^{-1} = \begin{bmatrix} \pmb R^T &-\pmb R^T\pmb t\\ \pmb 0^T&1 \end{bmatrix}
TTT−1=[RRRT000T−RRRTttt1]
旋转向量和欧拉角
旋转向量
希望有一种方式能够紧凑的描述旋转和平移。事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。旋转向量（轴角/角轴，
A
x
i
s
−
A
n
g
l
e
\rm Axis-Angle
Axis−Angle）,其方向与旋转轴一致，而长度等于旋转角。考虑到某个用
R
\pmb R
RRR表示的旋转。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量
n
\pmb n
nnn,角度为
θ
\theta
θ,那么向量
θ
n
\theta \pmb n
θnnn也可以描述这个旋转。
从旋转向量到旋转矩阵的转换过程由罗德里格斯公式
R
o
d
r
i
g
u
e
′
s
F
o
r
m
u
l
a
\rm Rodrigue's \ Formula
Rodrigue′s Formula表明
R
=
cos
θ
I
+
(
1
−
cos
θ
)
n
n
T
+
sin
θ
n
∧
\pmb R = \cos \theta \pmb I + (1-\cos \theta) \pmb n \pmb n^T + \sin \theta \pmb n^\wedge
RRR=cosθIII+(1−cosθ)nnnnnnT+sinθnnn∧
符号
∧
^\wedge
∧是向量到反对称矩阵的转换符。
反之，也可以计算一个旋转矩阵到旋转向量的转换。对于转角
θ
\theta
θ,
θ
=
arccos
t
r
(
R
)
−
1
2
\theta = \arccos \frac{tr(R)-1}{2}
θ=arccos2tr(R)−1
关于转轴
n
\pmb n
nnn,旋转轴上的向量在旋转后不发生变化，说明
R
n
=
n
\pmb R \pmb n = \pmb n
RRRnnn=nnn
因此，转轴 n n n是矩阵 R \pmb R RRR特征值 1 1 1对应的特征向量。求此方程，再归一化，就得到了旋转轴。
欧拉角
欧拉角提供了一种非常直观的方式来描述旋转，它使用了 3 3 3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。欧拉角使用偏航-俯仰-滚转 y a w − p i t c h − r o l l \rm yaw-pitch-roll yaw−pitch−roll , 3 3 3个角度来描述一个旋转。它等价于 Z Y X ZYX ZYX轴的旋转。假设一个刚体的前方为 X X X轴，右侧为 Y Y Y轴，上方为 Z Z Z轴。可以把任意旋转分解为以下操作
- 绕物体的 Z Z Z轴旋转，得到偏航角 y a w \rm yaw yaw
- 绕旋转之后的 Y Y Y轴旋转，得到俯仰角 p i t c h \rm pitch pitch
- 绕旋转之后的 X X X轴旋转，得到滚转角 r o l l \rm roll roll
欧拉角存在奇异性问题，在俯仰角为 ± 90 \pm 90 ±90度时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度。因此欧拉角不适用于插值和迭代，往往只用于人机交互中。
四元数
四元数定义
可以证明找不到不带奇异性的三维向量描述方式。四元数是
H
a
m
i
l
t
o
n
\rm Hamilton
Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。
把四元数与复数类比可以较好的理解四元数。例如，当我们想要将复平面的向量旋转
θ
\theta
θ角度时，可以给这个复向量乘以
e
i
θ
=
cos
θ
+
i
sin
θ
e^{i\theta} = \cos \theta + i \ \sin \theta
eiθ=cosθ+i sinθ。所以，在二维的情况下，旋转可以由单位复数来描述。类似的三维旋转可以由单位四元数来描述。
一个四元数
q
\pmb q
qqq拥有一个实部和三个虚部。
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
=
[
s
,
v
]
T
{
s
=
q
0
,
v
=
[
q
1
,
q
2
,
q
3
]
T
}
\pmb q = q_0+q_1i+q_2j+q_3k = [s,\pmb v]^T\{s = q_0,\pmb v = [q_1,q_2,q_3]^T\}
qqq=q0+q1i+q2j+q3k=[s,vvv]T{s=q0,vvv=[q1,q2,q3]T}
s
s
s称为四元数的实部，
v
\pmb v
vvv称为四元数的虚部。如果虚部为0，称为实四元数；如果实部为0，则称为虚四元数。虚部满足下列条件
{
i
2
=
j
2
=
k
2
=
−
1
i
j
=
k
,
j
i
=
−
k
j
k
=
i
,
k
j
=
−
i
k
i
=
j
,
i
k
=
−
j
\begin{cases} i^2=j^2=k^2 = -1\\ ij = k,ji = -k\\ jk = i,kj = -i\\ ki = j,ik = -j \end{cases}
⎩⎪⎪⎪⎨⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
四元数运算
- 加法和减法
q a ± q b = [ s a ± s b , v a ± v b ] T \pmb q_a \pm \pmb q_b = [s_a \pm s_b,\pmb v_a \pm \pmb v_b]^T qqqa±qqqb=[sa±sb,vvva±vvvb]T
- 乘法
每一项分别相乘后相加
q a q b = [ s a s b − v a T v b , s a v b + s b v a + v a × v b ] T \pmb q_a \pmb q_b = [s_as_b-\pmb v_a^T\pmb v_b,s_a\pmb v_b+s_b \pmb v_a + \pmb v_a \times \pmb v_b]^T qqqaqqqb=[sasb−vvvaTvvvb,savvvb+sbvvva+vvva×vvvb]T
- 模长
∣ ∣ q a ∣ ∣ = s a 2 + x a 2 + y a 2 + z a 2 \mid\mid \pmb q_a \mid\mid = \sqrt{s_a^2+x_a^2+y_a^2+z_a^2} ∣∣qqqa∣∣=sa2+xa2+ya2+za2
∣ ∣ q a q b ∣ ∣ = ∣ ∣ q a ∣ ∣ ⋅ ∣ ∣ q b ∣ ∣ \mid \mid \pmb q_a \pmb q_b \mid\mid = \mid\mid \pmb q_a \mid\mid \cdot \mid\mid \pmb q_b \mid\mid ∣∣qqqaqqqb∣∣=∣∣qqqa∣∣⋅∣∣qqqb∣∣
- 共轭
q a ∗ = [ s a , − v a ] T \pmb q_a^* = [s_a,-\pmb v_a]^T qqqa∗=[sa,−vvva]T
q ∗ q = q q ∗ = [ s a 2 + v T v , 0 ] T \pmb q^* \pmb q = \pmb q \pmb q^* = [s_a^2+\pmb v^T\pmb v,0]^T qqq∗qqq=qqqqqq∗=[sa2+vvvTvvv,0]T
- 逆
q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 , q q − 1 = 1 , ( q a q b ) − 1 = q b − 1 q a − 1 \pmb q^{-1} = \pmb q^* / \mid\mid \pmb q \mid\mid ^2,\pmb q \pmb q^{-1} = \pmb 1,(\pmb q_a \pmb q_b)^{-1} = q_b^{-1} q_a^{-1} qqq−1=qqq∗/∣∣qqq∣∣2,qqqqqq−1=111,(qqqaqqqb)−1=qb−1qa−1
- 数乘
k q = [ k s , k v ] T k \pmb q = [ks,k\pmb v]^T kqqq=[ks,kvvv]T
用四元数表示旋转
我们可以用四元数表达对一个点的旋转。假设有一个空间三维点
p
=
[
x
,
y
,
z
]
∈
R
3
\pmb p=[x,y,z] \in \mathbb{R}^3
ppp=[x,y,z]∈R3,以及一个由单位四元数
q
\pmb q
qqq指定的旋转。三维点
p
\pmb p
ppp经过旋转之后变为
p
′
\pmb p'
ppp′。
首先把三维空间点用一个虚四元数来描述
p
=
[
0
,
x
,
y
,
z
]
T
=
[
0
,
v
]
T
\pmb p = [0,x,y,z]^T = [0,\pmb v]^T
ppp=[0,x,y,z]T=[0,vvv]T
旋转后的点
p
′
\pmb p'
ppp′可表示为这样的乘积
p
′
=
q
p
q
−
1
\pmb p' = \pmb q \pmb p \pmb q^{-1}
ppp′=qqqpppqqq−1
相似、仿射、射影变换
-
相似变换
相似变换比欧式变换多了一个自由度-缩放因子 s s s，它允许物体进行均匀缩放。
T s = [ s R t 0 T 1 ] \pmb T s = \begin{bmatrix} s \pmb R &t\\ \pmb 0^T &1 \end{bmatrix} TTTs=[sRRR000Tt1]
-
仿射变换
与欧式变换不同，仿射变换只要求 A A A是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫做正交投影。
T A = [ A t 0 T 1 ] \pmb T_A = \begin{bmatrix} \pmb A & \pmb t\\ \pmb 0^T & 1 \end{bmatrix} TTTA=[AAA000Tttt1]
-
射影变换
射影变换是最一般的变换。左上角为可逆矩阵 A A A，右上角为平移 t \pmb t ttt，左下角为所缩放 a T \pmb a^T aaaT。由于采样了齐次坐标，当 v ≠ 0 v \neq 0 v=0时，我们可以对整个矩阵除以 v v v得到一个右下角为 1 1 1的矩阵；否则得到右下角为 0 0 0的矩阵
T P = [ A t a T v ] \pmb T_P = \begin{bmatrix} \pmb A &\pmb t\\ \pmb a^T &v \end{bmatrix} TTTP=[AAAaaaTtttv]
代码实现
| 数据结构 | Eigen:: |
|---|---|
| 旋转矩阵 3 × 3 3 \times 3 3×3 | Matrix3d |
| 旋转向量 3 × 1 3 \times 1 3×1 | AngleAxisd |
| 欧拉角 3 × 1 3 \times 1 3×1 | Vector3d |
| 四元数 4 × 1 4 \times 1 4×1 | Quaterniond |
| 欧式变换矩阵 4 × 4 4 \times 4 4×4 | Isometry3d |
| 仿射变换 4 × 4 4 \times 4 4×4 | Affine3d |
| 射影变换 4 × 4 4 \times 4 4×4 | Projective3d |
李群与李代数
由于$ \rm SLAM$中的状态估计问题是一个优化问题，因此涉及到用于描述相机位姿的变量的求导。因此引入李群与李代数。
群
群是一种集合加上一种运算的代数结构。我们把集合记作 A A A，运算记作 ⋅ \cdot ⋅,那么群可以记作 G = ( A , ⋅ ) G=(A,\cdot) G=(A,⋅)。群必须满足下列性质
-
封闭性： ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall a_1,a_2 \in A,a_1 \cdot a_2 \in A ∀a1,a2∈A,a1⋅a2∈A
-
结合律： ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall a_1,a_2,a_3 \in A,(a_1 \cdot a_2) \cdot a_3 = a_1 \cdot (a_2 \cdot a_3) ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
-
零元： ∃ a 0 ∈ A , s . t . ∃ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a \exists a_0 \in A, \ \ s.t. \ \exists a\in A,a_0 \cdot a = a \cdot a_0 = a ∃a0∈A, s.t. ∃a∈A,a0⋅a=a⋅a0=a
-
逆： ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 \forall a \in A, \exists a^{-1} \in A, \ \,s.t. \ \ a \cdot a^{-1}=a_0 ∀a∈A,∃a−1∈A, s.t. a⋅a−1=a0
矩阵中常见的群有：
- 一般线性群 G L ( n ) GL(n) GL(n)：指 n × n n \times n n×n的可逆矩阵，他们对矩阵乘法成群。
- 特殊正交群 S O ( n ) SO(n) SO(n)：也就是所谓的旋转矩阵群，其中 S O ( 2 ) SO(2) SO(2)和 S O ( 3 ) SO(3) SO(3)最为常见。
- 特殊欧氏群 S E ( n ) SE(n) SE(n)：也就是前面提到的 n n n维欧式变换，如 S E ( 2 ) SE(2) SE(2)和 S E ( 3 ) SE(3) SE(3)。
李群是指具有连续（光滑）性质的群，如 S O ( 3 ) , S E ( 3 ) SO(3),SE(3) SO(3),SE(3)
李代数的引出
- 旋转矩阵对时间的导数是一个反对称矩阵右乘它本身
R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) \dot{\pmb{R}}(t) = \pmb \phi(t)^\wedge \pmb R(t) = \begin{bmatrix} 0 &-\phi_3&\phi_2\\ \phi_3&0&-\phi_1\\ -\phi_2&\phi_1&0 \end{bmatrix} \pmb R(t) RRR˙(t)=ϕϕϕ(t)∧RRR(t)=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤RRR(t)
- 李群空间的任意一个旋转矩阵 R \pmb R RRR都可以用李代数空间的一个向量 R ˙ ( t 0 ) = ϕ ( t 0 ) = ϕ 0 \dot{\pmb{R}}(t_0) =\pmb \phi(t_0) = \pmb \phi_0 RRR˙(t0)=ϕϕϕ(t0)=ϕϕϕ0的反对称矩阵指数来近似
R ( t ) = exp ( ϕ 0 ∧ t ) \pmb R(t) = \exp(\pmb \phi_0^\wedge \pmb t) RRR(t)=exp(ϕϕϕ0∧ttt)
李代数的定义
每个李群都有与之对应的李代数。李代数描述了李群的局部性质，是单位元附近的正切空间。李代数的定义如下：
李代数由一个集合 V \mathbb V V，一个数域 F \mathbb F F和一个二元运算 [ , ] [,] [,]组成。如果满足以下性质，则称 ( V , F , [ , ] ) (\mathbb V,\mathbb F,[,]) (V,F,[,])为一个李代数，记为 g \frak g g
-
封闭性： ∀ X , Y ∈ V , [ X , Y ] ∈ V \forall \pmb X,\pmb Y \in \mathbb V,[\pmb X,\pmb Y] \in \mathbb V ∀XXX,YYY∈V,[XXX,YYY]∈V
-
双线性： ∀ X , Y , Z ∈ V , a , b ∈ F \forall \pmb X,\pmb Y ,\pmb Z \in \mathbb V,a,b \in \mathbb F ∀XXX,YYY,ZZZ∈V,a,b∈F,有
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [a\pmb X+b\pmb Y,\pmb Z] = a[\pmb X,\pmb Z] +b[\pmb Y,\pmb Z]\\ [\pmb Z,a\pmb X+b\pmb Y] = a[\pmb Z,\pmb X] + b[\pmb Z,\pmb Y] [aXXX+bYYY,ZZZ]=a[XXX,ZZZ]+b[YYY,ZZZ][ZZZ,aXXX+bYYY]=a[ZZZ,XXX]+b[ZZZ,YYY] -
自反性： ∀ X ∈ V , [ X , X ] = 0 \forall \pmb X\in \mathbb V,[\pmb X,\pmb X] = 0 ∀XXX∈V,[XXX,XXX]=0
-
雅可比等价： ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ X , Y ] ] + [ Y , [ Z , X ] ] = 0 \forall \pmb X,\pmb Y,\pmb Z \in \mathbb V,[\pmb X,[\pmb Y,\pmb Z]] + [\pmb Z,[\pmb X,\pmb Y]] + [\pmb Y,[\pmb Z,\pmb X]] = 0 ∀XXX,YYY,ZZZ∈V,[XXX,[YYY,ZZZ]]+[ZZZ,[XXX,YYY]]+[YYY,[ZZZ,XXX]]=0
其中二元运算被称为李括号。
李代数$\frak so \rm (3) $
之前提到的 ϕ \phi ϕ，事实上是一种李代数。 S O ( 3 ) SO(3) SO(3)对应的李代数是定义在 R 3 \mathbb R ^3 R3上的向量，我们记作 ϕ \phi ϕ。其中 Φ = ϕ ∧ \pmb \Phi = \phi^\wedge ΦΦΦ=ϕ∧。$\frak so \rm (3) $是一个三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表示旋转矩阵的导数。
- 李括号
[ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ [\pmb\phi_1,\pmb\phi_2] = (\pmb \Phi_1\pmb \Phi_2 - \pmb \Phi_2\pmb \Phi_1)^{\vee} [ϕϕϕ1,ϕϕϕ2]=(ΦΦΦ1ΦΦΦ2−ΦΦΦ2ΦΦΦ1)∨
- $\frak so \rm (3) $定义
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \frak so \rm{(3)} = \{\phi \in \mathbb{R}^3,\Phi\ = \phi^\wedge \in \mathbb{R}^{3 \times 3}\} so(3)={ϕ∈R3,Φ =ϕ∧∈R3×3}
- 指数映射
R = exp ( ϕ ∧ ) = exp ( θ a ∧ ) = cos θ I + ( 1 − cos θ ) a a T + sin θ a ∧ \pmb R = \exp(\pmb \phi^\wedge) = \exp(\theta \pmb a^\wedge) = \cos \theta \pmb I+(1-\cos \theta)\pmb a \pmb a^T + \sin \theta \pmb a^\wedge RRR=exp(ϕϕϕ∧)=exp(θaaa∧)=cosθIII+(1−cosθ)aaaaaaT+sinθaaa∧
- 对数映射
θ = arccos t r ( R ) − 1 2 , R a = a \theta = \arccos \frac{tr(\pmb R)-1}2,\pmb R\pmb a = \pmb a θ=arccos2tr(RRR)−1,RRRaaa=aaa
李代数$\frak se \rm (3) $
- 李括号
[ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ , ξ 2 ∧ ξ 1 ∧ ) ∨ [\pmb \xi_1,\pmb \xi_2] = (\pmb \xi_1^\wedge\pmb \xi_2^\wedge,\pmb \xi_2^\wedge\pmb \xi_1^\wedge)^\vee [ξξξ1,ξξξ2]=(ξξξ1∧ξξξ2∧,ξξξ2∧ξξξ1∧)∨
- $\frak se \rm (3) $定义
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ρ 0 T 0 ] ∈ R 4 × 4 } \frak se \rm (3) = \Big\{ \pmb \xi=\begin{bmatrix}\pmb\rho\\\pmb\phi\end{bmatrix} \in \mathbb{R}^6,\pmb \rho \in \mathbb{R}^3,\pmb \phi\in \frak so \rm (3),\pmb \xi^\wedge= \begin{bmatrix}\pmb\phi&\pmb\rho\\\pmb 0^T &0\end{bmatrix} \in \mathbb R^{4 \times 4} \Big\} se(3)={ξξξ=[ρρρϕϕϕ]∈R6,ρρρ∈R3,ϕϕϕ∈so(3),ξξξ∧=[ϕϕϕ000Tρρρ0]∈R4×4}
我们把每个$\frak se \rm (3) 元 素 记 作 元素记作 元素记作\pmb \xi ， 他 是 一 个 六 维 向 量 。 前 三 维 为 平 移 ， 记 作 ，他是一个六维向量。前三维为平移，记作 ，他是一个六维向量。前三维为平移，记作\pmb \rho ； 后 三 维 位 旋 转 ， 记 作 ；后三维位旋转，记作 ；后三维位旋转，记作\pmb \phi ， 实 际 上 是 ，实际上是 ，实际上是\frak so \rm (3) 的 元 素 。 同 时 扩 展 了 的元素。同时扩展了 的元素。同时扩展了 ^\wedge 的 含 义 。 在 的含义。在 的含义。在\frak se \rm (3) 中 ， 使 用 中，使用 中，使用 ^\wedge 将 一 个 六 维 向 量 转 换 为 四 维 矩 阵 ， 但 不 表 示 为 反 对 称 。 使 用 将一个六维向量转换为四维矩阵，但不表示为反对称。使用 将一个六维向量转换为四维矩阵，但不表示为反对称。使用 \wedge$和$ \vee$符号代指“从向量到矩阵”和“从矩阵到向量”的关系。
- 指数映射
T = exp ( ξ ∧ ) = exp ( θ a ∧ ) = [ R J ρ 0 T 1 ] \pmb T = \exp(\pmb \xi^\wedge) = \exp(\theta\pmb a^\wedge) = \begin{bmatrix} \pmb R &\pmb J \pmb \rho\\ \pmb 0^T&1 \end{bmatrix} TTT=exp(ξξξ∧)=exp(θaaa∧)=[RRR000TJJJρρρ1]
J = sin θ θ I + ( 1 − sin θ θ ) a a T + 1 − cos θ θ a ∧ \pmb J = \frac{\sin \theta}{\theta} \pmb I + (1-\frac{\sin \theta}{\theta}) \pmb a \pmb a^T + \frac{1-\cos \theta}{\theta} \pmb a^\wedge JJJ=θsinθIII+(1−θsinθ)aaaaaaT+θ1−cosθaaa∧
- 对数映射
θ = arccos t r ( R ) − 1 2 , R a = a , t = J ρ \theta = \arccos \frac{tr(\pmb R)-1}2,\pmb R\pmb a = \pmb a,\pmb t = \pmb J \pmb \rho θ=arccos2tr(RRR)−1,RRRaaa=aaa,ttt=JJJρρρ
李代数求导与扰动模型
BCH公式与近似公式
为了探究李代数的导数，先讨论李代数加法的性质，即探讨
S
O
(
3
)
SO(3)
SO(3)中完成两个矩阵乘法时，李代数中
s
o
(
3
)
\frak so \rm(3)
so(3)上是否完成了两个李代数的加法。
ln
(
exp
(
A
)
+
exp
(
B
)
)
=
A
+
B
+
1
2
[
A
,
B
]
+
1
12
[
A
,
[
A
,
B
]
]
−
1
12
[
B
,
[
A
,
B
]
]
+
⋯
(BCH)
\ln(\exp(A)+\exp(B)) = A + B +\frac12[A,B] + \frac{1}{12}[A,[A,B]] - \frac 1{12}[B,[A,B]]+\cdots \tag{BCH}
ln(exp(A)+exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+⋯(BCH)
考虑
S
O
(
3
)
SO(3)
SO(3)上的李代数
ln
(
exp
(
ϕ
1
∧
)
exp
(
ϕ
2
∧
)
)
∨
\ln (\exp(\phi_1^\wedge)\exp(\phi_2^\wedge))^\vee
ln(exp(ϕ1∧)exp(ϕ2∧))∨,当
ϕ
1
\phi_1
ϕ1或
ϕ
2
\phi_2
ϕ2为小量时，小量二次以上的项都可以被忽略。此时，BCH拥有线性近似表达
ln
(
exp
(
ϕ
1
∧
)
exp
(
ϕ
2
∧
)
)
∨
≈
{
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
ϕ
1
i
s
t
h
e
s
m
a
l
l
v
a
l
u
e
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
ϕ
2
i
s
t
h
e
s
m
a
l
l
v
a
l
u
e
(BCH近似)
\ln(\exp(\phi_1^\wedge) \exp(\phi_2^\wedge))^\vee \approx \begin{cases} \pmb J_l(\phi_2)^{-1} \phi_1 + \phi_2 &\phi_1 \ \rm is \ the \ small \ value\\ \pmb J_r(\phi_1)^{-1} \phi_2 + \phi_1 &\phi_2 \ \rm is \ the \ small \ value\\ \end{cases} \tag{BCH近似}
ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{JJJl(ϕ2)−1ϕ1+ϕ2JJJr(ϕ1)−1ϕ2+ϕ1ϕ1 is the small valueϕ2 is the small value(BCH近似)
以第一个近似为例。该式告诉我们，当对一个旋转矩阵
R
2
\pmb R_2
RRR2（李代数为
ϕ
2
\phi_2
ϕ2）左乘一个微小旋转矩阵
R
1
\pmb R_1
RRR1（李代数为
ϕ
1
\phi_1
ϕ1）时，可以近似地看做，在原有的李代数
ϕ
2
\phi_2
ϕ2上加上了一项
J
l
(
ϕ
2
)
−
1
ϕ
1
\pmb J_l(\phi_2)^{-1}\phi_1
JJJl(ϕ2)−1ϕ1。同样，第二个近似描述了右乘一个微小位移的情况。于是，李代数在
B
C
H
BCH
BCH近似的情况下，分成了左乘和右乘近似两种。
J l = J = sin θ θ I + ( 1 − sin θ θ ) a a T + 1 − cos θ θ a ∧ \pmb J_l = \pmb J = \frac{\sin \theta}{\theta} \pmb I + (1-\frac{\sin \theta}{\theta})\pmb a \pmb a^T +\frac{1-\cos \theta}{\theta} \pmb a^\wedge JJJl=JJJ=θsinθIII+(1−θsinθ)aaaaaaT+θ1−cosθaaa∧
J l − 1 = θ 2 cot θ 2 I + ( 1 − θ 2 cot θ 2 ) a a T − θ 2 a ∧ \pmb J_l^{-1} = \frac \theta2 \cot \frac \theta 2 \pmb I +(1-\frac \theta2\cot\frac \theta2)\pmb a\pmb a^T - \frac \theta 2 \pmb a^\wedge JJJl−1=2θcot2θIII+(1−2θcot2θ)aaaaaaT−2θaaa∧
J r ( ϕ ) = J l ( − ϕ ) \pmb J_r(\phi) = \pmb J_l(-\phi) JJJr(ϕ)=JJJl(−ϕ)
为了更好理解，简化BCH近似公式。假定对某个旋转
R
\pmb R
RRR，对应的李代数为
ϕ
\phi
ϕ。我们给它左乘一个微小旋转，记作
Δ
R
\Delta \pmb R
ΔRRR，对应的李代数为
Δ
ϕ
\Delta \phi
Δϕ。那么，在李群上得到的结果就是
Δ
R
⋅
R
\Delta \pmb R \cdot \pmb R
ΔRRR⋅RRR，而在李代数上，根据BCH近似，为
J
l
−
1
(
ϕ
)
Δ
ϕ
+
ϕ
\pmb J_l^{-1}(\phi)\Delta\phi+\phi
JJJl−1(ϕ)Δϕ+ϕ。即如下所示
exp
(
Δ
ϕ
∧
)
exp
(
ϕ
∧
)
≈
exp
(
(
ϕ
+
J
l
−
1
(
ϕ
)
Δ
ϕ
)
∧
)
\exp(\Delta \phi^\wedge) \exp(\phi^\wedge) \approx \exp\bigg((\phi+\pmb J_l^{-1}(\phi)\Delta\phi)^\wedge\bigg)
exp(Δϕ∧)exp(ϕ∧)≈exp((ϕ+JJJl−1(ϕ)Δϕ)∧)
反之，如果在李代数上进行加法，让一个
ϕ
\phi
ϕ加上
Δ
ϕ
\Delta\phi
Δϕ,那么可以近似为李群上带左右雅克比的乘法
exp
(
(
ϕ
+
Δ
ϕ
)
∧
)
≈
exp
(
(
J
l
Δ
ϕ
)
∧
)
exp
(
ϕ
∧
)
=
exp
(
ϕ
∧
)
exp
(
(
J
r
Δ
ϕ
)
∧
)
\exp((\phi+\Delta \phi)^\wedge) \approx \exp((\pmb J_l\Delta \phi)^\wedge)\exp(\phi^\wedge) = \exp(\phi^\wedge)\exp((\pmb J_r\Delta\phi)^\wedge)
exp((ϕ+Δϕ)∧)≈exp((JJJlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JJJrΔϕ)∧)
对于
S
E
(
3
)
SE(3)
SE(3)，也有类似的近似
exp
(
Δ
ξ
)
exp
(
ξ
∧
)
≈
exp
(
(
J
l
−
1
Δ
ξ
+
ξ
)
∧
)
\exp(\Delta\pmb \xi) \exp(\pmb \xi^\wedge) \approx \exp \bigg( ( \pmb{\mathcal{J}}_l^{-1}\Delta \pmb \xi + \pmb \xi )^\wedge \bigg)
exp(Δξξξ)exp(ξξξ∧)≈exp((JJJl−1Δξξξ+ξξξ)∧)
exp ( ξ ∧ ) exp ( Δ ξ ) ≈ exp ( ( J r − 1 Δ ξ + ξ ) ∧ ) \exp(\pmb \xi^\wedge) \exp(\Delta\pmb \xi) \approx \exp \bigg( ( \pmb{\mathcal{J}}_r^{-1}\Delta \pmb \xi + \pmb \xi )^\wedge \bigg) exp(ξξξ∧)exp(Δξξξ)≈exp((JJJr−1Δξξξ+ξξξ)∧)
李代数求导
应用场景
不防设某个时刻机器人的位姿为
T
\pmb T
TTT。它观察到了一个世界坐标位于
p
\pmb p
ppp的点，产生了一个观察数据
z
\pmb z
zzz。那么由坐标变化关系知
z
=
T
p
+
w
\pmb z = \pmb T \pmb p + \pmb w
zzz=TTTppp+www
其中
w
\pmb w
www为随机噪声。由于它的存在，
z
\pmb z
zzz往往不可能精确的满足
z
=
T
p
\pmb z = \pmb T \pmb p
zzz=TTTppp的关系。所以，我们通常会计算理想的观测与实际数据的误差
e
=
z
−
T
p
\pmb e = \pmb z-\pmb T \pmb p
eee=zzz−TTTppp
假设一共有
N
N
N个这样的路标点和观测，于是就
N
N
N个上式。对机器人进行位姿估计时，相当于寻找一个最优的
T
\pmb T
TTT，使得整体误差最小化
min
T
J
(
T
)
=
∑
i
=
1
N
∣
∣
z
i
−
T
p
i
∣
∣
2
2
\min_{\pmb T} J(\pmb T) = \sum_{i=1}^N \mid\mid \pmb z_i-\pmb T\pmb p_i\mid\mid^2_2
TTTminJ(TTT)=i=1∑N∣∣zzzi−TTTpppi∣∣22
求解此问题，需要计算目标函数
J
J
J关于变化矩阵
T
\pmb T
TTT的导数。我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。
求解途径
- 用李代数表示姿态，然后根据李代数加法对李代数求导
- 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动或右扰动模型
由于第一种方法得出的结果比第二种方法更复杂，所以普遍采用第二种方法求导
S O ( 3 ) SO(3) SO(3)李代数求导
对
R
\pmb R
RRR进行一次扰动
Δ
R
\Delta R
ΔR，看结果相对于扰动的变化率。这个扰动以左乘为例。设左扰动对应的李代数为
φ
\pmb \varphi
φφφ，然后对
φ
\pmb \varphi
φφφ求导
∂
(
R
p
)
∂
φ
=
lim
φ
→
0
exp
(
φ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
φ
=
lim
φ
→
0
(
I
+
φ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
φ
=
lim
φ
→
0
φ
∧
R
p
φ
=
lim
φ
→
0
−
(
R
p
)
∧
φ
φ
=
−
(
R
p
)
∧
\begin{aligned} \frac{\partial (\pmb R \pmb p)}{\partial \pmb \varphi} &= \lim_{\pmb \varphi \rightarrow \pmb 0} \frac{\exp(\pmb \varphi^\wedge)\exp(\pmb \phi^\wedge)\pmb p-\exp(\pmb \phi^\wedge)\pmb p}{\pmb \varphi}\\ & = \lim_{\pmb \varphi \rightarrow \pmb 0} \frac{(\pmb I+\pmb \varphi^\wedge)\exp(\pmb \phi^\wedge)\pmb p-\exp(\pmb \phi^\wedge)\pmb p}{\pmb \varphi} \\ & = \lim_{\pmb \varphi \rightarrow \pmb 0} \frac{\pmb \varphi^\wedge\pmb R \pmb p}{\pmb \varphi} \\ & = \lim_{\pmb \varphi \rightarrow \pmb 0} \frac{-(\pmb R \pmb p)^\wedge\pmb \varphi}{\pmb \varphi}\\ & = -(\pmb R\pmb p)^\wedge \end{aligned}
∂φφφ∂(RRRppp)=φφφ→000limφφφexp(φφφ∧)exp(ϕϕϕ∧)ppp−exp(ϕϕϕ∧)ppp=φφφ→000limφφφ(III+φφφ∧)exp(ϕϕϕ∧)ppp−exp(ϕϕϕ∧)ppp=φφφ→000limφφφφφφ∧RRRppp=φφφ→000limφφφ−(RRRppp)∧φφφ=−(RRRppp)∧
S E ( 3 ) SE(3) SE(3)李代数求导
假设某空间点
p
\pmb p
ppp经过一次变换
T
\pmb T
TTT，得到
T
p
\pmb T \pmb p
TTTppp。现在，给
T
\pmb T
TTT左乘一个扰动
Δ
T
=
exp
(
δ
ξ
∧
)
\Delta \pmb T = \exp(\delta \pmb \xi^\wedge)
ΔTTT=exp(δξξξ∧)，设扰动项的李代数为
δ
ξ
=
[
δ
p
,
δ
ϕ
]
T
\delta \pmb \xi=[\delta \pmb p,\delta \pmb \phi]^T
δξξξ=[δppp,δϕϕϕ]T，则
∂
(
T
p
)
∂
δ
ξ
=
lim
δ
ξ
→
0
exp
(
δ
ξ
∧
)
exp
(
ξ
∧
)
p
−
exp
(
ξ
∧
)
p
δ
ξ
=
lim
δ
ξ
→
0
(
I
+
δ
ξ
∧
)
exp
(
ξ
∧
)
p
−
exp
(
ξ
∧
)
p
δ
ξ
=
lim
δ
ξ
→
0
δ
ξ
∧
exp
(
ξ
∧
)
p
δ
ξ
=
lim
δ
ξ
→
0
[
δ
ϕ
∧
δ
ρ
0
T
0
]
[
R
p
+
t
1
]
δ
ξ
=
lim
δ
ξ
→
0
[
δ
ϕ
∧
(
R
p
+
t
)
+
δ
p
0
T
]
[
δ
p
,
δ
ϕ
]
T
=
[
E
−
(
R
p
+
t
)
∧
0
T
0
T
]
≜
(
T
P
)
⊙
\begin{aligned} \frac{\partial (\pmb T \pmb p)}{\partial \delta \pmb \xi} &= \lim_{\delta \pmb \xi \rightarrow \pmb 0} \frac{\exp(\delta \pmb \xi^\wedge)\exp(\pmb \xi^\wedge)\pmb p-\exp(\pmb \xi^\wedge)\pmb p}{\delta \pmb \xi}\\ &= \lim_{\delta \pmb \xi \rightarrow \pmb 0} \frac{(\pmb I+\delta \pmb \xi^\wedge)\exp(\pmb \xi^\wedge)\pmb p-\exp(\pmb \xi^\wedge)\pmb p}{\delta \pmb \xi}\\ &= \lim_{\delta \pmb \xi \rightarrow \pmb 0} \frac{\delta \pmb \xi^\wedge\exp(\pmb \xi^\wedge)\pmb p}{\delta \pmb \xi}\\ &= \lim_{\delta \pmb \xi \rightarrow \pmb 0} \frac{\begin{bmatrix}\delta\pmb\phi^\wedge &\delta\pmb\rho\\\pmb 0^T &0\end{bmatrix}\begin{bmatrix}\pmb R\pmb p+\pmb t\\1\end{bmatrix}}{\delta \pmb \xi}\\ &= \lim_{\delta \pmb \xi \rightarrow \pmb 0} \frac{\begin{bmatrix}\delta\pmb\phi^\wedge (\pmb R\pmb p+\pmb t)+\delta \pmb p\\\pmb 0^T\end{bmatrix}}{[\delta \pmb p,\delta \pmb \phi]^T}\\ &=\begin{bmatrix}\pmb E & -(\pmb R\pmb p+\pmb t)^\wedge\\\pmb 0^T &\pmb 0^T\end{bmatrix} \triangleq (\pmb T \pmb P)^{\odot} \end{aligned}
∂δξξξ∂(TTTppp)=δξξξ→000limδξξξexp(δξξξ∧)exp(ξξξ∧)ppp−exp(ξξξ∧)ppp=δξξξ→000limδξξξ(III+δξξξ∧)exp(ξξξ∧)ppp−exp(ξξξ∧)ppp=δξξξ→000limδξξξδξξξ∧exp(ξξξ∧)ppp=δξξξ→000limδξξξ[δϕϕϕ∧000Tδρρρ0][RRRppp+ttt1]=δξξξ→000lim[δppp,δϕϕϕ]T[δϕϕϕ∧(RRRppp+ttt)+δppp000T]=[EEE000T−(RRRppp+ttt)∧000T]≜(TTTPPP)⊙
把最后的结果定义为一个运算符$ ^\odot$，它把一个齐次坐标的空间点变换称一个
4
×
6
4 \times 6
4×6的矩阵。
上式中的矩阵求导规则为下
d
[
a
b
]
d
[
x
y
]
=
(
d
[
a
,
b
]
T
d
[
x
y
]
)
T
=
[
d
a
d
x
d
b
d
x
d
a
d
y
d
b
d
y
]
T
=
[
d
a
d
x
d
a
d
y
d
b
d
x
d
b
d
y
]
\frac{d\begin{bmatrix}a\\b\end{bmatrix}}{d\begin{bmatrix}x\\y\end{bmatrix}} = \Bigg(\frac{d[a,b]^T}{d\begin{bmatrix}x\\y\end{bmatrix}}\Bigg)^T = \begin{bmatrix} \frac{da}{dx}&\frac{db}{dx}\\ \frac{da}{dy}&\frac{db}{dy} \end{bmatrix}^T = \begin{bmatrix} \frac{da}{dx}&\frac{da}{dy}\\ \frac{db}{dx}&\frac{db}{dy} \end{bmatrix}
d[xy]d[ab]=(d[xy]d[a,b]T)T=[dxdadydadxdbdydb]T=[dxdadxdbdydadydb]
代码实现
评估轨迹的误差
在工程中，我们经常需要评估一个算法的估计轨迹与真实轨迹的差异来评价算法的精度。考虑一条估计轨迹 T e s t i , i \pmb T_{esti,i} TTTesti,i和真实轨迹 T g t , i \pmb T_{gt,i} TTTgt,i,其中 i = 1 , ⋯ , N i=1,\cdots,N i=1,⋯,N,那么我们可以定义一些误差来描述他们之间的差别
- 绝对轨迹误差 A b s o l u t e T r a j e c t o r y E r r o r , A T E \rm Absolute \ Trajectory \ Error,ATE Absolute Trajectory Error,ATE
A T E a l l = 1 N ∑ i = 1 N ∣ ∣ log ( T g t , i − 1 T e s t i , i ) ∨ ∣ ∣ 2 2 {\rm ATE_{\rm all}} = \sqrt{\frac1N \sum_{i=1}^N\mid\mid \log(\pmb T_{gt,i}^{-1}\pmb T_{esti,i})^\vee\mid\mid_2^2} ATEall=N1i=1∑N∣∣log(TTTgt,i−1TTTesti,i)∨∣∣22
这实际上是每个位子李代数的均方根误差 R o o t − M e a n − S q u a r e d E r r o r , R M S E \rm Root-Mean-Squared \ Error,RMSE Root−Mean−Squared Error,RMSE。这种误差可以刻画两条轨迹的旋转和平移误差
- 绝对平移误差 A v e r a g e T r a n s l a t i o n a l E r r o r \rm Average \ Translational \ Error Average Translational Error
A T E t r a n s = 1 N ∑ i = 1 N ∣ ∣ t r a n s ( T g t , i − 1 T e s t i , i ) ∣ ∣ 2 2 {\rm ATE_{\rm trans}} = \sqrt{\frac1N \sum_{i=1}^N\mid\mid trans(\pmb T_{gt,i}^{-1}\pmb T_{esti,i})\mid\mid_2^2} ATEtrans=N1i=1∑N∣∣trans(TTTgt,i−1TTTesti,i)∣∣22
该方法只考虑平移误差。 t r a n s trans trans表示括号内部变量的平移部分。因为从整条轨迹上看，旋转出现误差后，随后的轨迹在平移上也会出现误差，所以该指标也适用于实际工作。
-
相对位姿误差 R e l a t i v e P o s e E r r o r , R P E \rm Relative \ Pose \ Error,RPE Relative Pose Error,RPE
考虑 i i i时刻到 i + Δ i i+\Delta i i+Δi时刻的运动
R P E a l l = 1 N − Δ t ∑ i = 1 N − Δ t ∣ ∣ log ( ( T g t , i − 1 T g i , i + Δ t ) − 1 ( T e s t i , i − 1 T e s t i , i + Δ t ) ) ∨ ∣ ∣ 2 2 {\rm RPE_{\rm all}} = \sqrt{\frac1{N-\Delta t} \sum_{i=1}^{N-\Delta t}\mid\mid \log \Big((\pmb T_{gt,i}^{-1}\pmb T_{gi,i+\Delta t})^{-1} (\pmb T_{esti,i}^{-1}\pmb T_{esti,i+\Delta t}) \Big)^\vee\mid\mid_2^2} RPEall=N−Δt1i=1∑N−Δt∣∣log((TTTgt,i−1TTTgi,i+Δt)−1(TTTesti,i−1TTTesti,i+Δt))∨∣∣22
- 相对平移误差 R e l a t i v e T r a n s l a t i o n a l E r r o r , R P E \rm Relative \ Translational \ Error,RPE Relative Translational Error,RPE
R P E t r a n s = 1 N − Δ t ∑ i = 1 N − Δ t ∣ ∣ t r a n s ( ( T g t , i − 1 T g i , i + Δ t ) − 1 ( T e s t i , i − 1 T e s t i , i + Δ t ) ) ∣ ∣ 2 2 {\rm RPE_{\rm trans}} = \sqrt{\frac1{N-\Delta t} \sum_{i=1}^{N-\Delta t}\mid\mid trans\Big((\pmb T_{gt,i}^{-1}\pmb T_{gi,i+\Delta t})^{-1} (\pmb T_{esti,i}^{-1}\pmb T_{esti,i+\Delta t}) \Big) \mid\mid_2^2} RPEtrans=N−Δt1i=1∑N−Δt∣∣trans((TTTgt,i−1TTTgi,i+Δt)−1(TTTesti,i−1TTTesti,i+Δt))∣∣22
相似变换群与李代数
PASS
相机与图像
相机模型
相机将三维世界中的坐标点（单位为米）映射到二维图像平面（单位为像素）的过程能够用一个几何模型进行描述。最简单的模型是针孔模型。针孔模型是常用且有效的模型，它描述了一束光线通过针孔后，在针孔背面投影成像的关系。同时，由于相机镜头上的透镜的存在，使得光线投影到成像平面的过程中会产生畸变。我们使用针孔和畸变两个模型来描述整个投影的过程。
针孔相机模型
设
O
−
x
−
y
−
z
O-x-y-z
O−x−y−z为相机坐标系，我们让
z
z
z轴指向相机前方，
x
x
x轴向右,
y
y
y轴向下。
O
O
O是相机的光心，也是针孔模型中的针孔。现实世界中的空间点
P
=
[
X
,
Y
,
Z
]
T
P=[X,Y,Z]^T
P=[X,Y,Z]T经过小孔
O
O
O投影后，落在物理成像平面
O
′
−
x
′
−
y
′
−
z
′
O'-x'-y'-z'
O′−x′−y′−z′上，成像点为
P
′
=
[
X
′
,
Y
′
,
Z
′
]
P'=[X',Y',Z']
P′=[X′,Y′,Z′]。设物理成像平面到小孔的距离为
f
f
f（焦距）。则
Z
f
=
X
X
′
=
Y
Y
′
\frac Zf = \frac X{X'} = \frac Y{Y'}
fZ=X′X=Y′Y
整理得到
X
′
=
f
X
Z
Y
′
=
f
Y
Z
\begin{aligned} X' = f \frac XZ\\ Y' = f \frac YZ \end{aligned}
X′=fZXY′=fZY
为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面
o
−
u
−
v
o-u-v
o−u−v。我们在像素平面得到了
P
′
P'
P′的像素坐标：
[
u
,
v
]
T
[u,v]^T
[u,v]T。
像素坐标系的原点
o
o
o位于图像的左上角，
u
u
u轴向右与
x
x
x轴平行，
v
v
v轴向下与
y
y
y轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。我们设像素坐标在
u
u
u轴上缩放了
α
\alpha
α倍，在
v
v
v轴上缩放了
β
\beta
β倍。同时，原点平移了
[
c
x
,
c
y
]
T
[c_x,c_y]^T
[cx,cy]T。那么，
P
′
P'
P′像素坐标
[
u
,
v
]
T
[u,v]^T
[u,v]T的关系为
{
u
=
α
X
′
+
c
x
v
=
β
Y
′
+
c
y
→
{
u
=
α
f
X
Z
+
c
x
v
=
β
f
Y
Z
+
c
y
→
{
u
=
f
x
X
Z
+
c
x
v
=
f
y
Y
Z
+
c
y
\begin{cases} u = \alpha X' +c_x\\ v = \beta Y' + c_y \end{cases} \rightarrow \begin{cases} u = \alpha f \frac XZ +c_x\\ v = \beta f \frac YZ + c_y \end{cases} \rightarrow \begin{cases} u = f_x \frac XZ +c_x\\ v = f_y \frac YZ + c_y \end{cases}
{u=αX′+cxv=βY′+cy→{u=αfZX+cxv=βfZY+cy→{u=fxZX+cxv=fyZY+cy
写成矩阵形式，得到
[
u
v
1
]
=
1
Z
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
X
Y
Z
]
≜
1
Z
K
P
\begin{bmatrix}u\\v\\1\end{bmatrix} = \frac 1Z \begin{bmatrix} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{bmatrix} \begin{bmatrix}X\\Y\\Z\end{bmatrix} \triangleq \frac 1Z \pmb K \pmb P
⎣⎡uv1⎦⎤=Z1⎣⎡fx000fy0cxcy1⎦⎤⎣⎡XYZ⎦⎤≜Z1KKKPPP
习惯上将
Z
Z
Z挪到左边
Z
[
u
v
1
]
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
X
Y
Z
]
≜
K
P
(camera_f to image_f)
Z \begin{bmatrix}u\\v\\1\end{bmatrix} = \begin{bmatrix} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{bmatrix} \begin{bmatrix}X\\Y\\Z\end{bmatrix} \triangleq \pmb K \pmb P \tag{camera\_f to image\_f}
Z⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡XYZ⎦⎤≜KKKPPP(camera_f to image_f)
我们把中间的量组成的矩阵称为相机的内参数
C
a
m
e
r
a
I
n
t
r
i
n
s
i
c
s
\rm Camera \ Intrinsics
Camera Intrinsics矩阵
K
\pmb K
KKK。
上式中
P
\pmb P
PPP是
P
\pmb P
PPP在相机坐标系下的坐标，它的世界坐标系
P
W
P_W
PW是根据相机的当前位姿变化到相机坐标系下的结果。相机的位姿由它的旋转矩阵
R
\pmb R
RRR和平移向量
t
\pmb t
ttt来描述，则
Z
P
u
v
=
Z
[
u
v
1
]
=
K
(
R
P
W
+
t
)
=
K
T
P
W
(Word_f to imgae_f)
Z \pmb P_{uv} = Z \begin{bmatrix}u\\v\\1\end{bmatrix} = \pmb K(\pmb R \pmb P_W +\pmb t) = \pmb K \pmb T \pmb P_W \tag{Word\_f to imgae\_f}
ZPPPuv=Z⎣⎡uv1⎦⎤=KKK(RRRPPPW+ttt)=KKKTTTPPPW(Word_f to imgae_f)
相机的位姿
R
,
t
\pmb R,\pmb t
RRR,ttt又称为相机的外参数
C
a
m
e
r
a
E
x
t
r
i
n
s
i
c
s
\rm Camera \ Extrinsics
Camera Extrinsics。
相机的成像模型的另一种描述。我们可以把一个世界坐标点先转换到相机坐标系，再除掉它的最后一维的数值（即该点距离相机成像平面的深度），这相当于把最后一维进行归一化处理，得到点
P
P
P在相机归一化平面（
Z
=
1
Z=1
Z=1）上的投影
(
R
P
W
+
t
)
=
[
X
,
Y
,
Z
]
T
→
[
X
/
Z
,
Y
/
Z
,
1
]
T
(W_f to c_f to z=1_f)
(\pmb R \pmb P_W +\pmb t) = [X,Y,Z]^T \rightarrow [X/Z,Y/Z,1]^T \tag{W\_f to c\_f to z=1\_f}
(RRRPPPW+ttt)=[X,Y,Z]T→[X/Z,Y/Z,1]T(W_f to c_f to z=1_f)
畸变模型
为了获得更好的成像效果，我们在相机的前方加了透镜。透镜的加入会对成像过程中光线的传播产生新的影响：
- 一是透镜自身的形状对光线传播的影响
- 二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使光线穿过透镜投影到成像面时的位置发生变化。
由透镜形状引起的畸变称为径向畸变。在针孔模型中，一条直线投影到像素平面上还是一条直线。可是,在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称。它们主要分为两大类:桶形畸变和枕形畸变。
桶形畸变图像放大率随着与光轴之间的距离增加而减小，而枕形畸变则恰好相反。在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
除了透镜的形状会引径向畸变，由于在相机的组装过程中不能使透镜和成像面严格平行，所以也会引入切向畸变。
使用数学形式对两者进行描述。考虑归一化平面上的任意一点
p
\pmb p
ppp，它的坐标为
[
x
,
y
]
T
[x,y]^T
[x,y]T,也可以写成极坐标的形式
[
r
,
θ
]
T
[r,\theta]^T
[r,θ]T,其中
r
r
r表示点
p
\pmb p
ppp与坐标系原点之间的距离，
θ
\theta
θ表示与水平轴的夹角。径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。通常假设这些畸变成多项式关系,即
x
d
i
s
t
o
r
e
d
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
y
d
i
s
t
o
r
e
d
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
\begin{aligned} &x_{\rm distored} = x(1+k_1r^2+k_2r^4+k_3r^6)\\ &y_{\rm distored} = y(1+k_1r^2+k_2r^4+k_3r^6) \end{aligned}
xdistored=x(1+k1r2+k2r4+k3r6)ydistored=y(1+k1r2+k2r4+k3r6)
其中，
[
x
d
i
s
t
o
r
e
d
,
y
d
i
s
t
o
r
e
d
]
[x_{\rm distored},y_{\rm distored}]
[xdistored,ydistored]是畸变后点的归一化坐标。
对于切向畸变，可以使用另外两个参数
p
1
,
p
2
p_1,p_2
p1,p2进行纠正
x
d
i
s
t
o
r
e
d
=
x
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
y
d
i
s
t
o
r
e
d
=
y
+
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
\begin{aligned} &x_{\rm distored} = x + 2p_1xy + p_2(r^2+2x^2)\\ &y_{\rm distored} = y + p_1(r^2+2y^2) + 2p_2xy \end{aligned}
xdistored=x+2p1xy+p2(r2+2x2)ydistored=y+p1(r2+2y2)+2p2xy
因此，联合上面的公式，对于相机坐标系中一点
P
P
P,我们能通过
5
5
5个畸变参数找到这个点在像素平面上的正确位置：
-
将三维空间点投影到归一化图像平面。设它的归一化坐标为 [ x , y ] T [x,y]^T [x,y]T
-
对归一化平面上的点计算径向畸变和切向畸变
{ x d i s t o r e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y d i s t o r e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 2 x y + p 1 ( r 2 + 2 y 2 ) \begin{cases} x_{\rm distored} = x(1+k_1r^2+k_2r^4+k_3r^6) + 2p_1xy + p_2(r^2+2x^2)\\ y_{\rm distored} = y(1+k_1r^2+k_2r^4+k_3r^6) + 2p_2xy + p_1(r^2+2y^2) \end{cases} {xdistored=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ydistored=y(1+k1r2+k2r4+k3r6)+2p2xy+p1(r2+2y2) -
将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置
{ u = f x x d i s t o r e d + c x v = f y y d i s t o r e d + c y \begin{cases} u = f_x x_{\rm distored} +c_x\\ v = f_y y_{\rm distored} + c_y \end{cases} {u=fxxdistored+cxv=fyydistored+cy
最后总结单目相机的成像过程如下 -
世界坐标系下有一个固定的点 P P P,相机的世界坐标系为 P W \pmb P_W PPPW
-
由于相机在运动，它的运动由 R , t \pmb R,\pmb t RRR,ttt或变换矩阵 T ∈ S E ( 3 ) \pmb T \in SE(3) TTT∈SE(3)描述。 P P P的相机坐标为 P c ^ = R P W + t \hat{\pmb P_c} = \pmb R \pmb P_W + \pmb t PPPc^=RRRPPPW+ttt
-
这时的 P c ^ \hat{\pmb P_c} PPPc^的分量为 X , Y , Z X,Y,Z X,Y,Z,把它们投影到归一化平面 Z = 1 Z=1 Z=1上，得到 P P P的归一化坐标： P c = [ X / Z , Y / Z , 1 ] T \pmb P_c = [X/Z,Y/Z,1]^T PPPc=[X/Z,Y/Z,1]T
-
有畸变时，根据畸变参数计算 P c \pmb P_c PPPc发生畸变后的坐标
-
P P P的归一化坐标经过内参后，得到它的像素坐标： P u v = K P c \pmb P_{uv} = \pmb K \pmb P_{c} PPPuv=KKKPPPc
双目相机模型
双目相机模型原理是通过采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度。
O L O_L OL与 O R O_R OR之间的距离称为双目相机的基线（记作 b b b），是双目相机的重要参数。
根据三角形相似原理，可推导出下面公式
z
=
f
b
u
L
−
u
R
≜
f
b
d
z = \frac {fb}{u_L-u_R} \triangleq \frac {fb}d
z=uL−uRfb≜dfb
其中
d
d
d定义为左右图的横坐标之差，称为视差。根据视差，我们可以估计一个像素与相机之间的距离。视差与距离成反比，视差越大，距离越近。同时，由于视差最小单位是一个像素，因此双目的深度存在一个理论最大值由
f
b
fb
fb决定。
RGB-D相机模型
R G B − D \rm RGB-D RGB−D相机能够主动测量每个像素的深度。
- 通过**红外结构光 S t r u c t u r e d L i g h t \rm Structured \ Light Structured Light**原理测量像素距离。相机根据返回的结构光图案，计算物体与自身之间的距离。
- 通过**飞行时间 T i m e − o f − F l i g h t , T o F \rm Time-of-Flight,ToF Time−of−Flight,ToF**原理测量像素距离。相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体与自身的距离。
非线性优化
$ \rm SLAM$问题的数学描述
将连续时间采样为
1
,
⋯
,
K
1,\cdots,K
1,⋯,K个离散时间。用
x
\pmb x
xxx表示机器人自身的位置。机器人各时刻的位置为
x
1
,
⋯
,
x
K
\pmb x_1,\cdots,\pmb x_K
xxx1,⋯,xxxK，它们构成了机器人的轨迹。地图方面，我们假设地图是由许多个路标组成的，而每个时刻，传感器会测量到一部分路标点，得到他们的观测数据。设路径点一共由
N
N
N个，用
y
1
,
⋯
,
y
N
\pmb y_1,\cdots,\pmb y_N
yyy1,⋯,yyyN表示它们。
u
k
\pmb u_k
uuuk是运动传感器的读入或者输入，
w
k
\pmb w_k
wwwk为该过程加入的噪声。
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
(运动方程)
\pmb x_k = f(\pmb x_{k-1},\pmb u_k)+\pmb w_k \tag{运动方程}
xxxk=f(xxxk−1,uuuk)+wwwk(运动方程)
z
k
,
j
\pmb z_{k,j}
zzzk,j为机器人在
x
k
\pmb x_k
xxxk位置上看到路径点
y
j
\pmb y_j
yyyj的观测数据。
v
k
,
j
\pmb v_{k,j}
vvvk,j是观测的噪声。
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
(观测方程)
\pmb z_{k,j} = h(\pmb y_j,\pmb x_k)+\pmb v_{k,j} \tag{观测方程}
zzzk,j=h(yyyj,xxxk)+vvvk,j(观测方程)
状态估计问题
批量状态估计与最大后验估计
问题引出
在运动和观测方程中，我们假设两个噪声
w
k
,
v
k
,
j
\pmb w_k,\pmb v_{k,j}
wwwk,vvvk,j满足如下的高斯分布
w
k
∼
N
(
0
,
R
k
)
v
k
,
j
∼
N
(
0
,
Q
k
,
j
)
\begin{aligned} &\pmb w_k \sim \mathcal N(0,\pmb R_k)\\ &\pmb v_{k,j} \sim \mathcal N(0,\pmb Q_{k,j}) \end{aligned}
wwwk∼N(0,RRRk)vvvk,j∼N(0,QQQk,j)
R
k
,
Q
k
,
j
\pmb R_k,\pmb Q_{k,j}
RRRk,QQQk,j为协方差矩阵。
在这些噪声的影响下，我们希望通过带噪声的数据
z
\pmb z
zzz和
u
\pmb u
uuu推断位姿
x
\pmb x
xxx和地图
y
\pmb y
yyy（以及它们的概率分布）,这构成了一个状态估计问题。
解决方法
- 增量/渐进法 i n c r e m e n t a l \rm incremental incremental(滤波器)：由于在$ \rm SLAM 过 程 中 ， 数 据 是 随 时 间 逐 渐 到 来 的 ， 根 据 已 经 持 有 的 当 前 时 刻 的 估 计 状 态 ， 在 新 的 数 据 到 来 时 进 行 更 新 。 仅 关 心 当 前 时 刻 的 状 态 估 计 过程中，数据是随时间逐渐到来的，根据已经持有的当前时刻的估计状态，在新的数据到来时进行更新。仅关心当前时刻的状态估计 过程中，数据是随时间逐渐到来的，根据已经持有的当前时刻的估计状态，在新的数据到来时进行更新。仅关心当前时刻的状态估计\pmb x_k$，不考虑之前的状态
- 批法量 b a t c h \rm batch batch：把数据攒起来一并处理。可以在更大的范围达到最优化，被认为优于传统的滤波器，成了当前视觉$ \rm SLAM$的主流方法。
- S f M , S t r u c t u r e f r o m M o t i o n \rm SfM, Structure\ from \ Motion SfM,Structure from Motion：批量法的极端方式。让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理。但这种处理方式有时候会失去实时性。
- 滑动窗口估计法：一种折衷方法，固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化。
批量状态估计
机器人位姿和路标点坐标定义如下：
x
=
{
x
1
,
⋯
,
x
N
}
y
=
{
y
1
,
⋯
,
y
M
}
\begin{aligned} &\pmb x= \{\pmb x_1,\cdots,\pmb x_N\} \\ &\pmb y = \{\pmb y_1,\cdots, \pmb y_M \} \end{aligned}
xxx={xxx1,⋯,xxxN}yyy={yyy1,⋯,yyyM}
用不带下标的
u
\pmb u
uuu表示所有时刻的输入，
z
\pmb z
zzz表示所有时刻的观测数据。对机器人的状态估计，从概率学的观点来看，就是已知输入数据
u
\pmb u
uuu和观测数据
z
\pmb z
zzz的条件下，求状态
x
,
y
\pmb x,\pmb y
xxx,yyy的条件概率分布
P
(
x
,
y
∣
z
,
u
)
P(\pmb x,\pmb y \mid \pmb z,\pmb u)
P(xxx,yyy∣zzz,uuu)
特别的，如果不知道
u
\pmb u
uuu，则为
S
f
M
\rm SfM
SfM问题，即从许多图像中重建三维空间结构。
为了估计状态变量的条件分布，由贝叶斯公式，有
P
(
x
,
y
∣
z
,
u
)
=
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
P
(
z
,
u
)
=
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
P(\pmb x,\pmb y \mid \pmb z,\pmb u) = \frac{P(\pmb z,\pmb u\mid \pmb x,\pmb y)P(\pmb x,\pmb y)}{P(\pmb z,\pmb u)} = P(\pmb z,\pmb u \mid \pmb x,\pmb y) P(\pmb x,\pmb y)
P(xxx,yyy∣zzz,uuu)=P(zzz,uuu)P(zzz,uuu∣xxx,yyy)P(xxx,yyy)=P(zzz,uuu∣xxx,yyy)P(xxx,yyy)
P
(
x
,
y
∣
z
,
u
)
P(\pmb x,\pmb y \mid \pmb z,\pmb u)
P(xxx,yyy∣zzz,uuu)是一个后验概率，
P
(
z
,
u
∣
x
,
y
)
P(\pmb z,\pmb u \mid \pmb x,\pmb y)
P(zzz,uuu∣xxx,yyy)被称为似然
L
i
k
e
h
o
i
d
\rm Likehoid
Likehoid，而
P
(
x
,
y
)
P(\pmb x,\pmb y)
P(xxx,yyy)是一个先验概率
P
r
i
o
r
\rm Prior
Prior。这样一个后验概率变成了一个似然和一个先验概率的乘积。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大是可行的
(
x
,
y
)
M
A
P
∗
=
arg
max
P
(
x
,
y
∣
z
,
u
)
=
arg
max
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
(\pmb x,\pmb y)^*_{\rm{MAP}} = \arg \max P(\pmb x,\pmb y \mid \pmb z,\pmb u) = \arg \max P(\pmb z,\pmb u \mid \pmb x ,\pmb y) P(\pmb x,\pmb y)
(xxx,yyy)MAP∗=argmaxP(xxx,yyy∣zzz,uuu)=argmaxP(zzz,uuu∣xxx,yyy)P(xxx,yyy)
如果我们不知道机器人位姿或路标大概在什么地方，也就没有了先验，则可以求解最大似然估计
M
a
x
i
m
i
z
e
L
i
k
e
h
o
o
d
E
s
t
i
m
a
t
i
o
n
,
M
L
E
\rm Maximize \ Likehood \ Estimation,MLE
Maximize Likehood Estimation,MLE，即在什么样的状态下，最有可能产生观测到的数据。
(
x
,
y
)
M
L
E
∗
=
arg
max
P
(
z
,
u
∣
x
,
y
)
( \pmb x,\pmb y)^*_{\rm{MLE}} = \arg \max P(\pmb z,\pmb u \mid \pmb x,\pmb y)
(xxx,yyy)MLE∗=argmaxP(zzz,uuu∣xxx,yyy)
最小二乘的引出
对某一次观测:
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
\pmb z_{k,j} = h(\pmb y_j,\pmb x_k) + \pmb v_{k,j}
zzzk,j=h(yyyj,xxxk)+vvvk,j
由于
v
k
∼
N
(
0
,
Q
k
,
j
)
\pmb v_k \sim \mathcal N(0,\pmb Q_{k,j})
vvvk∼N(0,QQQk,j)，所以观测数据的条件概率为:
P
(
z
j
,
k
∣
x
k
,
y
j
)
=
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
P(\pmb z_{j,k} \mid \pmb x_k,\pmb y_j) = N(h(\pmb y_j,\pmb x_k),\pmb Q_{k,j})
P(zzzj,k∣xxxk,yyyj)=N(h(yyyj,xxxk),QQQk,j)
考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。
考虑高斯分布
x
∼
N
(
μ
,
∑
)
x \sim \mathcal N(\mu,\sum)
x∼N(μ,∑),概率密度为
P
(
x
)
=
1
(
2
π
)
N
d
e
t
(
∑
)
exp
(
−
1
2
(
x
−
μ
)
T
∑
−
1
(
x
−
u
)
)
P(x) = \frac {1}{\sqrt{(2\pi)^N det(\sum)}} \exp\bigg(-\frac12(x-\mu)^T∑^{-1}(x-u)\bigg)
P(x)=(2π)Ndet(∑)1exp(−21(x−μ)T∑−1(x−u))
两边取对数
−
ln
(
P
(
x
)
)
=
1
2
ln
(
(
2
π
)
N
d
e
t
(
∑
)
)
+
1
2
(
x
−
μ
)
T
∑
−
1
(
x
−
μ
)
-\ln(P(x)) =\frac 12 \ln\bigg((2\pi)^N det(\sum) \bigg) + \frac 12 (x-\mu)^T ∑^{-1}(x-\mu)
−ln(P(x))=21ln((2π)Ndet(∑))+21(x−μ)T∑−1(x−μ)
对数函数是单调递增的，对原函数求最大值即为对负对数求最小化。由于第一项与
x
x
x无关，可以略去。只需要最小化右侧的二次型项。代入$ \rm SLAM$观测模型，即相当于求
(
x
k
,
y
j
)
∗
=
arg
max
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
=
arg
min
(
(
z
k
,
j
−
h
(
x
k
,
y
j
)
)
T
Q
k
,
j
−
1
(
z
k
,
j
−
h
(
x
k
,
y
j
)
)
)
\begin{aligned} (\pmb x_k,\pmb y_j)^* &= \arg \max \mathcal N(h(\pmb y_j,\pmb x_k),\pmb Q_{k,j})\\ &= \arg \min \bigg((\pmb z_{k,j}-h(\pmb x_k,\pmb y_j))^T \pmb Q_{k,j}^{-1}(\pmb z_{k,j}-h(\pmb x_k,\pmb y_j))\bigg) \end{aligned}
(xxxk,yyyj)∗=argmaxN(h(yyyj,xxxk),QQQk,j)=argmin((zzzk,j−h(xxxk,yyyj))TQQQk,j−1(zzzk,j−h(xxxk,yyyj)))
该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离
M
a
h
a
l
a
n
o
b
i
s
d
i
s
t
a
n
c
e
\rm Mahalanobis \ distance
Mahalanobis distance,又称马氏距离。它也可以看成由
Q
k
,
j
−
1
\pmb Q_{k,j}^{-1}
QQQk,j−1加权之后的欧氏距离，这里
Q
k
,
j
−
1
\pmb Q_{k,j}^{-1}
QQQk,j−1也叫信息矩阵，即高斯分布协方差矩阵之逆。
现在考虑批量时刻的数据。通常假设各个时刻的输入和观测时相互独立的。因此可以对联合分布进行因式分解
P
(
z
,
u
∣
x
,
y
)
=
∏
k
P
(
u
k
∣
x
k
−
1
,
x
k
)
∏
k
,
j
P
(
z
k
,
j
∣
x
k
,
y
j
)
P(\pmb z,\pmb u\mid \pmb x,\pmb y) = \prod_k P(\pmb u_k \mid \pmb x_{k-1},\pmb x_k) \prod_{k,j} P(\pmb z_{k,j} \mid \pmb x_k,\pmb y_j)
P(zzz,uuu∣xxx,yyy)=k∏P(uuuk∣xxxk−1,xxxk)k,j∏P(zzzk,j∣xxxk,yyyj)
这说明可以独立的处理各个时刻的运动和观测。定义输入和观测数据与模型的误差为
e
u
,
k
=
x
k
−
f
(
x
k
−
1
,
u
k
)
e
z
,
j
,
k
=
z
k
,
j
−
h
(
x
k
,
y
j
)
\begin{aligned} &\pmb e_{\pmb u,k} = \pmb x_k - f(\pmb x_{k-1},\pmb u_k)\\ &\pmb e_{\pmb z,j,k} = \pmb z_{k,j} - h(\pmb x_k,\pmb y_j) \end{aligned}
eeeuuu,k=xxxk−f(xxxk−1,uuuk)eeezzz,j,k=zzzk,j−h(xxxk,yyyj)
最小化所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计。负对数允许把成绩变成求和
min
J
(
x
,
y
)
=
∑
k
e
u
,
k
T
R
k
−
1
e
u
,
k
+
∑
k
∑
j
e
z
,
k
,
j
T
Q
k
,
j
−
1
e
z
,
k
,
j
\min J(\pmb x,\pmb y) = \sum_{k} \pmb e_{\pmb u,k}^T \pmb R_{k}^{-1}\pmb e_{\pmb u,k} + \sum_{k}\sum_{j} \pmb e_{\pmb z,k,j}^T \pmb Q^{-1}_{k,j}\pmb e_{\pmb z,k,j}
minJ(xxx,yyy)=k∑eeeuuu,kTRRRk−1eeeuuu,k+k∑j∑eeezzz,k,jTQQQk,j−1eeezzz,k,j
至此得到了一个最小二乘问题
L
e
a
s
t
S
q
u
a
r
e
P
r
o
b
l
e
m
\rm Least\ Square\ Problem
Least Square Problem，它的解就是状态的最大似然估计。由于噪声的存在，当我们把估计的轨迹与地图带入$ \rm SLAM$的运动、观测方程中时，并不会完美的成立。因此需要对状态的估计进行微调，使得整体的误差下降一点，最终达到一个极小值。这是一个典型的非线性优化过程。
非线性最小二乘
考虑一个最小二乘问题
min
x
F
(
x
)
=
1
2
∣
∣
f
(
x
)
∣
∣
2
2
\min_{\pmb x} F(\pmb x) = \frac 12 \mid\mid f(\pmb x) \mid \mid _2^2
xxxminF(xxx)=21∣∣f(xxx)∣∣22
其中，
x
∈
R
n
\pmb x \in \mathbb R^n
xxx∈Rn,
f
f
f是任意标量非线性函数
f
(
x
)
:
R
n
→
R
f(\pmb x):\mathbb R^n \rightarrow \mathbb R
f(xxx):Rn→R。
如果 f f f为简单的线性函数，可通过求导得出最优解。但是有时候导函数可能形势复杂，使得方程不易求解。对于不方便直接求解的最小二乘问题，我们可以用迭代得到的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤如下：
- 给定某个初始值 x 0 \pmb x_0 xxx0
- 对于第 k k k次迭代，寻找一个增量 Δ x k \Delta \pmb x_k Δxxxk，使得 ∣ ∣ f ( x k + Δ x k ) ∣ ∣ 2 2 \mid \mid f(x_k+ \Delta x_k)\mid\mid_2^2 ∣∣f(xk+Δxk)∣∣22达到极小值
- 若 Δ x k \Delta \pmb x_k Δxxxk足够小，则停止
- 否则，令 x k + 1 = x k + Δ x k \pmb x_{k+1} = \pmb x_k + \Delta \pmb x_k xxxk+1=xxxk+Δxxxk,返回第 2 2 2步
这让求解导函数为零的问题变成了一个不断寻找下降增量 Δ x k \Delta \pmb x_k Δxxxk的问题。如何找到每次迭代点的增量，这是一个局部问题，我们只需要关心 f f f在迭代值处的局部性质而非全局性质。
一阶和二阶梯度法
考虑第
k
k
k次迭代，假设我们在
x
k
\pmb x_k
xxxk处，想要寻找增量
Δ
x
k
\Delta \pmb x_k
Δxxxk,那么最直观的方式是将目标函数在
x
k
\pmb x_k
xxxk处进行泰勒展开
F
(
x
k
+
Δ
x
k
)
≈
F
(
x
k
)
+
J
(
x
k
)
T
Δ
x
k
+
1
2
Δ
x
k
T
H
(
x
k
)
Δ
x
k
F(\pmb x_k + \Delta \pmb x_k) \approx F(\pmb x_k) + \pmb J(\pmb x_k)^T \Delta \pmb x_k + \frac 12 \Delta \pmb x_k^T\pmb H(\pmb x_k) \Delta \pmb x_k
F(xxxk+Δxxxk)≈F(xxxk)+JJJ(xxxk)TΔxxxk+21ΔxxxkTHHH(xxxk)Δxxxk
其中
J
(
x
k
)
\pmb J(\pmb x_k)
JJJ(xxxk)是
F
(
x
)
\pmb F(\pmb x)
FFF(xxx)关于
x
\pmb x
xxx的一阶导数[也叫梯度、雅可比Jacobian矩阵]，
H
\pmb H
HHH则是二阶导数[海塞矩阵Hessian]，它们都在
x
k
\pmb x_k
xxxk处取值。我们可以选择保留泰勒展开的一阶或二阶项，那么对应的求解方法则称为一阶梯度或二阶梯度法。
- 一阶梯度法
Δ x ∗ = − J ( x k ) \Delta \pmb x^* = - \pmb J(\pmb x_k) Δxxx∗=−JJJ(xxxk)
取增量为反向的梯度，即可保证函数的下降，通常我们还要再指定一个步长。
- 二阶梯度法
Δ x ∗ = arg min ( F ( x ) + J ( x ) T Δ x + 1 2 Δ x T H Δ x ) \Delta \pmb x^* = \arg \min \bigg( F(\pmb x) + \pmb J(\pmb x)^T \Delta \pmb x + \frac 12 \Delta \pmb x^T \pmb H \pmb \Delta x \bigg) Δxxx∗=argmin(F(xxx)+JJJ(xxx)TΔxxx+21ΔxxxTHHHΔΔΔx)
求等式右侧关于
Δ
x
\Delta \pmb x
Δxxx的导数并令其为零，则
J
+
H
Δ
x
=
0
⟹
H
Δ
x
=
−
J
(牛顿法)
\pmb J + \pmb H \Delta \pmb x = 0 \Longrightarrow \pmb H \Delta \pmb x = -\pmb J \tag{牛顿法}
JJJ+HHHΔxxx=0⟹HHHΔxxx=−JJJ(牛顿法)
高斯牛顿法
高斯牛顿法是最优化算法中最简单的方法之一。它的思想是将
f
(
x
)
f(\pmb x)
f(xxx)进行一阶的泰勒展开，不同于一阶梯度法对
F
(
x
)
F(\pmb x)
F(xxx)进行分析。
f
(
x
+
Δ
x
)
≈
f
(
x
)
+
J
(
x
)
T
Δ
x
f(\pmb x+ \Delta \pmb x) \approx f(\pmb x) + \pmb J(\pmb x)^T \Delta \pmb x
f(xxx+Δxxx)≈f(xxx)+JJJ(xxx)TΔxxx
Δ
x
∗
=
arg
min
1
2
∣
∣
f
(
x
)
+
J
(
x
)
T
Δ
x
∣
∣
2
\Delta \pmb x^* = \arg \min \frac12 \mid\mid f(\pmb x) + \pmb J(\pmb x)^T \Delta \pmb x \mid\mid^2
Δxxx∗=argmin21∣∣f(xxx)+JJJ(xxx)TΔxxx∣∣2
将上式展开后对
Δ
x
\Delta \pmb x
Δxxx求导，并令其为零
J
(
x
)
f
(
x
)
+
J
T
(
x
)
Δ
x
=
0
\pmb J(\pmb x) f(\pmb x)+ J^T(\pmb x) \Delta \pmb x =0
JJJ(xxx)f(xxx)+JT(xxx)Δxxx=0
J
(
x
)
J
T
(
x
)
Δ
x
=
−
J
(
x
)
f
(
x
)
(增量方程)
\pmb J(\pmb x) \pmb J^T(\pmb x) \Delta \pmb x = -\pmb J(\pmb x) f(\pmb x) \tag{增量方程}
JJJ(xxx)JJJT(xxx)Δxxx=−JJJ(xxx)f(xxx)(增量方程)
这个方程是关于变量
Δ
x
\Delta \pmb x
Δxxx的线性方程组，我们称其为增量方程，也称为高斯牛顿方程
G
a
u
s
s
−
N
e
w
t
o
n
e
q
u
a
t
i
o
n
\rm Gauss-Newton \ equation
Gauss−Newton equation或正规方程
N
o
r
m
a
l
e
q
u
a
t
i
o
n
\rm Normal \ equation
Normal equation。令
H
(
x
)
=
J
(
x
)
J
T
(
x
)
,
g
(
x
)
=
−
J
(
x
)
f
(
x
)
\pmb H(\pmb x) = J(\pmb x)J^T(\pmb x),\pmb g(\pmb x) = -\pmb J(\pmb x)f(\pmb x)
HHH(xxx)=J(xxx)JT(xxx),ggg(xxx)=−JJJ(xxx)f(xxx)，则
H
Δ
x
=
g
\pmb H \Delta \pmb x = \pmb g
HHHΔxxx=ggg
对比牛顿法，高斯牛顿法用
J
J
T
\pmb J \pmb J^T
JJJJJJT作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算
H
\pmb H
HHH的过程。求解增量方程是整个优化问题的核心所在。
总结高斯牛顿法的算法步骤为
- 给定初始值 x 0 \pmb x_0 xxx0
- 对于第 k k k次迭代，求出当前的雅可比矩阵 J ( x k ) \pmb J(\pmb x_k) JJJ(xxxk)和误差 f ( x k ) f(\pmb x_k) f(xxxk)
- 求解增量方程 H Δ x k = g \pmb H \Delta \pmb x_k = \pmb g HHHΔxxxk=ggg
- 若
Δ
x
k
\Delta \pmb x_k
Δxxxk足够小，则停止。否则，令
x
k
+
1
=
x
k
+
Δ
x
k
\pmb x_{k+1}=\pmb x_k+\Delta \pmb x_k
xxxk+1=xxxk+Δxxxk,否则返回第
2步
有时候 H \pmb H HHH是病态的，这时用高斯牛顿法可能会导致局部近似不够准确。
列文伯格-马夸尔特方法
高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以给
Δ
x
\Delta \pmb x
Δxxx添加一个范围，称为信赖区域
T
r
u
s
t
R
e
g
i
o
n
\rm Trust \ Region
Trust Region。这个范围定有了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法
T
r
u
s
t
R
e
g
i
o
n
M
e
t
h
o
d
\rm Trust \ Region \ Method
Trust Region Method。在信赖区域里，我们认为近似是有效的。
定义指标
ρ
\rho
ρ来刻画近似的好坏程度
ρ
=
f
(
x
+
Δ
x
)
−
f
(
x
)
J
(
x
)
T
Δ
x
\rho = \frac{f(\pmb x+ \Delta \pmb x) - f(\pmb x)}{\pmb J(\pmb x)^T\Delta \pmb x}
ρ=JJJ(xxx)TΔxxxf(xxx+Δxxx)−f(xxx)
ρ
\rho
ρ的分子是实际函数下降的值，分母是近似模型下降的值。如果
ρ
\rho
ρ较小，说明时实际减小的值远小于近似见效的值，近似较差，则应该缩小近似范围，否则放大。
一个改良版的非线性优化步骤如下
- 给定初始值 x 0 \pmb x_0 xxx0，以及初始优化半径 μ \mu μ
- 对于第
k
k
k次迭代，在高斯牛顿法的基础上加上信赖区域，求解
min Δ x k 1 2 ∣ ∣ f ( x k ) + J ( x k ) T Δ x k ∣ ∣ 2 , s . t . ∣ ∣ D Δ x k ∣ ∣ 2 ≤ μ \min_{\Delta \pmb x_k} \frac 12 \mid\mid f(\pmb x_k) + \pmb J(\pmb x_k)^T\Delta \pmb x_k \mid\mid^2,\rm{s.t.} \ \ \mid\mid\pmb D\Delta \pmb x_k\mid\mid^2 \leq \mu Δxxxkmin21∣∣f(xxxk)+JJJ(xxxk)TΔxxxk∣∣2,s.t. ∣∣DDDΔxxxk∣∣2≤μ
其中, μ \mu μ是信赖区域的半径， D \pmb D DDD为系数矩阵。 - 计算 ρ \rho ρ
- 若 ρ > 3 4 \rho > \frac34 ρ>43,则设置$\mu=2\mu $
- 若 ρ < 1 4 \rho < \frac 14 ρ<41,则设置 μ = 0.5 μ \mu = 0.5\mu μ=0.5μ
- 如果 ρ \rho ρ大于某阈值，则认为近似可行。令 x k + 1 = x k + Δ x k \pmb x_{k+1} = \pmb x_k +\Delta \pmb x_k xxxk+1=xxxk+Δxxxk
- 判断算法是否收敛。如不收敛则返回第
2步，否则结束。
对于第2步，这是一个带不等式约束的优化问题，用拉格朗日乘子构建拉格朗日函数
L ( Δ x k , λ ) = 1 2 ∣ ∣ f ( x k ) + J ( x k ) T Δ x k ∣ ∣ 2 + λ 2 ( ∣ ∣ D Δ x k ∣ ∣ 2 − μ ) \mathcal L(\Delta \pmb x_k,\lambda) = \frac 12 \mid\mid f(\pmb x_k)+\pmb J(\pmb x_k)^T\Delta \pmb x_k \mid\mid^2 + \frac{\lambda}{2}(\mid\mid \pmb D\Delta \pmb x_k \mid\mid^2 -\mu) L(Δxxxk,λ)=21∣∣f(xxxk)+JJJ(xxxk)TΔxxxk∣∣2+2λ(∣∣DDDΔxxxk∣∣2−μ)
对 Δ x \Delta \pmb x Δxxx求导并令其为零
( H + λ D T D ) Δ x k = g (\pmb H + \lambda \pmb D^T\pmb D)\Delta \pmb x_k = \pmb g (HHH+λDDDTDDD)Δxxxk=ggg
相对于高斯牛顿法，增量方程多了一个 λ D T D \lambda \pmb D^T\pmb D λDDDTDDD。如果考虑它的简化形式 D = E D=E D=E,则
( H + λ E ) Δ x k = g (\pmb H + \lambda \pmb E) \Delta \pmb x_k = \pmb g (HHH+λEEE)Δxxxk=ggg
当 λ \lambda λ较小时， H \pmb H HHH占主要地位，近似于高斯牛顿法。当 λ \lambda λ较大， λ E \lambda E λE占主要地位，近似于一阶梯度下降法。
代码实现
研究问题
下面用不同方法进行曲线拟合，所研究的问题是一致的
考虑一条满足下列方程的曲线
y
=
exp
(
a
x
2
+
b
x
+
c
)
+
w
y = \exp(ax^2+bx+c)+w
y=exp(ax2+bx+c)+w
a
,
b
,
c
a,b,c
a,b,c是曲线的参数，
w
w
w为高斯噪声，满足
w
∼
(
0
,
σ
2
)
w \sim (0,\sigma^2)
w∼(0,σ2)。
可以求解下面的最小二乘问题以估计曲线参数
min
a
,
b
,
c
1
2
∑
i
=
1
N
∣
∣
y
i
−
exp
(
a
x
i
2
+
b
x
i
+
c
)
∣
∣
2
\min_{a,b,c} \frac 12 \sum_{i=1}^N \mid\mid y_i - \exp(ax_i^2+bx_i+c)\mid\mid^2
a,b,cmin21i=1∑N∣∣yi−exp(axi2+bxi+c)∣∣2
定义误差为
e
i
=
y
i
−
exp
(
a
x
i
2
+
b
x
i
+
c
)
e_i = y_i - \exp(ax_i^2+bx_i+c)
ei=yi−exp(axi2+bxi+c)
可以求出每个误差项对于状态变量的导数
∂
e
i
∂
a
=
−
x
i
2
exp
(
a
x
i
2
+
b
x
i
+
c
)
∂
e
i
∂
b
=
−
x
i
exp
(
a
x
i
2
+
b
x
i
+
c
)
∂
e
i
∂
c
=
−
exp
(
a
x
i
2
+
b
x
i
+
c
)
\begin{aligned} & \frac{\partial e_i}{\partial a} = -x_i^2 \exp(ax_i^2+bx_i+c)\\ & \frac{\partial e_i}{\partial b} = -x_i \exp(ax_i^2+bx_i+c)\\ & \frac{\partial e_i}{\partial c} = -\exp(ax_i^2+bx_i+c)\\ \end{aligned}
∂a∂ei=−xi2exp(axi2+bxi+c)∂b∂ei=−xiexp(axi2+bxi+c)∂c∂ei=−exp(axi2+bxi+c)
于是
J
i
=
[
∂
e
i
∂
a
,
∂
e
i
∂
b
,
∂
e
i
∂
c
]
T
\pmb J_i = [\frac{\partial e_i}{\partial a},\frac{\partial e_i}{\partial b},\frac{\partial e_i}{\partial c}]^T
JJJi=[∂a∂ei,∂b∂ei,∂c∂ei]T,高斯牛顿法的增量为
(
∑
i
=
1
100
J
i
J
i
T
)
Δ
x
k
=
∑
i
=
1
100
−
J
i
e
i
\bigg(\sum_{i=1}^{100}\pmb J_i \pmb J_i^T\bigg) \Delta \pmb x_k = \sum_{i=1}^{100} - \pmb J_i e_i
(i=1∑100JJJiJJJiT)Δxxxk=i=1∑100−JJJiei
注意这里的最小二乘法是求和形式的。事实上，这是最小二乘法的经典形式。当最优化目标函数是一个平方求和时，其对某个自变量的求导刚好是目标函数是单个平方和时的求和。所以，最小二乘法的目标函数是平方求和时，其导数矩阵直接把上面推导得矩阵求和即可。
手写高斯牛顿法
使用Ceres进行曲线拟合
C
e
r
e
s
\rm Ceres
Ceres是一个广泛使用的最小二乘问题求解库。
C
e
r
e
s
\rm Ceres
Ceres求解的最小二乘问题最一般的形式如下
min
x
1
2
∑
i
ρ
i
(
∣
∣
f
i
(
x
i
1
,
⋯
,
x
i
n
)
∣
∣
2
)
s
.
t
.
l
j
≤
x
j
≤
u
j
\begin{aligned} &\min_x \frac 12 \sum_i \rho_i \bigg(\mid\mid f_i(x_{i1},\cdots,x_{in})\mid\mid^2 \bigg)\\ &{\rm s.t.} \ \ l_j \leq x_j \leq u_j \end{aligned}
xmin21i∑ρi(∣∣fi(xi1,⋯,xin)∣∣2)s.t. lj≤xj≤uj
其中，
x
1
,
⋯
,
x
n
x_1,\cdots,x_n
x1,⋯,xn为优化变量，又称参数块
P
a
r
a
m
e
t
e
r
b
l
o
c
k
s
\rm Parameter \ blocks
Parameter blocks,
f
i
f_i
fi称为代价函数
C
o
s
t
f
u
n
c
t
i
o
n
\rm Cost \ function
Cost function,也称为残差块
R
e
s
i
d
u
a
l
b
l
o
c
k
s
\rm Residual \ blocks
Residual blocks,在$\rm $
S
L
A
M
\rm SLAM
SLAM中也可以理解为误差项。
l
j
l_j
lj和
u
j
u_j
uj为第
j
j
j各优化变量的上限和下限。在简单情况下，可取
l
j
=
−
∞
,
u
j
=
∞
l_j=-\infty,u_j = \infty
lj=−∞,uj=∞。此时，目标函数由许多平方项经过一个核函数
ρ
(
⋅
)
\rho(\cdot)
ρ(⋅)之后求和组成。
使用步骤如下
- 定义每个参数块。参数块通常为平凡的向量，但是在
S
L
A
M
\rm SLAM
SLAM里也可以定义成四元数、李代数这种特殊的结构。如果是向量，那么我们需要为每个参数块分配一个
double数组来存储变量的值。 - 定义残差块的计算方式。残差块通常关联若干个参数块，对它们进行一些自定义的计算，然后返回残差值。 C e r e s \rm Ceres Ceres对它们求平方和之后，作为目标函数的值。
- 残差块往往也需要定义雅可比的计算方式。在 C e r e s \rm Ceres Ceres中，你可以使用它提供的"自动求导"功能，也可以手动指定雅可比的计算过程。如果要使用自动求导，那么残差块需要按照特定的写法书写：残差的计算过程应该是一个带模板的括号运算符。这一点我们通过例子来说明。
- 把所有的参数块和残差块加人
C
e
r
e
s
\rm Ceres
Ceres定义的
Problem对象中，调用Solve函数求解即可。求解之前，我们可以传入一些配置信息，例如迭代次数、终止条件等，也可以使用默认的配置。
使用g2o进行曲线拟合
g 2 o \rm g2o g2o是一个基于图优化的库。为了了解某个优化变量 x j x_j xj存在于多少个误差项中，引入了图优化。图优化中图的顶点表示优化变量，边表示误差项。
将图像拟合问题抽象为图优化，只需要记住结点为优化变量，边为误差项即可。一般需要以下步骤
- 定义顶点和边的类型
- 构建图
- 选择优化算法
- 调用 g 2 o \rm g2o g2o进行优化，返回结果
在我们选择的研究问题中，只有一个结点为
(
a
,
b
,
c
)
(a,b,c)
(a,b,c),有100个从该节点出发，指向该节点的数据。
首先从 g 2 o \rm g2o g2o派生除了用于图像拟合的图优化顶点和边，同时重写了重要的虚函数：
- 顶点的更新函数：
oplusImpl。我们知道优化过程最重要的是增量 Δ x \Delta \pmb x Δxxx的计算，而该函数处理的是 x k + 1 = x k + Δ x \pmb x_{k+1} = \pmb x_k + \Delta \pmb x xxxk+1=xxxk+Δxxx的过程 - 顶点的重置函数：
setToOriginImpl。 这是平凡的，我们把估计值置零即可。 - 边的误差计算函数：
computeError。 该函数需要取出边所连接的顶点的当前估计值，根据曲线模型，与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。 - 边的雅可比计算函数：
lnearizeOplus。这个函数里我们计算了每条边相对于顶点的雅可比。 - 存盘和读盘函数：
read,write。 由于我们并不想进行读/写操作，所以留空。
扫一扫
262
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
