一、李群与李代数
1.1 群
群是一种集合加上一种运算的代数结构。我们把集合记作 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. \forall 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
根据这个条件可以发现旋转矩阵、变换矩阵都可以称为群
- 旋转矩阵——特殊正交群(SO(3)), S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3) = \{ R \in R^{3 \times 3} | RR^T=I, det(R)=1\} SO(3)={R∈R3×3∣RRT=I,det(R)=1}
- 变换矩阵——特殊欧式群(SE(3)),如公式所示
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
SE(3)=\begin{cases} T= \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in R^{4 \times 4} | R \in SO(3), t \in R^{3} \end{cases}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3
tips:李群是指具有连续(光滑)性质的群。
SO(3)和SE(3)都是李群,由于群中只定义了乘法,没有定义加法,所以不能直接求导。
1.2. 李代数
每个李群都有与之对应的李代数。李代数由一个集合V,一个数域F和一个二元运算[,]组成。
注释:
- 每个李群都有与之对应的李代数:表示李群与李代数之间有联系,可以相互转化,但是并不表明是一对一的关系;
- 二元运算[,]: 也称为李括号,李代数的运算在形式已经规定好了,但是具体的李括号怎么算要按照条件定义。
李代数要满足的条件:
- 封闭性: ∀ X , Y ∈ A , [ X , Y ] ∈ A \forall X, Y \in A, [X, Y] \in A ∀X,Y∈A,[X,Y]∈A
- 双线性: ∀ X , Y , Z ∈ A , a , b ∈ F \forall X, Y, Z \in A, a, b \in F ∀X,Y,Z∈A,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 ] [aX+BY, Z] =a[X, Z] + b[Y, Z], [Z, aX+bY]= a[Z, X] + b[Z, Y] [aX+BY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]
- 自反性: ∀ X ∈ V , [ X , X ] = 0 \forall X \in V,[X,X] = 0 ∀X∈V,[X,X]=0
- 雅可比等价: ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ Y , X ] ] + [ Y , [ Z , x ] ] = 0 \forall X, Y, Z \in V, [X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,x]]=0 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,x]]=0
tips:
三维向量中的叉积是一种李括号,所以
g
=
(
R
3
,
R
,
×
)
g=(R^3, R, \times )
g=(R3,R,×)是一种李括号。
1.2.1 李群SO(3)的李代数so(3)
SO(3)对应的李代数为so(3),是定义在
R
3
R^3
R3上的向量,记作
ϕ
\phi
ϕ。根据前面的推导,可以得出每个
ϕ
\phi
ϕ都可以生成一个反对称矩阵:
Φ
=
ϕ
∧
[
0
−
ϕ
3
ϕ
2
ϕ
3
0
−
ϕ
1
−
ϕ
2
ϕ
1
0
]
\Phi = \phi^\land \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix}
Φ=ϕ∧
0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10
给出李括号
[
ϕ
1
,
ϕ
2
]
=
(
Φ
1
Φ
2
−
Φ
2
Φ
1
)
∨
[\phi_1, \phi_2] = (\Phi_1\Phi_2-\Phi_2\Phi_1)^{\lor}
[ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨
然后就可以说
s
o
(
3
)
=
ϕ
∈
R
3
,
Φ
=
ϕ
∧
∈
R
3
×
3
so(3)= { \phi \in R^3, \Phi = \phi^\land \in R^{3 \times 3}}
so(3)=ϕ∈R3,Φ=ϕ∧∈R3×3是SO(3)对应的李代数
1.2.2 李群SE(3)的李代数se(3)
变换矩阵要表示平移和旋转,在李代数中每个元素用
ξ
=
[
ρ
ϕ
]
\xi =\begin{bmatrix} \rho \\\phi\end{bmatrix}
ξ=[ρϕ] 表示,其中
ρ
\rho
ρ表示平移,
ϕ
\phi
ϕ表示旋转。这里定义
∧
\land
∧表示把四维矩阵升为六维,
∨
\lor
∨表示把六维降为四维,就有
x
i
∧
=
[
ϕ
∧
ρ
0
T
0
]
xi^{\land}=\begin{bmatrix} \phi^\land & \rho \\0^T & 0 \end{bmatrix}
xi∧=[ϕ∧0Tρ0] 。定义李括号
[
ξ
1
,
ξ
2
]
=
(
ξ
1
∧
ξ
2
∧
−
ξ
2
∧
ξ
1
∧
)
[\xi_1, \xi_2]=(\xi_1^{\land}\xi_2^{\land}-\xi_2^{\land}\xi_1^{\land})
[ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧),然后就可以说se(3)是SE(3)对应的李代数。
s
e
(
3
)
=
{
ξ
=
[
ρ
ϕ
]
∈
R
6
,
ρ
∈
R
3
,
ϕ
∈
s
o
(
3
)
,
ξ
∧
=
[
ϕ
∧
ρ
0
T
0
]
∈
R
4
×
4
se(3) = \begin{cases} \xi = \begin{bmatrix} \rho \\ \phi \end{bmatrix} \in R^6, \rho \in R^3, \phi \in so(3), \xi^{\land}=\begin{bmatrix} \phi^\land & \rho \\ 0^T & 0 \end{bmatrix} \in R^{4 \times 4} \end{cases}
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4
1.3. 李群和李代数的关系
1.3.1 指数关系
李群R和对应的李代数
ϕ
\phi
ϕ存在指数关系:
R
=
e
x
p
(
ϕ
)
R = exp(\phi)
R=exp(ϕ)
推导过程:
已知
R
R
T
=
I
RR^T = I
RRT=I
可以将
R
R
R看做时间的函数,所以
R
(
t
)
R
(
t
)
T
=
I
R(t)R(t)^T = I
R(t)R(t)T=I
对
R
(
t
)
R(t)
R(t)进行求导,可以得到
R
(
t
)
˙
R
(
t
)
T
+
R
(
t
)
R
(
t
)
˙
T
=
0
\dot{R(t)} R(t)^T + R(t)\dot{R(t)}^T=0
R(t)˙R(t)T+R(t)R(t)˙T=0
整理得:
R
(
t
)
˙
R
(
t
)
T
=
−
(
R
(
t
)
˙
R
(
t
)
T
)
T
\dot{R(t)}R(t)^T=-(\dot{R(t)}R(t)^T)^T
R(t)˙R(t)T=−(R(t)˙R(t)T)T
可以看出
R
(
t
)
˙
R
(
t
)
T
\dot{R(t)}R(t)^T
R(t)˙R(t)T是一个反对称矩阵。所以一定存在
R
(
t
)
˙
R
(
t
)
T
=
ϕ
(
t
)
∧
\dot{R(t)}R(t)^T=\phi(t)^{\land}
R(t)˙R(t)T=ϕ(t)∧
在等式两边都乘以
R
(
t
)
R(t)
R(t),可以得到
R
(
t
)
˙
=
ϕ
(
t
)
∧
R
(
t
)
=
[
0
−
ϕ
3
ϕ
2
ϕ
3
0
−
ϕ
1
−
ϕ
2
ϕ
1
0
]
R
(
t
)
\dot{R(t)}=\phi(t)^{\land}R(t)=\begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} R(t)
R(t)˙=ϕ(t)∧R(t)=
0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10
R(t)
可以看到,每对旋转矩阵求一次导数,只需左乘一个
ϕ
(
t
)
∧
ϕ(t)^{\land}
ϕ(t)∧矩阵即可。为方便讨论,我们设
t
0
=
0
t_0 = 0
t0=0,并设此时旋转矩阵为
R
(
0
)
=
I
R(0) = I
R(0)=I。按照导数定义,可以把
R
(
t
)
R(t)
R(t) 在0附近进行一阶泰勒展开:
R
(
t
)
≈
R
(
t
0
)
+
R
(
t
0
)
˙
(
t
−
t
0
)
=
I
+
ϕ
(
t
0
)
∧
R(t) \approx R(t_0)+\dot{R(t_0)}(t-t_0) = I + \phi(t_0)^{\land}
R(t)≈R(t0)+R(t0)˙(t−t0)=I+ϕ(t0)∧
可以得到
R
(
t
)
˙
=
ϕ
0
(
t
)
∧
R
(
t
)
\dot{R(t)} = \phi_0(t)^{\land}R(t)
R(t)˙=ϕ0(t)∧R(t)
解之得:
R
(
t
)
=
e
x
p
(
ϕ
0
∧
(
t
)
)
R(t)=exp(\phi_0^{\land}(t))
R(t)=exp(ϕ0∧(t))
1.3.2 指数与对数映射
1.4 李群和李代数的求导
l
n
(
e
x
p
(
ϕ
1
)
e
x
p
(
ϕ
2
)
)
≈
{
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
,
i
f
ϕ
1
i
s
s
m
a
l
l
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
i
f
ϕ
2
i
s
s
m
a
l
l
ln (exp (ϕ^ 1 ) exp (ϕ^ 2 ))_ ≈ \begin{cases} J_l(ϕ2)^{−1}ϕ_1 + ϕ_2 , \quad if\quad ϕ_1 \quad is \quad small \\ J_r(ϕ1)^{−1}ϕ_2 + ϕ_1 \quad if \quad ϕ_2 \quad is \quad small \end{cases}
ln(exp(ϕ1)exp(ϕ2))≈{Jl(ϕ2)−1ϕ1+ϕ2,ifϕ1issmallJr(ϕ1)−1ϕ2+ϕ1ifϕ2issmall
其中,
J
l
J_l
Jl为
J
l
=
J
=
s
i
n
θ
θ
I
+
(
1
−
s
i
n
θ
θ
)
a
a
T
+
1
−
c
o
s
θ
θ
a
∧
J_l = J = \frac{sin θ}{θ} I + (1 − \frac{sin θ}{θ}) aa^T + 1 −\frac{cos θ}{θ} a^{\land}
Jl=J=θsinθI+(1−θsinθ)aaT+1−θcosθa∧
它的逆为:
J
l
−
1
=
θ
2
c
o
t
θ
2
I
+
(
1
−
θ
2
c
o
t
θ
2
)
a
a
T
−
θ
2
a
∧
J^{−1}_l=\frac{θ}{2}cot\frac{θ}{2}I + (1 − \frac{θ}{2} cot\frac{θ}{2} ) aa^T − \frac{θ}{2}a^{\land}
Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧
而右乘雅可比仅需要对自变量取负号即可:
J
r
(
ϕ
)
=
J
l
(
−
ϕ
)
J_r(ϕ) = J_l(−ϕ)
Jr(ϕ)=Jl(−ϕ)
假定对某个旋转
R
R
R,对应的李代数为
ϕ
ϕ
ϕ。我们给它左乘一个微小旋转,记作∆R,对应的李代数为
∆
ϕ
∆ϕ
∆ϕ。那么,在李群上,得到的结果就是
∆
R
⋅
R
∆R·R
∆R⋅R,而在李代数上,根据 BCH近似,为:
J
l
−
1
(
ϕ
)
∆
ϕ
+
ϕ
J_l^{−1}(ϕ)∆ϕ + ϕ
Jl−1(ϕ)∆ϕ+ϕ。合并起来,可以简单地写成:
e
x
p
(
∆
ϕ
∧
)
e
x
p
(
ϕ
∧
)
=
e
x
p
(
(
ϕ
+
J
l
−
1
(
ϕ
)
∆
ϕ
)
∧
)
exp (∆ϕ^{\land}) exp (ϕ^{\land}) = exp ((ϕ + J_l^{−1} (ϕ) ∆ϕ)^{\land} )
exp(∆ϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)∆ϕ)∧)
反之,如果我们在李代数上进行加法,让一个
ϕ
ϕ
ϕ 加上
∆
ϕ
∆ϕ
∆ϕ,那么可以近似为李群上带左右雅可比的乘法:
e
x
p
(
(
ϕ
+
∆
ϕ
)
∧
)
=
e
x
p
(
(
J
l
∆
ϕ
)
∧
)
e
x
p
(
ϕ
∧
)
=
e
x
p
(
ϕ
∧
)
e
x
p
(
(
J
r
∆
ϕ
)
∧
)
exp ((ϕ + ∆ϕ)^{\land} ) = exp ((J_l∆ϕ)^{\land}) exp (ϕ^{\land} ) = exp (ϕ^{\land} ) exp ((J_r∆ϕ)^{\land} )
exp((ϕ+∆ϕ)∧)=exp((Jl∆ϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((Jr∆ϕ)∧)
这将为之后李代数上的做微积分提供了理论基础。同样的,对于
S
E
(
3
)
SE(3)
SE(3),亦有类似的BCH 近似公式:
e
x
p
(
∆
ξ
∧
)
e
x
p
(
ξ
∧
)
≈
e
x
p
(
(
J
l
−
1
∆
ξ
+
ξ
)
∧
)
exp (∆ξ^{\land}) exp (ξ^{\land}) ≈ exp ((J_l^{-1}∆ξ + ξ)^{\land}) \\
exp(∆ξ∧)exp(ξ∧)≈exp((Jl−1∆ξ+ξ)∧)
e
x
p
(
ξ
∧
)
e
x
p
(
∆
ξ
∧
)
≈
e
x
p
(
(
J
r
−
1
∆
ξ
+
ξ
)
∧
)
exp (ξ^{\land}) exp (∆ξ^{\land}) ≈ exp ((J_r^{-1}∆ξ + ξ)^{\land})
exp(ξ∧)exp(∆ξ∧)≈exp((Jr−1∆ξ+ξ)∧)
1.4.1 求导模型
求导模型做的事情是:将旋转矩阵对应的李代数加一个小量,求相对于小量的变化率。
对于SO(3),要计算的是旋转之后的坐标相对于旋转的导数,记做
∂
R
(
t
)
R
\partial \frac{R(t)}{R}
∂RR(t),李群无法求导,转而对李代数so(3)求导,计算
∂
(
e
x
p
(
ϕ
∧
)
p
)
∂
ϕ
\frac{ \partial (exp(\phi^\land)p)}{\partial \phi}
∂ϕ∂(exp(ϕ∧)p)。
1.4.2 扰动模型
扰动模型做的事情是:左乘或者右乘一个小量,求相对于小量的李代数的变化率。
左乘扰动的推导如下:
对于SE(3),根据扰动模型求导的推导如下:
以上,现在就可以对旋转矩阵进行求导了。
二、相机与图像
2.1 单目相机
2.1.1 小孔成像模型
和以前物理课上讲的一样,朴素又简单的小孔成像模型,一个蜡烛放在P处,经过一个小孔,会在后面的屏幕上呈现一个倒像。
这里,红色的 P P ′ PP^{'} PP′线表示光线,f是成像平面到小孔的距离,Z是物体P到小孔(相机)的距离,X是物体的长度, X ′ X^{'} X′是像的长度。
根据相似三角形定理,可以得到
Z
f
=
−
X
X
′
=
−
Y
Y
′
\frac{Z}{f} = -\frac{X}{X^{'}} = - \frac{Y}{Y^{'}}
fZ=−X′X=−Y′Y
如果把成像平面对称到相机的前面,如图5-2中间所示,则P’的坐标[X’,Y’,Z’]与P点坐标符号相同,根据相似三角形,可以导出点和像的关系:
![[]]
Z
f
=
X
X
′
=
Y
Y
′
\frac{Z}{f} = \frac{X}{X^{'}} = \frac{Y}{Y^{'}}
fZ=X′X=Y′Y
从而得出焦距与成像的关系
X
′
=
f
X
Z
X^{'} = f \frac{X}{Z} \\
X′=fZX
Y
′
=
f
Y
Z
Y^{'} = f \frac{Y}{Z}
Y′=fZY
像素坐标系通常的定义方式是:原点o′位于图像的左上角,u 轴向右与x轴平,v
轴向下与 y 轴平行。像素坐标系与成像平面之间,相差了一个缩放和一个原点的平移。我们设像素坐标在 u 轴上缩放了 α 倍,在 v 上缩放了 β 倍。同时,原点平移了
[
c
x
;
c
y
]
T
[cx; cy]^T
[cx;cy]T。那么, P′ 的坐标与像素坐标
[
u
;
v
]
T
[u; v]^T
[u;v]T 的关系为:
{
u
=
α
X
′
+
c
x
v
=
β
Y
′
+
c
y
\begin{cases} u &= &\alpha X^{'} + c_x \\ v &=& \beta Y^{'} +c_y \end{cases}
{uv==αX′+cxβY′+cy
代入式并把 αf 合并成 fx,把 βf 合并成 fy,得
{
u
=
f
x
X
′
+
c
x
v
=
f
y
Y
′
+
c
y
\begin{cases} u &= &f_x X^{'} + c_x \\ v &=& f_y Y^{'} +c_y \end{cases}
{uv==fxX′+cxfyY′+cy
其中, f 的单位为米, α; β 的单位为像素每米,所以
f
x
f_x
fx,
f
y
f_y
fy 的单位为像素。把该式写成矩阵形式,会更加简洁,不过左侧需要用到齐次坐标:
(
u
v
1
)
=
1
Z
(
f
x
0
c
x
0
f
y
c
y
0
0
1
)
=
1
Z
K
P
\begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = \frac{1}{Z} \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} = \frac{1}{Z}KP
uv1
=Z1
fx000fy0cxcy1
=Z1KP
从公式(5.6)中可以看到,对于静止相机来说,K固定且相机坐标系固定;对于静止物体来说P固定。但这个物体的像素坐标系却不能固定,因为有Z的存在,通过Z可以将物体P的像素坐标放大或者缩小一定的倍数。这就是单目相机的缺陷,无法确定景深,只能获得物体的相对大小,不能得到物体的尺寸。
相机是机器人的眼睛,机器人在动,相机也在动,相机坐标系就发生了变化。记物体P的世界坐标为 P w P_w Pw,前面讲过可以通过变换矩阵,将世界坐标转换到机器人(相机)坐标,有 P c = ( R P w + t ) = T c w P w P_c=(RP_w+t)=T_{cw}P_w Pc=(RPw+t)=TcwPw,所以像素坐标系和世界坐标系的关系,可以写为: Z P u v = Z [ u v 1 ] = K ( R P c w + t ) = K T c w P w ZP_{uv}=Z\begin{bmatrix} u \\ v \\1\end{bmatrix}=K(RP_{cw}+t)=KT_{cw}P_w ZPuv=Z uv1 =K(RPcw+t)=KTcwPw,其中R,t称为相机的外参数。
再通过归一化,把非齐次的相机坐标 P ~ c [ x , y , z ] \tilde P_c[x, y, z] P~c[x,y,z],变为齐次坐标 P c [ x z , y z , 1 ] P_c[\frac{x}{z}, \frac{y}{z}, 1] Pc[zx,zy,1],称 P c P_c Pc为归一化坐标,位于归一化平面上。再和K相乘,得到在像素坐标系中的齐次坐标,忽略非零常数Z,可以简写为 P u v = K T c w P w P_{uv}=KT_{cw}P_w Puv=KTcwPw.
2.2 畸变
相机的前方通常会有一个透镜,会对成像产生影响,称为畸变。
由透镜形状引起的畸变称为径向畸变,包括桶形畸变和枕形畸变。桶形畸变是由于图像放大率随着与光轴之间的距离增加而减小;枕形畸变相反。
桶形畸变是由于图像放大率随着离光轴的距离增加而减小,而枕形畸变却恰好相反。在这两种畸变中,穿过图像中心和光轴有交点的直线还能保持形状不变。
除了透镜的形状会引入径向畸变外,在相机的组装过程中由于不能使得透镜和成像面严格平行也会引入切向畸变。如图 5-4 所示。
对于径向畸变,无论是桶形畸变还是枕形畸变,由于它们都是随着离中心的距离增加而增加。我们可以用一个多项式函数来描述畸变前后的坐标变化:这类畸变可以用和距中心距离有关的二次及高次多项式函数进行纠正:
x
c
o
r
r
e
c
t
e
d
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
x_{corrected} = x(1+k_1r^2+k_2r^4+k_3r^6)
xcorrected=x(1+k1r2+k2r4+k3r6)
y
c
o
r
r
e
c
t
e
d
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
y_{corrected} = y(1+k_1r^2+k_2r^4+k_3r^6)
ycorrected=y(1+k1r2+k2r4+k3r6)
且向畸变通过公式(5.12)矫正。
x
c
o
r
r
e
c
t
e
d
=
x
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
x_{corrected} = x+2p_1xy+p_2(r^2+2x^2)
xcorrected=x+2p1xy+p2(r2+2x2)
y
c
o
r
r
e
c
t
e
d
=
y
+
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
y_{corrected} = y+p_1(r^2+2y^2)+2p_2xy
ycorrected=y+p1(r2+2y2)+2p2xy
2.3 双目相机
普通的双目相机利用视差计算深度,RGB-D相机直接测量深度。
两个相机水平放置,中间的距离称为基线,物体P在两个相机中的位置有差别,利用这个差别计算景深。
图5-6的几何模型中,分别以两个相机为原点,以右边为x轴正方向建立坐标系,点P在左右两个相机的成像分别为
P
l
P_l
Pl ,
P
r
P_r
Pr ,从坐标系的建立中可以知道,两个成像点的横坐标符号不同,其中
P
l
P_l
Pl的横坐标为正,
P
r
P_r
Pr的横坐标为负。
根据相似三角形定理,可以得到
z
−
f
z
=
b
−
u
L
+
u
R
b
\frac{z-f}{z}=\frac{b-u_L+u_R}{b}
zz−f=bb−uL+uR
稍加整理得到:
z
=
f
b
d
,
d
=
u
L
−
u
R
z=\frac{fb}{d}, d=u_L-u_R
z=dfb,d=uL−uR
其中,d称为视差,视差越大,距离越近;视差越小,距离越大。双目的深度z存在一个理论上的最大值,由fb决定。
在计算视差时,需要比较左右相机中物品的位置,耗费大量的计算资源,目前仍需要使用GPU或者FPGA来计算。
RGB-D通过红外结构光或者飞行时间法测量像素距离。红外结构光原理中,相机根据反悔的结构光图案,计算物体自身之间的距离。飞行时间法,向目标发射脉冲光,然后根据发送到返回之间的光束飞行时间,确定物体与自身的距离。
RGB-D不需要大量计算,但是对于测量环境要求高,使用范围有限。
2.4 图像
一个宽度w,高度h的图像,数学上可以用 ��∗� 的矩阵表示。
计算机上,灰度图用0-255表示,宽度为640,高度为480像素的图像,在计算机中表示为:
unsigned char image[480][640];
三、非线性优化
本章讲的是非线性优化求解的基础知识。
3.1 状态估计问题
经典的SLAM模型包括一个运动方程和观测方程,如公式(6.1)所示。其中u是传感器输入,z是观测变量,w和v是噪声。
{
x
k
=
f
(
x
k
−
1
,
u
k
)
+
ω
k
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
\begin{cases} x_k&=&f(x_{k-1}, u_k)+\omega_k \\ z_{k,j} &=& h(y_j,x_k)+v_{k,j} \end{cases}
{xkzk,j==f(xk−1,uk)+ωkh(yj,xk)+vk,j
也就是说SLAM问题是通过带噪声的数据z和u推断位姿x和地图y的问题,这是一个状态估计问题。
从概率学上讲,如果把所有的待估计量,也就是所有时刻的位姿和地图放到一个状态变量中,有
x
=
x
1
,
⋯
,
x
N
,
y
1
,
⋯
,
y
M
x={x_1, \cdots, x_N, y_1, \cdots, y_M}
x=x1,⋯,xN,y1,⋯,yM
那么SLAM问题就是一直输入u和观测数据z的条件下,计算状态x的条件概率分布问题:
P
(
x
∣
z
,
u
)
P(x|z,u)
P(x∣z,u)。
在不考虑传感器输入u时 P ( x ∣ z ) P(x|z) P(x∣z),该问题变成,已知观测如何推断位姿和地图,称为Structure from Motion(SfM),即如何从许多图像中重建三维空间结构。
根据贝叶斯法则,有
P
(
x
∣
z
)
=
P
(
z
∣
x
)
P
(
x
)
P
(
z
)
∝
P
(
z
∣
x
)
P
(
x
)
P(x|z)=\frac{P(z|x)P(x)}{P(z)} \propto P(z|x)P(x)
P(x∣z)=P(z)P(z∣x)P(x)∝P(z∣x)P(x)
P(x)称为先验,表示预先知道x的状态;P(z|x)称为似然,表示在当前位姿x下产生观测z的概率;P(x|z)称为后验概率,表示产生观测z时位姿是x的概率。目标是求解使得P(x|z)最大的x,也就是
x
M
a
p
∗
=
a
r
g
m
a
x
P
(
x
∣
z
)
=
a
r
g
m
a
x
P
(
z
∣
x
)
P
(
x
)
x^*_{Map} = {\rm arg max} P(x|z) = {\rm arg max} P(z|x)P(x)
xMap∗=argmaxP(x∣z)=argmaxP(z∣x)P(x)
在不知道机器人位姿x的情况下,可以近似求解x的最大似然估计
x
M
a
p
∗
=
a
r
g
m
a
x
P
(
z
∣
x
)
x^*_{Map} = {\rm arg max} P(z|x)
xMap∗=argmaxP(z∣x),表示在位姿x下,最可能产生观测z。
对于观测方程公式(6.1)中第二个公式,如果认为
v
k
N
(
0
,
Q
k
,
j
)
v_k ~ N(0, Q_{k,j})
vk N(0,Qk,j),则条件概率
P
(
z
j
,
x
∣
x
k
,
y
j
)
=
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
P(z_{j,x}|x_k, y_j)=N(h(y_j, x_k), Q_{k,j})
P(zj,x∣xk,yj)=N(h(yj,xk),Qk,j) ,我们的目标变成了求解
x
∗
=
[
x
k
,
⋯
,
y
j
,
⋯
]
x^*=[x_k,\cdots,y_j, \cdots]
x∗=[xk,⋯,yj,⋯],使得
P
(
z
j
,
x
∣
x
k
,
y
j
)
P(z_{j,x}|x_k, y_j)
P(zj,x∣xk,yj)最大,也就是求解高斯分布
μ
=
h
(
y
j
,
x
k
)
\mu = h(y_j, x_k)
μ=h(yj,xk),
∑
=
Q
k
,
j
\sum =Q_{k,j}
∑=Qk,j最大似然。
P
(
x
)
=
1
(
2
π
)
N
d
e
t
(
∑
)
e
x
p
(
−
1
2
(
x
−
μ
)
T
∑
−
1
(
x
−
μ
)
)
P(x) = \frac{1}{\sqrt{(2\pi)^N det(\sum)}}exp(- \frac{1}{2}(x-\mu)^T {\sum}^{-1}(x-\mu))
P(x)=(2π)Ndet(∑)1exp(−21(x−μ)T∑−1(x−μ))
对高斯分布的概率密度函数取负对数,有公式(6.9)
−
l
n
(
P
(
x
)
)
=
1
2
l
n
(
(
2
π
)
N
d
e
t
(
∑
)
)
+
1
2
(
x
−
μ
)
∑
−
1
(
x
−
μ
)
-{\rm ln} (P(x)) = \frac{1}{2}{\rm ln}((2 \pi)^N {\rm det}(\sum))+\frac{1}{2}(x-\mu){\sum}^{-1}(x-\mu)
−ln(P(x))=21ln((2π)Ndet(∑))+21(x−μ)∑−1(x−μ)
SLAM问题中需要求解x,公式(6.9)中第一项与x无关,可以忽略。所以求解高斯
μ
=
h
(
y
j
,
x
k
)
\mu = h(y_j, x_k)
μ=h(yj,xk),
∑
=
Q
k
,
j
\sum =Q_{k,j}
∑=Qk,j分布的最大似然,等价于求解公式(6.10)。
x
∗
=
a
r
g
m
i
n
(
(
z
k
,
j
−
h
(
x
k
.
y
j
)
)
T
Q
k
,
j
−
1
(
z
k
,
j
−
h
(
x
k
.
y
j
)
)
)
x^* = {\rm argmin} \left( \big (z_{k,j}-h(x_k.y_j) \big )^TQ_{k,j}^{-1}\big (z_{k,j}-h(x_k.y_j) \big )\right)
x∗=argmin((zk,j−h(xk.yj))TQk,j−1(zk,j−h(xk.yj)))
定义观测误差为
e
y
,
j
,
k
=
z
k
,
j
−
h
(
x
k
,
y
j
)
e_{y,j,k}=z_{k,j}-h(x_k,y_j)
ey,j,k=zk,j−h(xk,yj),则公式(6.10)等价于计算误差项的平方。定义运动误差为
e
v
,
k
=
x
k
−
f
(
x
k
−
1
,
u
k
)
e_{v,k}=x_k-f(x_{k-1}, u_k)
ev,k=xk−f(xk−1,uk),那么对于所有的运动和观测而言,SLAM问题相当于找到一个最有解,使得所有误差的平方和最小,如公式(6.12)所示。
J
(
x
)
=
∑
k
e
v
,
k
T
R
k
−
1
e
v
,
k
+
∑
k
∑
j
e
y
,
j
,
k
T
Q
k
,
j
−
1
e
y
,
j
,
k
J(x)=\sum_{k}e_{v,k}^TR_k^{-1}e_{v,k}+\sum_k \sum_j e_{y,j,k}^TQ_{k,j}^{-1} e_{y,j,k}
J(x)=k∑ev,kTRk−1ev,k+k∑j∑ey,j,kTQk,j−1ey,j,k
3.2 最小二乘求解
当把问题转化为最小二乘问题之后,需要进行求解。最简单的最小二乘问题
min
x
1
2
∥
f
(
x
)
∥
2
2
\min_x \frac{1}{2}\| f(x) \|_2^2
xmin21∥f(x)∥22
可以直接求导,导数为零计算极值。或者进行迭代求解,如图迭代步骤所示。
在SLAM优化问题中,通常很难求解导数,转而使用迭代计算。在迭代计算中
Δ
x
\Delta x
Δx增量有不同的确定方法,对应着不同的求解算法。
∥
f
(
x
+
Δ
x
)
∥
2
2
=
∥
f
(
x
)
∥
2
2
+
J
(
x
)
Δ
x
+
1
2
Δ
x
T
H
Δ
x
\left \| f(x+\Delta x) \right \|_2^2 = \left \| f(x) \right \|_2^2 + J(x) \Delta x + \frac{1}{2} \Delta x^T H \Delta x
∥f(x+Δx)∥22=∥f(x)∥22+J(x)Δx+21ΔxTHΔx
3.2.1 一阶或二阶梯度法
对目标函数在x附近进行泰勒展开,得到公式(6.15)
∥
f
(
x
+
Δ
x
)
∥
2
2
=
∥
f
(
x
)
∥
2
2
+
J
(
x
)
Δ
x
+
1
2
Δ
x
T
H
Δ
x
\left\| f(x+\Delta x) \right\|_2^2 = \left\| f(x) \right\|_2^2 + J(x)\Delta x + \frac{1}{2}\Delta x^T H \Delta x
∥f(x+Δx)∥22=∥f(x)∥22+J(x)Δx+21ΔxTHΔx
J称为雅可比矩阵,是f(x)的一阶导数,H称为黑塞矩阵,是f(x)的二阶导数。
如果保留一阶展开,则关于增量的目标函数变成
min
Δ
x
∥
f
(
x
)
∥
2
2
+
J
(
x
)
Δ
x
\min_{\Delta x} \left \| f(x) \right\|_2^2 + J(x) \Delta x
Δxmin∥f(x)∥22+J(x)Δx
对增量
Δ
x
\Delta x
Δx求导,取负梯度方向作为增量方向(梯度表示某一函数在该点处的方向导数沿着该方向取得最大值,即函数在该点处沿着该方向(此梯度的方向)变化最快,想要获得最小值应该沿着梯度的反方向变化可得。在图-迭代步骤中,使用这个增量进行迭代求解的算法称为最速下降法。
如果保留二阶展开,则关于增量的目标函数变成
min
Δ
x
∥
f
(
x
)
∥
2
2
+
J
(
x
)
Δ
x
+
1
2
Δ
x
T
H
Δ
x
\min_{\Delta x} \left \| f(x) \right\|_2^2 + J(x) \Delta x + \frac{1}{2} \Delta x^T H \Delta x
Δxmin∥f(x)∥22+J(x)Δx+21ΔxTHΔx
同样的对增量求导,令导数为零,可得
H
Δ
x
∗
=
−
J
T
H\Delta x^* = -J^T
HΔx∗=−JT
在图-迭代步骤中,使用这个增量进行迭代求解的算法称为牛顿法。
最速下降法存在zigzag问题,如图-zigzag问题所示,在求解时直接沿着反向梯度方向,有时会绕路。牛顿法要计算黑塞矩阵,计算量很大。
3.2.2 高斯牛顿法
前面是对平方项进行展开,高斯牛顿法对f(x)做一阶泰勒展开,有
f
(
x
+
Δ
x
)
≈
f
(
x
)
+
J
(
x
)
Δ
x
f(x+\Delta x) \approx f(x) +J(x)\Delta x
f(x+Δx)≈f(x)+J(x)Δx
对于最小二乘问题,需要进行平方,然后对
Δ
x
\Delta x
Δx求导,将目标函数的平方项展开,得到公式-展开。
先展开目标函数的平方项:
1 2 ∥ f ( x + J ( x ) Δ x ) ∥ 2 = 1 2 ( f ( x ) + J ( x ) Δ x ) T ( f ( x ) + J ( x ) Δ x ) = 1 2 ( ∥ f ( x ) ∥ 2 + 2 f ( x ) T J ( x ) Δ x + Δ x T J ( x ) T J ( x ) Δ x ) \begin{split} \frac{1}{2}\| f(x+J(x)\Delta x) \|^2 &= \frac{1}{2}(f(x) + J(x)\Delta x)^T(f(x) + J(x)\Delta x) \\ &= \frac{1}{2} \left( \|f(x)\|^2 + 2f(x)^TJ(x)\Delta x + \Delta x^T J(x)^T J(x) \Delta x \right ) \end{split} 21∥f(x+J(x)Δx)∥2=21(f(x)+J(x)Δx)T(f(x)+J(x)Δx)=21(∥f(x)∥2+2f(x)TJ(x)Δx+ΔxTJ(x)TJ(x)Δx)
求上式关于∆x的导数,并令其为零
2
J
(
x
)
T
f
(
x
)
+
2
J
(
x
)
T
J
(
x
)
Δ
x
=
0
2J(x)^Tf(x)+2J(x)^TJ(x)\Delta x = 0
2J(x)Tf(x)+2J(x)TJ(x)Δx=0
可以得到如下方程组:
J
(
x
)
T
J
(
x
)
Δ
x
=
−
J
(
x
)
T
f
(
x
)
J(x)^T J(x) \Delta x = -J(x)^T f(x)
J(x)TJ(x)Δx=−J(x)Tf(x)
我们要求解的变量是∆x,因此这是一个线性方程组,我们称它为增量方程,也
可以称为高斯牛顿方程 (Gauss Newton equations) 或者正规方程 (Normal equations)。我们把左边的系数定义为 H,右边定义为 g,那么上式变为:
H
Δ
x
=
g
H \Delta x = g
HΔx=g
3.2.3 列文伯格-马夸尔特方法
列文伯格——马夸尔特方法有称为阻尼牛顿法,收敛速度比高斯牛顿法慢,但是在SLAM中很常用。
一阶或者二阶泰勒展开只是在 Δx附近有效,当范围变大时泰勒近似得到的值会产生比较大的误差。阻尼牛顿法考虑给 Δx添加一个信赖区域,对其进行约束。
ρ
=
f
(
x
+
∆
x
)
−
f
(
x
)
J
(
x
)
∆
x
ρ =\frac{f (x + ∆x) − f (x)}{J (x) ∆x}
ρ=J(x)∆xf(x+∆x)−f(x)
该指标用来判断泰勒近似的好坏,分母表示实际的函数变化,分子表示近似模型的变化。如果
ρ
\rho
ρ太大,表示当前近似很差,需要缩小范围;如果
r
h
o
rho
rho太小,表示当前近似非常好,可以适当扩大范围。如此而来的阻尼牛顿法步骤如图-阻尼牛顿所示
其中,第4,5步中涉及到的数值均为经验值。公式(6.24)中的约束条件表示,把增量限定在一个球体中,矩阵D可以对这个球体的形状进行调节。
在求解公式(6.24)使,使用拉格朗日乘子法转化为无约束优化问题,然后求解,可以得到
(
H
+
λ
D
T
D
)
Δ
x
=
g
(H+\lambda D^TD)Δx=g
(H+λDTD)Δx=g,当D取单位矩阵时,有
(
H
+
λ
I
)
Δ
x
=
g
(H+\lambda I)Δx=g
(H+λI)Δx=g .可以认为在阻尼牛顿法中,
λ
\lambda
λ比较小的时候,H矩阵主导结果,此时阻尼牛顿法接近于高斯牛顿法;当
λ
\lambda
λ比较大的时候,
λ
I
\lambda I
λI主导结果,此时阻尼牛顿法更接近于最速下降法。
另外,在迭代求解中,一个好的初始值很重要。