视觉slam十四讲学习笔记——第三章 三维空间刚体运动
3.1 旋转矩阵
3.1.1 点、向量和坐标系
- 一个空间点的位置可以由三个坐标指定。对于刚体而言,不仅有位置,还应该有姿态。相机也可以看成三维空间的刚体,位置是指相机在空间中的哪个地方,姿态是指相机的朝向。
- 点和向量构成空间中最基本的元素,我们规定坐标系中的点我们可用以原点为起点、该点为终点的向量描述。
- 向量和坐标是两个概念;向量是在空间中客观存在的定值,在一个给定的坐标系中,其坐标值才有意义。
- 三维空间中的某个向量的坐标可以用 R 3 \R^3 R3 当中的三个数来描述。某个点的坐标也可以用 R 3 \R^3 R3来描述。
- 描述方法:当确定了一个坐标系后,即可以确定一个线性空间的基 (
e
1
\bm{e_1}
e1,
e
2
\bm{e_2}
e2,
e
3
\bm{e_3}
e3)。例如,向量
a
\bm{a}
a在上述坐标系中的形式为:
- 坐标系通常由三个正交的坐标轴组成。
- 给定 x \bm{x} x 和 y \bm{y} y 轴时, z \bm{z} z 就可以通过右手(或左手)法则由 x × y \bm{x × y} x×y 定义出来。
- 根据定义方式的不同,坐标系又分为左手系和右手系。左手系的第三个轴与右手系相反。通常用右手系。
- 向量的重要计算:
- 内积:描述了向量间的投影关系
- 外积:外积只对三维向量存在定义,外积的方向垂直于这两个向量,大小为
∣
a
∣
∣
a
∣
s
i
n
⟨
a
,
b
⟩
\left|\bm{a}\right|\left|\bm{a}\right| sin ⟨\bm{a},\bm{b}⟩
∣a∣∣a∣sin⟨a,b⟩,是两个向量张成的四边形的有向面积。
引入了 ^ 符号,把 a \bm{a} a 写成一个矩阵,^ 是反对称符号, a \bm{a} a^ 是反对称矩阵。
3.1.2 坐标系中的欧式变换
- 旋转关系加上平移关系统称为坐标系之间的变换关系。
- 我们用两个参考系考察机器人运动,一个是固定不动的世界坐标系,一个是依靠机器人自身建立的移动坐标系,我们研究的问题就是如何把一个向量在这两个坐标系中相互转换。
- 转换思路:需要先得到该点针对机器人坐标系坐标值,再根据机器人位姿转换到世界坐标系中。
- 欧式变换:同一个向量在各个坐标系下的长度和夹角都不会发生变化的变换。一个欧氏变换由一个旋转和一个平移两部分组成。
- 设某个单位正交基
(
e
1
,
e
2
,
e
3
)
(\bm{e_1}, \bm{e_2}, \bm{e_3})
(e1,e2,e3) 经过一次旋转,变成了
(
e
1
′
,
e
2
′
,
e
3
′
)
(\bm{e_1'}, \bm{e_2'}, \bm{e_3'})
(e1′,e2′,e3′) 。对于同一个向量
a
\bm{a}
a(注意该向量并没有随着坐标系的旋转而发生运动),它在两个坐标系下的坐标为
[
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。根据坐标的定义,有:
所以
- 上述的
R
\bm{R}
R被称为旋转矩阵:矩阵由两组基之间的内积组成,刻
画了旋转前后同一个向量的坐标变换关系。 - 旋转矩阵的性质:
- 行列式为1
- 正交阵
- 对于同一个旋转变化,对应唯一的旋转矩阵
- 旋转矩阵的充要条件:行列式为 1 的正交矩阵
- 集合定义: S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ( R ) = 1 } SO(n) = \left\{\bm{R}\in \R^{n×n} | \bm{RR^T} = \bm{I}, \det(\bm{R}) = 1\right\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
- 旋转矩阵为正交阵,它的逆(即转置)描述了一个相反的旋转
- 考虑平移,设平移向量为
t
\bm{t}
t ,旋转矩阵为
R
\bm{R}
R ,则
3.1.3 变换矩阵与齐次坐标
- 齐次坐标:射影几何里的概念,把一个三维向量的末尾添加 1,变成了四维向量,称为齐次坐标。
- 变换矩阵
T
\bm{T}
T:4 × 4矩阵,左上角是 3 × 3 旋转矩阵
R
\bm{R}
R ,下面是3 × 1零向量,右面是1 × 3平移向量
t
\bm{t}
t ,右下角是1。用
a
~
\bm{\tilde{a}}
a~ 表示
a
\bm{a}
a 的齐次坐标。
- 在齐次坐标中,某个点 x 的每个分量同乘一个非零常数 k 后, 仍然表示的是同一个点,因此,一个点的具体坐标值不是唯一的。当最后一项不为零时,我们总可以把所有坐标除以最后一项,强制最后一项为 1,从而得到一个点唯一的坐标表示。忽略掉最后一项,这个点的坐标和欧氏空间就是一样。
- 依靠齐次坐标和变换矩阵,两次变换的累加就可以有很好的形式。
- 特殊欧式群:左上角为旋转矩阵,右侧为平移向量,左下角为 0 向量,右下角为 1。
- 变换矩阵的逆,表示反向变换。
3.2 Eigen库的使用
参考链接:
3.3 旋转向量和欧拉角
3.3.1 旋转向量
- 旋转矩阵有9个量,但只有3个自由度;变换矩阵由16个量,但只有6个自由度,表达冗余,浪费空间。
- 任意旋转都可以用一个旋转轴和一个旋转角来刻画。
- 旋转向量(轴角,角轴):方向与旋转轴一致,长度等于旋转角的向量。
- 旋转向量有3个量和3个自由度,使用一个旋转向量和一个平移向量即可表达一次变换。
- 旋转向量和旋转向量的转换:
- 旋转向量==>旋转矩阵:罗德里格斯公式
设旋转轴向量(单位向量) n \bm{n} n ,旋转角为 θ \theta θ ,则旋转向量为 θ n \theta\bm{n} θn ,罗德里格斯公式如下:
- 旋转矩阵==>旋转向量,公式如下:
{ θ = arccos ( t r ( R ) − 1 2 ) R n = n \begin{cases}\ \theta = \arccos(\frac{tr(\bm{R})-1}{2}) \\ \ \bm{Rn} = \bm{n}\end{cases} { θ=arccos(2tr(R)−1) Rn=n
转轴 n 是矩阵 R 特征值 1 对应的特征向量,旋转轴经过旋转之后不变。
- 旋转向量==>旋转矩阵:罗德里格斯公式
3.3.2 欧拉角
- 欧拉角使用了三个分离的转角,把一个旋转分解成三次绕不同轴的旋转。
- 由于分解方式有许多种,所以欧拉角存在着不同的定义方法。而且还需要区分每次旋转是绕固定轴旋转的,还是绕旋转之后的轴旋转的。
- 欧拉角常使用“偏航-俯仰-滚转”(yaw-pitch-roll),等价于 ZY X 轴的旋转。使用 [ r , p , y ] T [r, p, y]^T [r,p,y]T 这样一个三维的向量描述任意旋转,注意是先 y y y 再 p p p 再 r r r 。
- 万向锁:在俯仰角为 ± 9 0 ◦ ±90^◦ ±90◦ 时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由三次旋转变成了两次旋转),也被称为奇异性问题。理论上可以证明,只要我们想用三个实数来表达三维旋转时,都会不可避免地碰到奇异性问题。
- 由于奇异性问题,欧拉角不适于插值和迭代,往往只用于人机交互中,也很少出现在slam中。
3.4 四元数
3.4.1 四元数的定义
- 四元数是扩展的复数,由一个实部和三个虚部构成,它是紧凑的,没有歧义性
- 虚部满足如下关系式:
- 四元数还可以表示成如下表达式:
s 称为四元数的实部,而 v \bm{v} v 称为它的虚部。如果一个四元数虚部为 0 \bm{0} 0,称之为实四元数。反之,若它的实部为 0,称之为虚四元数。 - 模长为 1 的复数,可以表示复平面上的纯旋转。
- 用单位四元数表示三维空间中任意一个旋转。设某个旋转是绕单位向量
n
=
[
n
x
,
n
y
,
n
z
]
T
n = [n_x, n_y, n_z]^T
n=[nx,ny,nz]T 进行了角度为
θ
θ
θ 的旋转,那么这个旋转的四元数形式为
- 四元素到旋转向量的转换
- 在四元数中, 任意的旋转都可以由两个互为相反数的四元数表示。
- 取 θ θ θ 为 0,则得到一个没有任何旋转的实四元数。
3.4.2 四元数的运算
设两个四元数为: q a = s a + x a i + y a j + z a k \bm{q_a} = s_a + x_ai + y_aj + z_ak qa=sa+xai+yaj+zak , q b = s b + x b i + y b j + z b k \bm{q_b} = s_b + x_bi + y_bj + z_bk qb=sb+xbi+ybj+zbk
- 加减法:
- 乘法:括号打开,按照实部和虚部分别整理
用向量和内外积形式表示:
注意四元数乘法是不可交换的,除非 v a \bm{v_a} va 和 v b \bm{v_b} vb 在 R 3 \R^3 R3 中共线(外积项为零) - 共轭:把虚部取成相反数
四元数与其共轭相乘等于模的平方:
- 模长
- 定义:各个部系数平方和开根号
- 四元数乘积的模等于模的乘积,对于单位四元数,则意味着单位四元数乘积仍为单位四元数
- 定义:各个部系数平方和开根号
- 逆
- 定义:
- 性质
- 与逆相乘可交换、与逆相乘为1
- 乘积取逆等于交换顺序取逆的乘积
- 与逆相乘可交换、与逆相乘为1
- 定义:
- 数乘和点乘
- 数乘
- 点乘
- 数乘
3.4.3 用四元数表示旋转
- 三维空间的点用虚四元数描述:设
p
=
[
x
,
y
,
z
]
∈
R
3
p = [x, y, z] \in \R^3
p=[x,y,z]∈R3 ,则
- 旋转也用四元数表示
- 则旋转后的向量为: p ′ = q p q − 1 \bm{p′} = \bm{qpq^{−1}} p′=qpq−1 ,旋转后的向量的对应四元数实部仍然为0
3.4.4 四元数到旋转矩阵的转换
- 四元数==>旋转矩阵:设四元数
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
\bm{q} = q_0 + q_1i + q_2j + q_3k
q=q0+q1i+q2j+q3k ,则
- 旋转矩阵==>四元数:设矩阵为
R
=
{
m
i
j
}
,
i
,
j
∈
[
1
,
2
,
3
]
\bm{R} = \{m_{ij}\}, i, j \in [1, 2, 3]
R={mij},i,j∈[1,2,3] ,则
- 一个 R \bm{R} R 对应的四元数表示并不惟一。
*3.5 相似、仿射、射影变换
3.5.1 相似变换
- 相似变换比欧氏变换多了一个自由度,它允许物体进行均匀的缩放,其矩阵如下:
s s s称为缩放因子,表示在对向量旋转之后,可以在 x , y , z x, y, z x,y,z 三个坐标上进行均匀的缩放 - 由于含有缩放,相似变换不再保持图形的面积不变。
3.5.2 仿射变换
- 仿射变换也称正交投影,变换矩阵如下
- 仿射变换只要求 A 是一个可逆矩阵,而不必是正交矩阵。
- 经过仿射变换之后,立方体就不再是方的了,但是各个面仍然是平行四边形。
3.5.3 射影变换
- 变换矩阵:
- 左上角为可逆矩阵 A \bm{A} A,右上为平移 t \bm{t} t ,左下缩放 a T \bm{a^T} aT。
- 2D 的射影变换一共有 8 个自由度, 3D 则共有 15 个自由度。