《视觉SLAM十四讲》学习系列（2）—三维空间刚体运动

本文主要内容来自《视觉SLAM十四讲》第三讲-三维空间刚体运动。介绍SLAM中的一个基本问题：刚体在三维空间中的运动如何描述。在书中介绍了四种方法：旋转矩阵、旋转向量、欧拉角和四元数。

基础数学知识

标准正交基

在线性代数中，一个内积空间的正交基（orthogonal basis）是元素两两正交的基。称基中的元素为基向量。假若，一个正交基的基向量的模长都是单位长度1，则称这正交基为标准正交基。

反对称矩阵

对于三维空间中的向量a,b，外积可用来描述a到b之间是如何选择的，其中外积的方向即为旋转矩阵的方向。外积也可写成：
a×b=⎡⎣⎢ia1b1ja2b2ka3b3⎤⎦⎥=⎡⎣⎢0a3−a2−a30a1a2−a10⎤⎦⎥b(2)
将上述过程记为a^b，其中^为反对称符号。将外积a×b转为a^b，在运算时即可转为矩阵运算，有利于提高计算机处理效率，类似情况在接下来的部分中还将多次出现。

正交矩阵

正交矩阵即逆为自身转置的矩阵。

旋转矩阵

坐标系间的欧氏变换

相机运动时一个刚体运动，即同一个在各个坐标系下的长度和夹角都不会发生变化，这类变换称为欧式变换。这样的欧式变换由一个旋转和一个平移组成。设某个单位正交基(e1,e2,e3)经过一次旋转后变成了(e′′1,e′′2,e′′3)，同一向量a的坐标有以下关系：

[e1,e2,e3]⎡⎣⎢a1a2a3⎤⎦⎥=[e′′1,e′′2,e′′3]⎡⎣⎢a′′1a′′2a′′3⎤⎦⎥

左乘[e1,e2,e3]T后有

⎡⎣⎢⎢eT1e1eT2e1eT3e1eT1e2eT2e2eT3e2eT1e3eT2e3eT3e3⎤⎦⎥⎥⎡⎣⎢a1a2a3⎤⎦⎥=⎡⎣⎢⎢eT1e′′1eT2e′′1eT3e′′1eT1e′′2eT2e′′2eT3e′′2eT1e′′3eT2e′′3eT3e′′3⎤⎦⎥⎥⎡⎣⎢a′′1a′′2a′′3⎤⎦⎥

⎡⎣⎢100010001⎤⎦⎥⎡⎣⎢a1a2a3⎤⎦⎥=⎡⎣⎢⎢eT1e′′1eT2e′′1eT3e′′1eT1e′′2eT2e′′2eT3e′′2eT1e′′3eT2e′′3eT3e′′3⎤⎦⎥⎥⎡⎣⎢a′′1a′′2a′′3⎤⎦⎥

⎡⎣⎢a1a2a3⎤⎦⎥=⎡⎣⎢⎢eT1e′′1eT2e′′1eT3e′′1eT1e′′2eT2e′′2eT3e′′2eT1e′′3eT2e′′3eT3e′′3⎤⎦⎥⎥⎡⎣⎢a′′1a′′2a′′3⎤⎦⎥≐Ra”

我们将中间的矩阵拿出来定义为旋转矩阵R。它是一个行列式为1的正交矩阵，可以描述相机的旋转。再将平移定义为t。则有

a′′=Ra+t

变换矩阵与齐次坐标

为了便于描述，引入齐次坐标和变换矩阵，重写为：

[a′′1]=[R0t1][a1]≐T[a1]

我们在三维向量的末尾添加1，将其变为四维向量，称齐次坐标。公式中矩阵T为变换矩阵。特别的，矩阵T的逆矩阵为：

T−1=[RT0−RTt1]

旋转向量

旋转向量是旋转矩阵的另一种表达方式，使用一个三维向量来描述旋转，由旋转轴n和旋转角θ来刻画。从旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明，具体形式如下：

R=cosθI+(1−cosθ)nnT+sinθn^

从旋转矩阵求旋转角：

θ=arccos(tr(R)−12)

关于转轴n，由于旋转轴上的向量在旋转后不发生改变（欧式变换），说明

Rn=n

即转轴n是旋转矩阵R特征值1对应的特征向量。

欧拉角

