文章目录

参考资料
1. 旋转矩阵
- 1.1 内积、外积
- 1.2 坐标变换
- - 1.2.1 旋转
  - 1.2.2 平移
  - 1.2.3 变换矩阵与齐次坐标
2. 旋转向量和欧拉角
- 2.1 旋转向量
- 2.2 欧拉角
3. 四元数
- 3.1 旋转向量与四元数
- 3.2 四元数的运算
- 3.3 用四元数表示旋转
- 3.4 四元数与旋转矩阵
4. 相似、仿射、射影变换
- 4.1 相似变换
- 4.2 仿射变换
- 4.3 射影变换

参考资料

高翔slam14讲

1. 旋转矩阵

1.1 内积、外积

对于 a , b ∈ R 3 a,b \in \mathbb{R}^3 a,b∈R3，内积可以写成

内积可以描述向量间的投影关系。而外积呢是这个样子:

外积的方向垂直于这两个向量,大小为 ∣ a ∣ ∣ b ∣ s i n ( a , b ) |a||b| sin (a,b) ∣a∣∣b∣sin(a,b),是两个向量张成的四边形的有向面积。对于外积,我们引入了^符号,把a写成一个矩阵，事实上是一个反对称矩阵(Skew-symmetric)。这样就把外积 a x b axb axb,写成了矩阵与向量的乘法a^b,把它变成了线性运算。这个符号将在后文经常用到,请记住它。外积只对三维向量存在定义,我们还能用外积表示向量的旋转。

为什么外积可以表示旋转呢?

考虑两个不平行的向量a,b,我们要描述从a到b之间是如何旋转的,如图3-1所示。我们可以用一个向量来描述三维空间中两个向量的旋转关系。在右手法则下,我们用右手的四个指头从a转向b,其大拇指朝向就是旋转向量的方向,事实上也是 a × b a\times b a×b的方向。它的大小则由a和b的夹角决定。通过这种方式,我们构造了从a到b的一个旋转向量。这个向量同样位于三维空间中,在此坐标系下,可以用三个实数来描述它。

1.2 坐标变换

与向量间的旋转类似,我们同样可以描述两个坐标系之间的旋转关系,再加上平移,统称为坐标系之间的变换关系。
一个欧氏变换由一个旋转和一个平移两部分组成。

1.2.1 旋转

