目标:理解slam的框架以及它的理论知识。供以后自己查阅。
第三章:主要讲一些基本的数学知识,包括点,向量,旋转矩阵三大类,高翔老师图文并用的方式讲的非常清晰。
第一部分
点
点需要在某个坐标系下的表达,一般在世界坐标系表达,一般情况是
e
0
=
[
1
0
0
]
e_0=\begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}
e0=⎣⎡100⎦⎤,
e
1
=
[
0
1
0
]
e_1=\begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix}
e1=⎣⎡010⎦⎤,
e
2
=
[
0
0
1
]
e_2=\begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}
e2=⎣⎡001⎦⎤。在日常表达中,这个这类基向量是省略的。
点存在于三维空间之中,因此在上述向量基空间中,由一个三维向量表示
p
→
=
(
x
,
y
,
z
)
\overrightarrow{p} =(x,y,z)
p=(x,y,z)。
向量
因为有点的表达,但是点与点之间关系需要数学表达方式,它就也是由向量表示。也是由一个三维向量表示
A
B
=
(
a
,
b
,
c
)
AB=(a,b,c)
AB=(a,b,c),它只有方向和大小的概念。同时它在不同的坐标系下,一般和点在用一个坐标系下(也可以等同于同一个向量基)。
点可以向量都可以用向量表示。因此基于向量的操作,有以下几类。
点乘(内积): a → ⋅ b → = a → T b → = ∑ i = 1 3 a i b i = ∣ a → ∣ b → ∣ c o s ( a , b ) \overrightarrow{a} \cdot \overrightarrow{b} = \overrightarrow{a}^T \overrightarrow{b} = \sum^{3}_{i=1}a_ib_i = |\overrightarrow{a}|\overrightarrow{b}|cos(a,b) 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 − b 1 a 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 → = s k e w ( a → ) b → \overrightarrow{a} \times \overrightarrow{b} = \begin{bmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{bmatrix} = \begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-b_1a_3 \\ a_1b_2-a_2b1 \end{bmatrix} = \begin{bmatrix} 0& -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \overrightarrow{b} = skew( \overrightarrow{a}) \overrightarrow{b} a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−b1a3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=skew(a)b
其中 s k e w ( a → ) skew(\overrightarrow{a}) skew(a)为反对称矩阵。
第二部分
三维刚体运动
上述介绍了一些向量和点等等的基本运算。进一步需要知道在空间中,坐标系是怎么变化的。以及怎么样计算不同坐标系之间,同一个点的坐标表达。
高翔老师介绍的怎么样从一个坐标系转化为另一个坐标系。可以看到,它在自己的坐标系下,先进行旋转,使得它和目标坐标系方向一致,轴对应。然后用平移向量,移动到目标坐标系即可。它包括这两个过程,使用表达方式为:旋转,平移。
1)旋转:
向量基:它是一组相互垂直的单位向量组成的。一般坐标系都是一组向量基组成,
假设原始向量基为:
e
0
→
\overrightarrow{e_0}
e0,
e
1
→
\overrightarrow{e_1}
e1,
e
2
→
\overrightarrow{e_2}
e2。目标向量基为:
e
0
→
′
\overrightarrow{e_0}'
e0′,
e
1
→
′
\overrightarrow{e_1}'
e1′,
e
2
→
′
\overrightarrow{e_2}'
e2′。假设一个向量
a
→
\overrightarrow{a}
a是固定向量,它不随着坐标系变化而移动,因此还是
a
→
\overrightarrow{a}
a,但是它不同坐标系下,它的坐标发生了变化。可以表达为:
[
e
0
→
e
1
→
e
2
→
]
[
a
0
a
1
a
2
]
=
[
e
0
→
′
e
1
→
′
e
2
→
′
]
[
a
0
′
a
1
′
a
2
′
]
(
1
)
\begin{bmatrix} \overrightarrow{e_0} & \overrightarrow{e_1}&\overrightarrow{e_2} \end{bmatrix} \begin{bmatrix} a_0 \\ a_1 \\ a_2 \end{bmatrix} = \begin{bmatrix} \overrightarrow{e_0}' & \overrightarrow{e_1}'&\overrightarrow{e_2}' \end{bmatrix} \begin{bmatrix} a_0' \\ a_1' \\ a_2' \end{bmatrix} \space \space \space \space (1)
[e0e1e2]⎣⎡a0a1a2⎦⎤=[e0′e1′e2′]⎣⎡a0′a1′a2′⎦⎤ (1)
其中 a → \overrightarrow{a} a在 E = [ e 0 → e 1 → e 2 → ] E=\begin{bmatrix} \overrightarrow{e_0} & \overrightarrow{e_1}&\overrightarrow{e_2} \end{bmatrix} E=[e0e1e2]坐标系下的坐标; a → ′ \overrightarrow{a}' a′在 E ′ = [ e 0 → ′ e 1 → ′ e 2 → ′ ] E'=\begin{bmatrix} \overrightarrow{e_0}' & \overrightarrow{e_1}'&\overrightarrow{e_2}' \end{bmatrix} E′=[e0′e1′e2′] 坐标系下的坐标。
对
1
1
1公式左乘
E
E
E:
1)因为
E
E
E是一组向量基组成的向量,因此
E
T
E
=
I
E^TE=I
ETE=I为单位矩阵。因此左边的为
[
a
0
a
1
a
2
]
\begin{bmatrix} a_0 \\ a_1 \\ a_2 \end{bmatrix}
⎣⎡a0a1a2⎦⎤(因为
e
0
→
T
e
1
→
\overrightarrow{e_0}^T\overrightarrow{e_1}
e0Te1=0),通过计算容易得到。
2)因为1)属性,得到如下:
[
a
0
a
1
a
2
]
=
[
e
0
→
T
e
0
→
′
e
0
→
T
e
1
→
′
e
0
→
T
e
2
→
′
e
1
→
T
e
0
→
′
e
1
→
T
e
1
→
′
e
1
→
T
e
2
→
′
e
2
→
T
e
0
→
′
e
2
→
T
e
1
→
′
e
2
→
T
e
2
→
′
]
[
a
0
′
a
1
′
a
2
′
]
=
R
[
a
0
′
a
1
′
a
2
′
]
(
2
)
\begin{bmatrix} a_0 \\ a_1 \\ a_2 \end{bmatrix} =\begin{bmatrix} \overrightarrow{e_0}^T\overrightarrow{e_0}' &\overrightarrow{e_0}^T\overrightarrow{e_1}' & \overrightarrow{e_0}^T\overrightarrow{e_2}' \\ \overrightarrow{e_1}^T\overrightarrow{e_0}' &\overrightarrow{e_1}^T\overrightarrow{e_1}' & \overrightarrow{e_1}^T\overrightarrow{e_2}' \\ \overrightarrow{e_2}^T\overrightarrow{e_0}' &\overrightarrow{e_2}^T\overrightarrow{e_1}' & \overrightarrow{e_2}^T\overrightarrow{e_2}' \end{bmatrix} \begin{bmatrix} a_0' \\ a_1' \\ a_2' \end{bmatrix} = \bold{R} \begin{bmatrix} a_0' \\ a_1' \\ a_2' \end{bmatrix} \space \space \space \space (2)
⎣⎡a0a1a2⎦⎤=⎣⎢⎡e0Te0′e1Te0′e2Te0′e0Te1′e1Te1′e2Te1′e0Te2′e1Te2′e2Te2′⎦⎥⎤⎣⎡a0′a1′a2′⎦⎤=R⎣⎡a0′a1′a2′⎦⎤ (2)
因此得到
R
=
[
e
0
→
T
e
0
→
′
e
0
→
T
e
1
→
′
e
0
→
T
e
2
→
′
e
1
→
T
e
0
→
′
e
1
→
T
e
1
→
′
e
1
→
T
e
2
→
′
e
2
→
T
e
0
→
′
e
2
→
T
e
1
→
′
e
2
→
T
e
2
→
′
]
\bold{R} =\begin{bmatrix} \overrightarrow{e_0}^T\overrightarrow{e_0}' &\overrightarrow{e_0}^T\overrightarrow{e_1}' & \overrightarrow{e_0}^T\overrightarrow{e_2}' \\ \overrightarrow{e_1}^T\overrightarrow{e_0}' &\overrightarrow{e_1}^T\overrightarrow{e_1}' & \overrightarrow{e_1}^T\overrightarrow{e_2}' \\ \overrightarrow{e_2}^T\overrightarrow{e_0}' &\overrightarrow{e_2}^T\overrightarrow{e_1}' & \overrightarrow{e_2}^T\overrightarrow{e_2}' \end{bmatrix}
R=⎣⎢⎡e0Te0′e1Te0′e2Te0′e0Te1′e1Te1′e2Te1′e0Te2′e1Te2′e2Te2′⎦⎥⎤
1:
R
\bold{R}
R它是一个单位正交矩阵,因为
E
,
E
′
E,E'
E,E′都是单位正交矩阵,可以自己计算,两两向量相乘,非常容易得到。这类的矩阵也是逆等于它的转置。
2:
R
\bold{R}
R的秩为
1
1
1
这类可以表达为数学形式为:
S
O
(
n
)
=
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
(
3
)
SO(n)={\bold{R} \in R^{n\times n} | \bold{R} \bold{R} ^T=I,det(\bold{R} )=1} \space \space \space \space (3)
SO(n)=R∈Rn×n∣RRT=I,det(R)=1 (3)
公式 ( 3 ) (3) (3)称这类旋转矩阵为特殊正交群(群的概念第 4 4 4)章将介绍。
因为在空间坐标系下
n
=
3
n=3
n=3。满足以上公式的
3
×
3
3\times3
3×3为空间坐标系下的旋转矩阵。
n
=
3
n=3
n=3代入上述公式
(
3
)
(3)
(3)得到三维空间的旋转矩阵,也是slam中的基本概念
S
O
(
3
)
SO(3)
SO(3):
S
O
(
3
)
=
R
∈
R
3
×
3
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
(
3
)
SO(3)={\bold{R} \in R^{3\times 3} | \bold{R} \bold{R} ^T=I,det(\bold{R} )=1} \space \space \space \space (3)
SO(3)=R∈R3×3∣RRT=I,det(R)=1 (3)
它有如下几种性质:
假设:
a
1
=
R
12
a
2
a_1=\bold{R}_{12}a_2
a1=R12a2;
a
2
=
R
21
a
1
a_2=\bold{R}_{21}a_1
a2=R21a1,其中
R
12
,
R
21
\bold{R}_{12},\bold{R}_{21}
R12,R21在两个坐标系的相互转化的旋转矩阵。它们的关系为:
R
12
=
R
21
−
1
=
R
21
T
(
4
)
\bold{R}_{12}=\bold{R}_{21}^{-1}=\bold{R}_{21}^{T} \space \space \space \space (4)
R12=R21−1=R21T (4)
公式 4 4 4非常容易得到,因为它是它是一个正交矩阵,通过公式可以得到,以后证明,很简单,直接相乘就可以了,相乘后除了对角线为1,其它都为0。
2)平移:
这个简单,暂且省略。它表达为 t t t
3)旋转加平移(后续 n = 3 n=3 n=3):
基于上述的先旋转后平移得到公式如下:
a
→
′
=
R
a
→
+
t
(
5
)
\overrightarrow{a}'= \bold{R}\overrightarrow{a} +t \space \space \space \space (5)
a′=Ra+t (5)
对于多次坐标系转化,上述方式写法,不容易写,因此它们统一到矩阵表达上,采用了齐次坐标的表达方式。如下:
[
a
→
′
1
]
=
[
R
t
0
T
1
]
[
a
→
1
]
=
T
[
a
→
1
]
(
6
)
\begin{bmatrix} \overrightarrow{a}' \\ 1 \end{bmatrix} = \begin{bmatrix} \bold{R} & t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} \overrightarrow{a} \\ 1 \end{bmatrix} = \bold{T}\begin{bmatrix} \overrightarrow{a} \\ 1 \end{bmatrix} \space \space \space \space (6)
[a′1]=[R0Tt1][a1]=T[a1] (6)
因此多次坐标系的转化可以表达为:
b
→
=
T
1
a
→
,
c
→
=
T
2
b
→
=
>
c
→
=
T
2
T
1
a
→
(
7
)
\overrightarrow{b}= \bold{T}_{1}\overrightarrow{a}, \overrightarrow{c}= \bold{T}_{2}\overrightarrow{b} => \overrightarrow{c}=\bold{T}_{2}\bold{T}_{1} \overrightarrow{a} \space \space \space \space (7)
b=T1a,c=T2b=>c=T2T1a (7)
这类引入齐次坐标系的旋转平移向量,用统一代数表达为:
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
T
∈
R
3
}
(
8
)
SE(3) = \begin{Bmatrix} \bold{T} = \begin{bmatrix} \bold{R} & t \\ 0^T & 1 \end{bmatrix} \in R^{4 \times 4} | \bold{R} \in SO(3), T \in R^3 \end{Bmatrix} \space \space \space \space (8)
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),T∈R3} (8)
公式
8
8
8可以得到
T
\bold{T}
T,这类矩阵也被称为特殊欧式群。它的逆变化为:
T
−
1
=
[
R
T
−
R
T
t
0
T
1
]
(
9
)
\bold{T}^{-1} = \begin{bmatrix} \bold{R}^T & -\bold{R}^T t \\ 0^T & 1 \end{bmatrix} \space \space \space \space (9)
T−1=[RT0T−RTt1] (9)
4)角轴
上面介绍的理论知识非常重要。但是它都是基于矩阵的表达形式,它和矩阵表达的意义等价的。下面介绍另一种表达形式(角轴)。
在真实的世界中,向量也可以表示旋转矩阵,它比上面的矩阵表示更为紧凑。如下图,给出了物理意义上的向量表示旋转。
从上图可以看到旋转可以表示为绕着一根轴旋转一定的角度。使用
W
=
θ
n
→
W=\theta \overrightarrow{n}
W=θn表示。其中
θ
\theta
θ为角度,
n
→
\overrightarrow{n}
n表示旋转轴。可以见我之前写的blog,(我使用的是
u
→
\overrightarrow{u}
u表示,这里使用
n
→
\overrightarrow{n}
n表示,其实意思是一样的)。
https://blog.csdn.net/weixin_43851636/article/details/126004987?spm=1001.2014.3001.5502
它计算罗德里公式:
R
=
I
c
o
s
θ
+
(
1
−
c
o
s
θ
)
n
→
n
→
T
+
n
→
×
s
i
n
θ
(
10
)
\bold{R}=Icos\theta+(1-cos\theta)\overrightarrow{n}\overrightarrow{n}^T+\overrightarrow{n} \times sin\theta \space \space \space \space (10)
R=Icosθ+(1−cosθ)nnT+n×sinθ (10)
它的逆变化,从
R
\bold{R}
R到旋转角,和角轴
θ
,
n
→
\theta, \overrightarrow{n}
θ,n为(那篇blog后续补上这个推导):
角度:
θ
=
a
r
c
c
o
s
(
t
r
(
R
)
−
1
2
)
(
11
)
\theta = arccos(\cfrac{tr(\bold{R})-1}{2}) \space \space \space \space (11)
θ=arccos(2tr(R)−1) (11)
轴:
R
n
→
=
n
→
(
12
)
\bold{R}\overrightarrow{n}=\overrightarrow{n} \space \space \space \space (12)
Rn=n (12)
上述的表达,可以看到旋转,可以按照某种顺序旋转。如果按照
Z
−
Y
−
X
Z-Y-X
Z−Y−X的顺序旋转物体。
1)绕着
Z
Z
Z轴旋转,得到的是偏航角yaw。
2)绕着
Y
Y
Y轴旋转,得到是俯仰角pitch。
3)绕着
x
x
x轴旋转,得到是翻滚角roll。
它符合人的视角,但是在计算不好。还有一些缺陷。比如旋转顺序,还有万向锁的问题等等,用的不多,所以简略记录。
四元素
上述的欧拉角具有的缺点,四元素不具有,它具有广泛性,且紧凑性,在工程中常常遇到。它也是可以表达旋转矩阵。
四元素定义:一种扩展的复数,
(二维的)可以表达平面旋转。
表达为:
q
=
q
0
+
q
1
i
→
+
q
2
j
→
+
q
3
k
→
(
13
)
{
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
→
\bold{q}=q_0 + q_1 \overrightarrow{i} + q_2 \overrightarrow{j}+ q_3 \overrightarrow{k} \space \space \space (13) \\ \begin{cases} \overrightarrow{i}^2 = \overrightarrow{j}^2 = \overrightarrow{k}^2 = -1 \\ \overrightarrow{i} \overrightarrow{j}= \overrightarrow{k}, \overrightarrow{j} \overrightarrow{i} = -\overrightarrow{k} \\ \overrightarrow{j} \overrightarrow{k}= \overrightarrow{i}, \overrightarrow{k} \overrightarrow{j} = -\overrightarrow{i} \\ \overrightarrow{k} \overrightarrow{i}= \overrightarrow{j}, \overrightarrow{i} \overrightarrow{k} = -\overrightarrow{j} \end{cases}
q=q0+q1i+q2j+q3k (13)⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧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
(
14
)
{
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
→
\bold{q}=[s, \overrightarrow{v}], s=q_0 \in R, \overrightarrow{v} = [q_1, q_2, q_3 ]^T \in R^3 \space \space \space (14) \\ \begin{cases} \overrightarrow{i}^2 = \overrightarrow{j}^2 = \overrightarrow{k}^2 = -1 \\ \overrightarrow{i} \overrightarrow{j}= \overrightarrow{k}, \overrightarrow{j} \overrightarrow{i} = -\overrightarrow{k} \\ \overrightarrow{j} \overrightarrow{k}= \overrightarrow{i}, \overrightarrow{k} \overrightarrow{j} = -\overrightarrow{i} \\ \overrightarrow{k} \overrightarrow{i}= \overrightarrow{j}, \overrightarrow{i} \overrightarrow{k} = -\overrightarrow{j} \end{cases}
q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3 (14)⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
它的虚部的向量基和相机坐标系中的右手坐标系一样。
这些四元素的基本运算如下,它是基本的运算规则:
这些可以手动运算一下验证,这些都是基本的代数运算规则。
基于四元素的定义,它怎么表示三维旋转?
答:
直观上可以得到从角轴到四元素(形式转换):
q
=
[
c
o
s
θ
2
n
x
s
i
n
θ
2
n
y
s
i
n
θ
2
n
z
s
i
n
θ
2
]
\bold{q}=\begin{bmatrix} cos \cfrac{\theta}{2} & n_x sin \cfrac{\theta}{2} & n_y sin \cfrac{\theta}{2} & n_z sin \cfrac{\theta}{2} \end{bmatrix}
q=[cos2θnxsin2θnysin2θnzsin2θ]
四元素到角轴:
{
θ
=
2
a
r
c
c
o
s
q
0
[
n
x
n
y
n
z
]
T
=
[
q
1
q
2
q
3
]
T
/
s
i
n
θ
2
\begin{cases} \theta=2arccos q_0 \\ \begin{bmatrix} n_x & n_y & n_z \end{bmatrix} ^T= \begin{bmatrix} q_1 & q_2 & q_3 \end{bmatrix} ^T / sin \cfrac{\theta}{2} \end{cases}
⎩⎨⎧θ=2arccosq0[nxnynz]T=[q1q2q3]T/sin2θ
以上是和欧拉角进行互算,但是基于物理意义上,怎么利用四元素对空间的一个点进行旋转?
假设一个顶点
p
=
[
0
x
y
z
]
p=\begin{bmatrix} 0 & x & y & z \end{bmatrix}
p=[0xyz] 经过
q
\bold{q}
q旋转后得到
p
′
p'
p′,这类关系怎么用代数表达?
旋转后得到如下:
p
′
=
q
p
q
−
1
p'=\bold{q}p\bold{q}^{-1}
p′=qpq−1
它是先转到四维空间(第一维不为0),然后再转回三维空间。
实践部分,后面补充。