欧拉角提供了一种非常直观的方式来描述旋转，它使用三个分离的转角，即将一个旋转分解成三次饶不同轴的旋转。
ZYX 转角相当于把任意旋转分解成以下三个轴上的转角 :
1、绕物体的 Z 轴旋转，得到偏航角 yaw
2、绕旋转之后的 Y 轴旋转，得到俯仰角 pitch
3、绕旋转之后的 X 轴旋转，得到滚转角 roll
但是欧拉角的一个重大缺陷是会碰到万向锁问题：在俯仰角为±90度时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失一个自由度。

（确实不好理解，笔者最终找到了直观演示视频，其中讲解比较详细，也可参考维基）

四元数

定义与基本运算

四元数是Hamilton找到的一种扩展的复数。一个四元数拥有一个实部和三个虚部（故事上说他原先找了很久带两个虚部的，结果怎么也找不到，最后豁然开朗找到了三虚部的四元数）：

q=q0+q1i+q2j+q3k

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

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j

现有两个四元数qa,qb，它们的向量表示为[sa,va],[sb,vb]，或者原始四元数表示为：

sa+xai+yaj+zak,sb+xbi+ybj+zbk.

那么，它们的运算可表示如下。

加法和减法

四元数qa,qb的加减运算为：

qa±qb=[sa±sb,va±vb].

乘法

乘法是把qa的每一项与qb每项相乘，最后相加，虚部要按照式进行：

qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zabb)j+(sazb+xayb−xbya+zasb)k

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

qaqb=[sasb−va⋅vb,savb+sbva+va×vb]

在该乘法定义下，两个实的四元数乘积仍是实的，这与复数也是一致的。然而，注意到，由于最后一项外积的存在，该乘法通常是不可交换的，除非va和vb在R3中共线。

共轭

四元数的共轭为：

q∗a=sa−xai−yaj−zak=[sa,−va]

即把虚部取成相反数。四元数共轭与自己本身相乘，会得到一个实四元数，其实部为模长的平方:

q∗q=qq∗=[s2a+vTv,0]=s2a+vTv

模长

四元数的模长定义为：

∥qa∥=s2a+x2a+y2a+z2a−−−−−−−−−−−−−−√=q∗Taqa−−−−−√

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

∥qaqb∥=∥qa∥∥qb∥

逆

一个四元数的逆为：

q−1=q∗/∥q∥2

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

qq−1=q−1q=1

同时，乘积的逆有和矩阵相似的性质：

(qaqb)−1=q−1bq−1a

对于单位四元数，即∥q∥=1，它的逆即是它的共轭四元数。

数乘与点乘

和向量相似，四元数可以与数相乘：

kq=[ks,kv]

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

qa⋅qb=sasb+xaxbi+yaybj+zazbk

用四元数表述旋转

在复数域C，我们可以用一个复数eiθ表示2D的旋转，类似的，3D空间也可以用单位四元数表示旋转。假设一个空间三维点v=[x,y,z]∈R3，以及一个由旋转轴和夹角n,θ 指定的旋转，下面讨论如何用四元数表示它们。

首先，我们把三维空间点用一个虚四元数来描述：p=[0,x,y,z]=[0,v].

然后，用另一个四元数q表示这个旋转：q=[cosθ2,nsinθ2].

那么，旋转后的点p′即可表示为这样的乘积：p′=qpq−1
可以验证，计算结果的实部为nT(n×v)=0，故计算结果为纯虚四元数。其虚部的三个分量表示旋转后3D点的坐标。

四元数到旋转矩阵的转换

设四元数q=q0+q1i+q2j+q3k，对应的旋转矩阵R为：

R=⎡⎣⎢⎢1−2q22−2q232q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q21−2q232q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q21−2q22⎤⎦⎥⎥

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

q0=tr(R)+1−−−−−−−−√2,q1=m23−m324q0,q2=m31−m134q0,q3=m12−m214q0

各种表示方式的比较

任务/性质 旋转矩阵\旋转向量 欧拉角 四元数 在坐标系间旋转点 能 不能（需转为旋转矩阵） 不能（需转为旋转矩阵） 连接或增量旋转 能 不能 能 难易程度 难 容易 难 存储内存 9个数字 3个数字 4个数字

相似、仿射、射影变换

详细性质可见下表(相似变换的不必性质为“体积比“，不是体积！)：

其中s表示缩放因子，说明相似变换在对向量旋转后，可以在x,y,z三个坐标上进行均匀缩放。
仿射变换中的矩阵A只要求为可逆矩阵，而不必是正交矩阵。
影射变换左下角为缩放aT，当v≠0时，可以用整个矩阵除以v得到右下角为1的矩阵，否则为右下角为0的矩阵。从真实世界到相机照片的变换就可以看做是仿射变换。