首先来考虑旋转。我们设某个单位正交基 ( e 1 , e 2 , e 3 ) (e_1,e_2,e_3) (e1,e2,e3)经过一次旋转,变成了 ( e 1 ′ , e 2 ′ , e 3 ′ ) (e'_1, e'_2,e'_3) (e1′,e2′,e3′)。那么,对于同一个向量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,根据坐标的定义,有:

为了描述两个坐标之间的关系,我们对上面等式左右同时左乘 [ e 1 , e 2 , e 3 ] T [e_1,e_2,e_3]^T [e1,e2,e3]T，那么左边的系数变成了单位矩阵,所以:

我们把中间的阵拿出来,定义成一个矩阵R。这个矩阵由两组基之间的内积组成,刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的,那么这个矩阵也是一样的。可以说,矩阵R描述了旋转本身。因此它又称为旋转矩阵。

旋转矩阵有一些特别的性质。事实上,它是一个行列式为1的正交矩阵。反之,行列式为1的正交矩阵也是一个旋转矩阵。所以,我们可以把旋转矩阵的集合定义如下:

SO(n)是特殊正交群(Special Orthogonal Group)的意思。

1.2.2 平移

在欧氏变换中,除了旋转之外还有一个平移。考虑世界坐标系中的向量 a a a,经过一次旋转(用R描述)和一次平移 t t t后,得到了 a ′ a' a′,那么把旋转和平移合到一起,有：

其中, t t t称为平移向量。相比于旋转,平移部分只需把这个平移量加到旋转之后的坐标上,显得非常简洁。

1.2.3 变换矩阵与齐次坐标

式(3.8)完整地表达了欧氏空间的旋转与平移,不过还存在一个小问题:这里的变换关系不是一个线性关系。假设我们进行了两次变换: R 1 , t 1 R_1,t_1 R1,t1和 R 2 , t 2 R_2,t_2 R2,t2,满足:

这样的形式在变换多次之后会过于复杂。因此,我们要引入齐次坐标和变换矩阵重写式(3.8):

这是一个数学技巧:我们把一个三维向量的末尾添加1,变成了四维向量,称为齐次坐标。对于这个四维向量,我们可以把旋转和平移写在一个矩阵里面,使得整个关系变成了线性关系。该式中,矩阵T称为变换矩阵(Transform Matrix)。我们暂时用 a ~ \tilde{a} a~表示a的齐次坐标。

齐次坐标

它是射影几何里的概念。通过添加最后一维,我们用四个实数描述了一个三维向量,这显然多了一个自由度,但允许我们把变换写成线性的形式。在齐次坐标中,某个点 x x x的每个分量同乘一个非零常数k后,仍然表示的是同一个点。因此,一个点的具体坐标值不是唯一的。如 [ 1 , 1 , 1 , 1 ] T [1,1,1,1]^T [1,1,1,1]T和 [ 2 , 2 , 2 , 2 ] T [2,2,2,2]^T [2,2,2,2]T是同一个点。但当最后一项不为零时,我们总可以把所有坐标除以最后一项,强制最后一项为1,从而得到一个点唯一的坐标表示(也就是转换成非齐次坐标):

这时,忽略掉最后一项,这个点的坐标和欧氏空间就是一样的。依靠齐次坐标和变换矩阵,两次变换的累加就可以有很好的形式:

变换矩阵
关于变换矩阵T,它具有比较特别的结构:左上角为旋转矩阵,右侧为平移向量,左下角为0向量,右下角为1。这种矩阵又称为特殊欧氏群(Special Euclidean Group):

与SO(3)一样,求解该矩阵的逆表示一个反向的变换:

最后,为了保持符号的简洁,在不引起歧义的情况下,我们以后不区别齐次坐标与普通的坐标的符号,默认我们使用的是符合运算法则的那一种。

回顾一下我们介绍的内容:首先,我们说了向量和它的坐标表示,并介绍了向量间的运算;然后,坐标系之间的运动由欧氏变换描述,它由平移和旋转组成。旋转可以由旋转矩阵 S O ( 3 ) SO(3) SO(3)描述,而平移直接由一个 R 3 \mathbb{R}^3 R3向量描述。最后,如果将平移和旋转放在一个矩阵中,就形成了变换矩阵 S E ( 3 ) SE(3) SE(3)。

Eigen是一个C++开源线性代数库。它提供了快速的有关矩阵的线性代数运算,还
包括解方程等功能。许多上层的软件库也使用Eigen进行矩阵运算,包括g2o,Sophus等。可自行学习

2. 旋转向量和欧拉角

2.1 旋转向量

旋转矩阵表示方式至少有以下几个缺点:

SO(3)的旋转矩阵有九个量,但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理,变换矩阵用十六个量表达了六自由度的变换。
旋转矩阵自身带有约束：它必须是个正交矩阵,且行列式为1。变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时,这些约束会使得求解变得更困难。

任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是,我们可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量,称为旋转向量(或轴角, Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。同样,对于变换矩阵,我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。

旋转向量和旋转矩阵之间是如何转换的呢?
假设有一个旋转轴为 n n n,角度为 θ \theta θ的旋转,显然,它对应的旋转向量为 θ n \theta n θn。由旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula)表明，由于推导过程比较复杂,我们不作描述,只给出转换的结果:

符号KaTeX parse error: Expected group after '^' at position 1: ^̲是向量到反对称的转换符,见式(3.3)。反之,我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角 θ \theta θ有:

因此,转轴 n n n是矩阵 R R R特征值1对应的特征向量。求解此方程,再归一化,就得到了旋转轴。读者也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。

2.2 欧拉角

无论是旋转矩阵、旋转向量,虽然它们能描述旋转,但对我们人类是非常不直观的。
当我们看到一个旋转矩阵或旋转向量时,很难想象出来这个旋转究竟是什么样的。当它们变换时,我们也不知道物体是向哪个方向在转动。而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了三个分离的转角,把一个旋转分解成三次绕不同轴的旋转。
当然,由于分解方式有许多种,所以欧拉角也存在着不同的定义方法。比如说,当我先绕X轴旋转,再绕Y轴,最后绕Z轴,就得到了一个XYZ轴的旋转。同理,可以定义,ZYZ、 ZYX等等旋转方式。
欧拉角当中比较常用的一种,便是用“偏航-俯仰-滚转”(yaw-pitch-roll)三个角度来描述一个旋转的。由于它等价于ZYX轴的旋转,我们就以ZYX为例。假设一个刚体的前方(朝向我们的方向)为X轴,右侧为Y轴,上方为Z轴,见图3-3。那么,ZYX转角相当于把任意旋转分解成以下三个轴上的转角:

绕物体的Z轴旋转,得到偏航角yaw；
绕旋转之后的Y轴旋转,得到俯仰角pitch；
绕旋转之后的X轴旋转,得到滚转角roll。

欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock):在俯仰角为 ± 90 ° \pm90° ±90°时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由三次旋转变成了两次旋转)。这被称为奇异性问题,在其他形式的欧拉角中也同样存在。

3. 四元数

旋转矩阵用九个量描述三自由度的旋转,具有冗余性；

