SLAM是Simultaneous Localization and Mapping的缩写，中文译作同时定位与地图构建。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。
习题答案汇总：链接
自主运动两大基本问题：我在什么地方？（定位）周围什么样子？（建图）
How to SLAM？——Sensors
两类传感器：
相机分类：
整体视觉SLAM流程图如下：
视觉里程计VO：又称为前端
后端优化：
-从带有噪声的数据中优化轨迹和地图
回环检测：
建图：
SLAM问题的数学表述：
小萝卜携带着传感器在环境中运动”，由如下两件事情描述:
1.什么是运动 ?我们要考虑从
k
−
1
k-1
k−1时刻到
k
k
k时刻，小萝卜的位置
x
x
x是如何变化的。
运动方程:
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
x_k = f(x_{k-1},u_k,w_k)
xk=f(xk−1,uk,wk)
2.什么是观测 ?假设小萝卜在k时刻于
x
k
x_k
xk处探测到了某一个路标
y
j
y_j
yj，我们要考虑这件事情是如何用数学语言来描述的。
z
k
,
j
=
h
(
y
j
,
x
k
,
v
k
,
j
)
z_{k,j} = h(y_j,x_k,v_{k,j})
zk,j=h(yj,xk,vk,j)
这两个方程描述了最基本的SLAM问题：当知道运动测量的读数 u u u，以及传感器的读数 z z z时,如何求解定位问题(估计 x x x)和建图问题(估计 y y y)？这时，我们就把SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？
向量 α \alpha α在线性空间的基 [ e 1 , e 2 , e 3 ] [e_{1},e_{2},e_{3}] [e1,e2,e3]下的坐标为 [ α 1 , α 2 , α 3 ] ⊤ [\alpha_{1},\alpha_{2},\alpha_{3}]^\top [α1,α2,α3]⊤
向量的内积: 描述向量间的投影关系
a
⋅
b
=
a
T
b
=
∑
i
=
1
3
a
i
b
i
=
∣
a
∣
∣
b
∣
cos
⟨
a
,
b
⟩
a \cdot b=a^{T} b=\sum_{i=1}^{3} a_{i} b_{i}=|a||b| \cos \langle a, b\rangle
a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,b⟩
向量的外积: 描述向量的旋转
a
×
b
=
[
i
j
k
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
1
−
a
2
a
1
0
]
b
≜
a
∧
b
a \times b=\left[\begin{array}{ccc} i & j & k \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{array}\right]=\left[\begin{array}{c} a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{1} \end{array}\right]=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] b \triangleq a^{\wedge} b
a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b
其中
a
∧
a^{\wedge}
a∧ 表示
a
a
a 的反对称矩阵
a
∧
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
a^{\wedge}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right]
a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
旋转矩阵
R
R
R 的推导:
设单位正交基
[
e
1
,
e
2
,
e
3
]
\left[e_{1}, e_{2}, e_{3}\right]
[e1,e2,e3] 经过一次旋转变成了
[
e
1
′
,
e
2
′
,
e
3
′
]
\left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right]
[e1′,e2′,e3′], 对于同一个向量
a
a
a, 在两个坐标系下的坐标分别为
[
a
1
,
a
2
,
a
3
]
T
\left[a_{1}, a_{2}, a_{3}\right]^{T}
[a1,a2,a3]T 和
[
a
1
′
,
a
2
′
,
a
3
′
]
T
\left[a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime}\right]^{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
′
]
\left[e_{1}, e_{2}, e_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right]\left[\begin{array}{l} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right]
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
等式左右两边同时左乘
[
e
1
T
,
e
2
T
,
e
3
T
]
T
\left[e_{1}^{T}, e_{2}^{T}, e_{3}^{T}\right]^{T}
[e1T,e2T,e3T]T, 得到
[
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
′
\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{ccc} e_{1}^{T} e_{1}^{\prime} & e_{1}^{T} e_{2}^{\prime} & e_{1}^{T} e_{3}^{\prime} \\ e_{2}^{T} e_{1}^{\prime} & e_{2}^{T} e_{2}^{\prime} & e_{2}^{T} e_{3}^{\prime} \\ e_{3}^{T} e_{1}^{\prime} & e_{3}^{T} e_{2}^{\prime} & e_{3}^{T} e_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \triangleq R a^{\prime}
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′
矩阵
R
R
R 描述了旋转称为旋转矩阵。
旋转矩阵 R R R的性质
旋转矩阵是行列式为1的正交矩阵，任何行列式为1的正交矩阵也是一个旋转矩阵。所有旋转矩阵构成特殊正交群
S
O
SO
SO
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
det
(
R
)
=
1
}
S O(n)=\left\{R \in \mathbb{R}^{n \times n} \mid R R^{T}=I, \operatorname{det}(R)=1\right\}
SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
旋转矩阵是正交矩阵(其转置等于其逆), 旋转矩阵的逆 R − 1 R^{-1} R−1 (即转置 R T R^{T} RT )描述了一个相反的旋转。
变换矩阵
T
T
T :
在三维向量的末尾添加 1 , 构成的四维向量称为齐次坐标将旋转和平移写入变换矩阵
T
T
T 中,得到:
[
a
′
1
]
=
[
R
t
0
1
]
[
a
1
]
≜
T
[
a
1
]
\left[\begin{array}{c} a^{\prime} \\ 1 \end{array}\right]=\left[\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} a \\ 1 \end{array}\right] \triangleq T\left[\begin{array}{l} a \\ 1 \end{array}\right]
[a′1]=[R0t1][a1]≜T[a1]
齐次坐标的意义在于将欧式变换表示为线性关系。
变换矩阵 T T T 的性质:
方便判断是否在直线或平面上
若点
p
=
(
x
,
y
)
p=(x, y)
p=(x,y) 在直线
l
=
(
a
,
b
,
c
)
l=(a, b, c)
l=(a,b,c) 上, 则有:
a
x
+
b
y
+
c
=
[
a
,
b
,
c
]
T
⋅
[
x
,
y
,
1
]
=
l
T
⋅
p
′
=
0
a x+b y+c=[a, b, c]^{T} \cdot[x, y, 1]=l^{T} \cdot p^{\prime}=0
ax+by+c=[a,b,c]T⋅[x,y,1]=lT⋅p′=0
若点
p
=
(
x
,
y
,
z
)
p=(x, y, z)
p=(x,y,z) 在平面
A
=
(
a
,
b
,
c
,
d
)
A=(a, b, c, d)
A=(a,b,c,d) 上, 则有:
a
x
+
b
y
+
c
z
+
d
=
[
a
,
b
,
c
,
d
]
T
⋅
[
x
,
y
,
z
,
1
]
=
A
T
⋅
p
′
=
0
a x+b y+c z+d=[a, b, c, d]^{T} \cdot[x, y, z, 1]=A^{T} \cdot p^{\prime}=0
ax+by+cz+d=[a,b,c,d]T⋅[x,y,z,1]=AT⋅p′=0
方便表示线线交点和点点共线
在齐次坐标下,
性质1：可以用两个点
p
p
p，
q
q
q的齐次坐标叉乘结果表示它们的共线
l
l
l
性质2：可以用两条直线
l
l
l，
m
m
m的齐次坐标叉乘结果表示它们的交点
x
x
x
这里利用了叉乘的性质: 叉乘结果与两个运算向量都垂直。
能够表达无穷远点
对于平行直线
l
=
(
a
,
b
,
c
)
l=(a, b, c)
l=(a,b,c) 和
m
=
(
a
,
b
,
d
)
m=(a, b, d)
m=(a,b,d), 求取其交点的齐次坐标
x
=
l
×
m
=
(
k
b
,
−
k
a
,
0
)
x=l \times m=(k b,-k a, 0)
x=l×m=(kb,−ka,0), 将其转为非齐次坐标,得到
x
=
x=
x=
(
k
b
/
0
,
−
k
a
/
0
)
=
(
inf
,
−
inf
)
(k b / 0,-k a / 0)=(\inf ,-\inf )
(kb/0,−ka/0)=(inf,−inf), 这表示无穷远点.
能够简洁的表示变换
使用齐次坐标, 可以将加法运算转化为乘法运算。
假设有一个旋转轴为 n n n,角度为 θ \theta θ的旋转,其对应的旋转向量为 θ n \theta n θn。
设旋转向量 R R R表示一个绕单位向量 n n n，角度为 θ \theta θ的旋转。
为什么需要四元数：对于三维旋转，找不到不带奇异性的三维向量描述方式。因此引入四元数。四元数是一种扩展的复数，既是紧凑的,也没有奇异性。
四元数的定义
一个四元数
q
q
q 拥有一个实部和三个虚部
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q=q_{0}+q_{1} i+q_{2} j+q_{3} k
q=q0+q1i+q2j+q3k
其中
i
,
j
,
k
i, j, k
i,j,k, 为四元数的 3 个虚部,它们满足以下关系式(自己和自己的运算像复数,自己和别人的运算像叉乘):
{
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
\left\{\begin{array}{l} 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 \end{array}\right.
⎩⎪⎪⎨⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
也可以用一个标量和一个向量来表达四元数:
q
=
[
s
,
v
]
,
s
=
q
0
∈
R
v
=
[
q
1
,
q
2
,
q
3
]
T
∈
R
3
q=[s, v], \quad s=q_{0} \in \mathbb{R} \quad v=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3}
q=[s,v],s=q0∈Rv=[q1,q2,q3]T∈R3
s
s
s 为四元数的实部,
v
v
v 为四元数的虚部。有实四元数和虚四元数的概念。
四元数与旋转角度的关系:
单位四元数和旋转向量之间的转换:
设单位四元数
q
q
q 表示一个绕单位向量
n
=
[
n
x
,
n
y
,
n
z
]
T
n=\left[n_{x}, n_{y}, n_{z}\right]^{T}
n=[nx,ny,nz]T, 角度为
θ
\theta
θ 的旋转。
给定一个空间三维点 p = [ x , y , z ] ∈ R 3 p=[x, y, z] \in \mathbb{R}^{3} p=[x,y,z]∈R3,以及一个由轴角 n , θ n, \theta n,θ 指定的旋转,三维点 p p p 经过旋箦后变为 p ′ p^{\prime} p′ 。如何使用单位四元数 q q q 表达旋转?
注意：只有单位四元数才能表示旋转，因此在程序中创建四元数后，记得调用normalize()将其单位化。