欧拉角和旋转向量是紧凑的，但具有奇异性。

事实上,我们找不到不带奇异性的三维向量描述方式。这有点类似于,当我们想用两个坐标表示地球表面时(如经度和纬度),必定存在奇异性(纬度为 ± 90 ° \pm90° ±90°时经度无意义)。

三维旋转是一个三维流形,想要无奇异性地表达它,用三个量是不够的。

回忆我们以前学习过的复数。我们用复数集C表示复平面上的向量,而复数的乘法则能表示复平面上的旋转:例如,乘上复数i相当于逆时针把一个复向量旋转90度。类似的,在表达三维空间旋转时,也有一种类似于复数的代数:四元数(Quaternion)。四元数是Hamilton找到的一种扩展的复数.它既是紧凑的,也没有奇异性。如果说缺点的话,四元数不够直观,其运算稍为复杂一些。

一个四元数q拥有一个实部和三个虚部：

其中 i , j , k i,j,k i,j,k为四元数的三个虚部。这三个虚部满足关系式:

由于它的这种特殊表示形式,有时人们也用一个标量和一个向量来表达四元数:

这里, s s s称为四元数的实部,而 v v v称为它的虚部。如果一个四元数虚部为0,称之为实四元数。反之,若它的实部为0,称之为虚四元数。

3.1 旋转向量与四元数

假设某个旋转是绕单位向量 n = [ n x , n y , n z ] T n = [n_x,n_y,n_z]^T n=[nx,ny,nz]T进行了角度为0的旋转,那么这个旋转的四元数形式为:

反之,我们亦可从单位四元数中计算出对应旋转轴与夹角:

这式子给我们一种微妙的“转了一半”的感觉。同样,对式(3.19)的0加上2m,我们得到一个相同的旋转,但此时对应的四元数变成了 − q -q −q。因此,在四元数中,任意的旋转都可以由两个互为相反数的四元数表示。同理,取 θ \theta θ为0,则得到一个没有任何旋转的实四元数:

3.2 四元数的运算

四元数和通常复数一样,可以进行一系列的运算。常见的有四则运算、数乘、求逆、共轭等等。
现有两个四元数 q a , q b q_a,q_b qa,qb,它们的向量表示为 [ s a , v a ] , [ s b , v b ] [s_a,v_a], [s_b,v_b] [sa,va],[sb,vb],或者原始四元数表示为:

加法和减法
四元数 q a , q b q_a,q_b qa,qb的加减运算为:
乘法
乘法是把 q a q_a qa的每一项与 q b q_b qb每项相乘,最后相加,虚部要按照式(3.18)进行。

虽然稍为复杂,但形式上是整齐有序的。如果写成向量形式并利用内外积运算,该表达会更加简洁:

在该乘法定义下,两个实的四元数乘积仍是实的,这与复数也是一致的。然而,注意到,由于最后一项外积的存在,四元数乘法通常是不可交换的,除非 v a v_a va和 v b v_b vb在 R 3 \mathbb{R}^3 R3中共线,那么外积项为零。
共轭
四元数的共轭是把虚部取成相反数:

四元数共轭与自己本身相乘,会得到一个实四元数,其实部为模长的平方:
模长
四元数的模长定义为:

可以验证,两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四元数。
逆
一个四元数的逆为:

按此定义,四元数和自己的逆的乘积为实四元数的1:

如果q为单位四元数,逆和共轭就是同一个量。同时,乘积的逆有和矩阵相似的性质：
数乘与点乘
和向量相似,四元数可以与数相乘:

点乘是指两个四元数每个位置上的数值分别相乘:

3.3 用四元数表示旋转

我们可以用四元数表达对一个点的旋转。假设一个空间三维点 p = [ r , y , z ] ∈ R 3 p=[r,y,z] \in \mathbb{R}^3 p=[r,y,z]∈R3,以及一个由轴角 n , θ n,\theta n,θ指定的旋转。三维点 p p p经过旋转之后变成为 p ′ p' p′。如果使用矩阵描述，那么有 p ′ = R p p'=Rp p′=Rp。如果用四元数描述旋转,它们的关系如何来表达呢?

首先,把三维空间点用一个虚四元数来描述:

这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后,参照式(3.19),用四元数 q q q表示这个旋转:

那么,旋转后的点 p ′ p' p′即可表示为这样的乘积:

可以验证，计算结果的实部为0,故为纯虚四元数。其虚部的三个分量表示旋转后3D点的坐标

3.4 四元数与旋转矩阵

设四元数 q = q 0 + q 1 i + q 2 j + q 3 k q=q_0+q_1i+q_2j+q_3k q=q0+q1i+q2j+q3k,对应的旋转矩阵R为:

反之,由旋转矩阵到四元数的转换如下。假设矩阵为 R = m i j ， i , j ∈ [ 1 , 2 , 3 ] R= {m_{ij}}，i,j\in [1,2,3] R=mij，i,j∈[1,2,3],其对应的四元数 q q q由下式给出:

值得一提的是,由于 q q q和 − q -q −q表示同一个旋转,事实上一个R对应的四元数表示并不是惟一的。同时,除了上面给出的转换方式之外,还存在其他几种计算方法,这里不再赘述。

4. 相似、仿射、射影变换

4.1 相似变换

相似变换比欧氏变换多了一个自由度,它允许物体进行均匀的缩放,其矩阵表示为:

注意到旋转部分多了一个缩放因子s,表示我们在对向量旋转之后,可以在 x , y , z x,y,z x,y,z三个坐标上进行均匀的缩放。由于含有缩放,相似变换不再保持图形的面积不变。可以想象一个边长为1的立方体通过相似变换后,变成边长为10的样子(但仍然是立方体)。

4.2 仿射变换

仿射变换矩阵形式如下：

与欧氏变换不同的是,仿射变换只要求A是一个可逆矩阵,而不必是正交矩阵。仿射变换也叫正交投影。经过仿射变换之后,立方体就不再是方的了,但是各个面仍然是平行四边形。

4.3 射影变换

射影变换是最一般的变换，矩阵形式为：

slam14讲 |二、三维空间刚体运动相关推荐

深蓝学院-视觉SLAM课程-第2讲笔记--三维空间刚体运动
课程Github地址:https://github.com/wrk666/VSLAM-Course/tree/master 0. 内容 C++没有矩阵的运算,用别人的库来进行矩阵运算,其中Eigen库 ...
【视觉SLAM十四讲】三维空间刚体运动
本文为视觉 SLAM 学习总结.第三讲讲解的是观测方程中的 x x x 是什么. 本讲内容概要三维空间的刚体运动的描述方式:旋转矩阵.变换矩阵.四元数和欧拉角 Eigen 库的矩阵.几何模块的使用方 ...
视觉SLAM十四讲 ch3 (三维空间刚体运动)笔记
本讲目标 ●理解三维空间的刚体运动描述方式:旋转矩阵.变换矩阵.四元数和欧拉角. ●学握Eigen库的矩阵.几何模块使用方法. 旋转矩阵.变换矩阵向量外积向量外积(又称叉积或向量积)是一种重要的向 ...
SLAM十四讲-(3)三维空间刚体运动
描述刚体在三维空间的运动:一次旋转加一次平移. 旋转矩阵坐标系间的欧式变换对于刚体运动,可以表示为一次旋转加一次平移运动.因此,可以定义一个物体的刚体运动表示为: [ a 1 ′ a 2 ′ ] ...
视觉SLAM十四讲：第3讲三维空间刚体运动
第3讲:三维空间刚体运动三维空间中刚体运动的描述方式:旋转矩阵.变换矩阵.四元数和欧拉角 3.1 旋转矩阵 3.1.1 点和向量,坐标系三维空间中,给定线性空间基(e1,e2,e3)(\mathb ...
高博SLAM十四讲书本程序学习——第3讲三维空间刚体运动
小白高博SLAM十四讲书本程序学习_1 第3讲三维空间刚体运动在高博原始注释上,针对我自己不明白的部分,做额外注释如果有错误的地方,请大家指点指点博文目录一.P.48 eigenMatrix ...
视觉SLAM十四讲第3讲三维空间刚体运动（相关知识点汇总）
视觉SLAM十四讲第3讲三维空间刚体运动 1. 刚体 2. 欧氏空间(euclidean space) 2.1 欧氏距离: 2.2 欧氏变换: 3. 笛卡尔坐标系 4. 透视空间 5. 齐次坐标系 ...
高博14讲--第三讲三维空间刚体运动
高博14讲--第三讲三维空间刚体运动旋转矩阵点和向量.坐标系坐标系间的欧式变换变换矩阵与齐次坐标旋转向量和欧拉角旋转向量欧拉角四元数四元数的定义四元数的运算用四元数表示旋转四 ...
三维空间刚体运动5：详解SLAM中显示机器人运动轨迹及相机位姿（原理流程）
三维空间刚体运动5:详解SLAM中显示机器人运动轨迹及相机位姿(原理流程) 一.显示运动轨迹原理讲解二.前期准备三.git管理子模块及克隆源代码 1.学习使用Git Submodule 2.克隆源 ...
三维空间刚体运动4-3：四元数线性插值方法：Squad
三维空间刚体运动4-3:四元数线性插值方法:Squad Squad的引出 B e ˊ z i e r c u r v e B\acute{e}zier \space curveB e ˊ zier c ...

slam14讲 |二、三维空间刚体运动