第三讲 三维空间刚体运动

3.0 目标

1.理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角。

2.掌握 Eigen 库的矩阵、几何模块使用方法。

3.1 旋转矩阵

3.1.1 点和向量，坐标系

三维空间中，刚体的运动可以用两个概念来表示：旋转和平移。平移比较简单一些，一般用一个表示位移的向量来表示。而旋转则有多种表示方法，例如旋转矩阵、旋转向量等等。

外积还可以表示向量的旋转，任何旋转都可以描述为：绕某个旋转轴顺时针/ 逆时针旋转多少角度.

3.1.2 坐标系间的欧氏变换

与向量间的旋转类似，我们同样可以描述两个坐标系间的旋转关系，其与平移统称为坐标系间的变换关系。常见做法是设定固定不动的一个惯性坐标系（世界坐标系），相机与机器人则是一个移动坐标系。

对于移动坐标系中的某个向量 $p$ ，其坐标为 $p_C$，在世界坐标系下其坐标为 $p_W$，要转换这两个坐标，需要通过一个矩阵 $T$ 描述。

对于刚体运动，其中的向量在不同坐标系下的模长与方向不会改变。故我们可以由旋转与平移两部分来构建这个欧氏变换。

首先考虑旋转，设某单位正交基 $[e_1,e_2,e_3]$ 旋转为 $[e_1^{\prime },e_2^{\prime },e_3^{\prime }]$ ，对于同一个向量 $a$ ，其在两个坐标系下的坐标为 $\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$ 和 $\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$，则有：
$$[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
故：
$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\triangleq \mathbf{R}{\mathbf{a}}^{\prime }$$
这里我们得到了一个矩阵 $\mathbf{R}$ ，这个矩阵由两组基的内积组成，构成了旋转前后的同一个向量的坐标变换关系。这个矩阵描述了旋转本身，也被称为旋转矩阵。

性质：行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。

故其逆描述了一个相反的旋转。

旋转矩阵集合定义如下：
$$SO(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}\mid \mathbf{R}{\mathbf{R}}^T=\mathbf{I},\det (\mathbf{R})=1\right\}$$
$SO(n)$为特殊正交群，具体解释在下一讲。这个集合由n维空间的旋转矩阵组成。

通过旋转矩阵，我们可以直接讨论两个坐标系之间的旋转变换。即：旋转矩阵可以描述相机的旋转。

至于平移，只需在旋转后直接与平移向量 t 相加即可。
$$\mathbf{a}=\mathbf{R}\mathbf{a}+\mathbf{t}$$

3.1.3 变换矩阵与齐次坐标

上面的公式虽然完整地表达了欧氏空间的旋转与平移，但其中的变换并不是一个线性关系。若我们进行了两次变换 $R_1,t_1$和$R_2,t_2$，且
$$b=R_{1}a+t_1,c=R_{2}b+t_2$$
但从$a$到$c$的变换为：
$$c=R_2(R_{1}a+t_1)+t_2$$
在变换多次后形式会变得过于复杂。故我们引入齐次坐标和变换矩阵重写式：
$$\left[\begin{array}{c}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]\triangleq \mathbf{T}\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]$$
我们在一个三维向量的末尾添加1，改写为了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵中，使其变为线性关系。该式中$T$称为变换矩阵。我们暂用$ \tilde{a}$表示$a$的齐次坐标。

在齐次坐标中，某个点$x$的每个分量同乘一个非零常数$k$后仍表示同一个点，我们可以通过把所有坐标除以最后一项得到一个点唯一的坐标表示。
$$\tilde{\mathbf{x}}=[x,y,z,w{]}^T=[x/w,y/w,z/w,1{]}^T$$
此时忽略掉最后一项，这个点的坐标与欧氏空间一样。这时变换为线性的。
$$\tilde{b}=T_1\tilde{a},\tilde{c}=T_2\tilde{b}⟹\tilde{c}=T_2{T}_1\tilde{a}$$
对于变换矩阵$T$，其左上角为旋转矩阵，右上角为平移向量，左下角为0向量，右下角为1，这种矩阵称为特殊欧式群。
$$SE(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in SO(3),\mathbf{t}\in {\mathbb{R}}^3\right\}$$
与$SO(3)$一样，求解该矩阵的逆表示反向转换。

3.2 旋转向量和欧拉角

3.2.1 旋转向量

3维变换矩阵用16个量来表达了6自由度的变换，或许有些冗余了。是否有更紧凑的表达方式？

旋转矩阵和变换矩阵本身有着约束：其必须是正交矩阵，行列式为1。故难以优化和求解。

我们知道，任何旋转都可以用一个旋转轴和一个旋转角来刻画，于是我们可以设定一个向量，其方向与旋转轴一致，长度等于旋转角，这个向量被称为旋转向量（轴角）。这种表示法只需要一个三维向量即可表示旋转。
同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换，维数刚好为六维。

这里的旋转向量即下一讲中的李代数。

旋转向量和旋转矩阵之间如何转换？

假如有一个旋转轴为$n$，角度为$\theta$的旋转，其对应的旋转向量为$\theta n$。由罗德里格斯公式可得结果：
$$\mathbf{R}=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{n}{\mathbf{n}}^T+sin\theta {\mathbf{n}}^{\wedge },\mathbf{R}\in {\mathbb{R}}^{3\times 3}$$
其中$\wedge$为向量到反对称的转换符号。反之，我们也可以计算一个旋转矩阵到旋转向量的转换，对于转角$\theta$，有：
$$\begin{array}{rl}\mathrm{tr}(R)& =\cos \theta \mathrm{tr}(I)+(1-\cos \theta )\mathrm{tr}(n{n}^T)+\sin \theta \mathrm{tr}(n^{\wedge })\\ & =3\cos \theta +(1-\cos \theta )\\ & =1+2\cos \theta .\end{array}$$
因此：
$$\theta =\arccos \left(\frac{\mathrm{tr}(\mathbf{R})-1}{2}\right).$$
由于旋转后的旋转轴向量$n$不发生改变，则：
$$\mathbf{R}n=n$$
则转轴$n$为矩阵$\mathbf{R}$特征值1对应的特征向量。求解方程后再归一化，就得到了旋转轴。

3.2.2 欧拉角

常用yaw pitch roll描述一个旋转，等价于绕ZYX轴的旋转。ZYX转角相当于把任意旋转分解为：

1.绕物体的Z轴旋转，得到偏航角yaw

2.绕旋转之后的Y轴旋转，得到俯仰角pitch

3.绕旋转之后的X轴旋转，得到滚转角roll

这样，我们就可以用$[r,p,y{]}^T$这个三维向量描述任意旋转。

3.3 四元数

3.3.1四元数的定义

旋转矩阵用九个量描述三自由度的旋转，具有冗余性：欧拉角和旋转向量是紧凑的，但具有奇异性，且我们无法找到不带奇异性的三维向量描述方式。三维旋转是一个三维流形，想无奇异性地表达它，三个量是不够的。

我们可以用复数集$\mathbb{C}$表示复平面上的向量，而复数的乘法可以表示复平面上的旋转。在三维空间中有类似复数的代数——四元数。其既紧凑又没有奇异性，缺点为不够直观、运算稍复杂。

一个四元数$q$包含一个实部和三个虚部：
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$$
其中$i,j,k$为虚部，虚部满足：
$$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$
也可以一个标量和一个向量来表示四元数：
$$\mathbf{q}=[s,\mathbf{v}],s=q_0\in \mathbb{R},\mathbf{v}=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$$
$s$为四元数的实部，$\mathbf{v}$为虚部。若一个四元数的虚部为0，称为实四元数，反之若实部为0，称为虚四元数。

设单位四元数$q$表示一个绕单位向量$n=[n_x,n_y,n_z{]}^T$，角度为$\theta$的旋转

旋转向量到单位四元数：
$$\mathbf{q}={\left[\cos \left(\frac{\theta }{2}\right),\mathbf{n}\sin \left(\frac{\theta }{2}\right)\right]}^T={\left[\cos \left(\frac{\theta }{2}\right),n_x\sin \left(\frac{\theta }{2}\right),n_y\sin \left(\frac{\theta }{2}\right),n_z\sin \left(\frac{\theta }{2}\right)\right]}^T$$
单位四元数到旋转向量:
$$\{\begin{array}{l}\theta =2\arccos q_0\\ {[n_x,n_y,n_z]}^T={[q_1,q_2,q_3]}^T/\sin \frac{\theta }{2}\end{array}$$
在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取$\theta$为0则为一个没有任何旋转的实四元数：
$$q_{=}[\pm 1,0,0,0{]}^T$$

3.3.2四元数的运算

四元数之间运算时与复数相同。设两个四元数：
$$\begin{gathered} q_a=s_a+x_{a}i+y_{a}j+z_{a}k \\ {q}_b=s_b+x_{b}i+y_{b}j+z_{b}k \end{gathered}$$
则运算规则如下：

$ 加减：\boldsymbol q_a\pm \boldsymbol q_b=[s_a\pm s_b, v_a\pm v_b]$

$\begin{array}{rl}乘法：{\mathbf{q}}_a{\mathbf{q}}_b=& s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)\mathbf{i}\\ & +(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b)\mathbf{j}\\ & +(s_a{z}_b+x_a{y}_b-y_b{x}_a+z_a{s}_b)\mathbf{k}\end{array}$

$四元数乘法不可交换。$

$也可化简为{\mathbf{q}}_a{\mathbf{q}}_b=\left[s_a{s}_b-{\mathbf{v}}_a^T{\mathbf{v}}_b,s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\right]$

$共轭：{\mathbf{q}}_a^{\ast }=s_a-x_a\mathbf{i}-y_a\mathbf{j}-z_a\mathbf{k}=\left[s_a,-{\mathbf{v}}_a\right]$

$$\begin{gathered} 四元数共轭与自己本身相乘，会得到一个实四元数，其实部为模长的平方： \\ {\mathbf{q}}^{\ast }\mathbf{q}=\mathbf{q}{\mathbf{q}}^{\ast }=\left[s_a^2+{\mathbf{v}}^T\mathbf{v},0\right] \end{gathered}$$

$模长：\Vert {\mathbf{q}}_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$

$$\begin{gathered} 四元数的乘积的模长等于模长的乘积，单位四元数相乘后仍是单位四元数 \\ \Vert {\mathbf{q}}_a{\mathbf{q}}_b\Vert =\Vert {\mathbf{q}}_a\Vert \Vert {\mathbf{q}}_b\Vert \end{gathered}$$

$逆：{\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/\Vert \mathbf{q}{\Vert }^2$

$$\begin{gathered} 四元数和自己的逆的乘积为实四元数的1： \\ \mathbf{q}{\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\mathbf{q}=1 \end{gathered}$$

$$\begin{gathered} 若\mathbf{q}为单位四元数，则逆和共轭为同一个量。且乘积的逆等于逆的乘积。 \\ ({\mathbf{q}}_a{\mathbf{q}}_b{)}^{-1}={\mathbf{q}}_b^{-1}{\mathbf{q}}_a^{-1} \end{gathered}$$

$数乘：k\mathbf{q}=[ks,k\mathbf{v}]$

$点乘：{\mathbf{q}}_a\cdot {\mathbf{q}}_b=s_a{s}_b+x_a{x}_b+y_a{y}_b+z_a{z}_b$

3.3.3 用四元数表示旋转

我们可以用四元数表示对一个点的旋转。设点$\mathbf{p}=[x,y,z]\in {\mathbb{R}}^3$和由轴角$\mathbf{n},\theta$指定的旋转，点$\mathbf{p}$经过旋转后变为${\mathbf{p}}^{\prime }$用矩阵描述为${\mathbf{p}}^{\prime }=\mathbf{R}\mathbf{p}$。若用四元数表示，则：

将点用一个虚四元数表示：$\mathbf{p}=[0,x,y,z]=[0,\mathbf{v}]$

即将四元数的虚部与空间中三个轴对应，随后以四元数$\mathbf{q}$表示这个旋转：
$$\mathbf{q}=[cos\frac{\theta }{2},\mathbf{n}sin\frac{\theta }{2}]$$
则旋转后的点${\mathbf{p}}^{\prime }$可表示为如下乘积：
$${\mathbf{p}}^{\prime }=\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}$$
计算结果实部为0，为纯虚四元数，其他三个分量表示旋转后的3D点坐标

过程如下：

设$\mathbf{q}=[s,\mathbf{v}],\mathbf{p}=[0,\mathbf{u}]$，则
$\mathbf{qp}=[s,\mathbf{v}][0,\mathbf{u}]=[-{\mathbf{v}}^T\mathbf{u},s\mathbf{u}+\mathbf{v}\times \mathbf{u}]$
又${\mathbf{q}}^{-1}=[s,-\mathbf{v}]$
则${\mathbf{p}}^{\prime }=[0,{\mathbf{u}}^{\prime }]$
且${\mathbf{u}}^{\prime }=\mathbf{u}cos\theta +(\mathbf{n}\times \mathbf{u})sin\theta +\mathbf{n}({\mathbf{n}}^T\mathbf{u})(1-cos\theta )$

过程可视为：$\mathbf{q}$描述了怎么旋转，$\mathbf{p}$表示被旋转的对象，${\mathbf{q}}^{-1}$表示撤销多余的影响，保证结果回到三维世界。

3.3.4 四元数到旋转矩阵的转换

任意单位四元数都描述了一个旋转，这个旋转也可以转换为旋转矩阵或旋转向量。我们在以上通过把四元数$\mathbf{q}$转换为轴角$\theta$和$\mathbf{n}$，再根据罗德里格斯公式转换为矩阵。计算中涉及到$arccos$，代价较大，可以通过一定的技巧绕过：

设四元数$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$，对应的旋转矩阵$\mathbf{R}$为：
$$\mathbf{R}=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2+2q_0{q}_3& 2q_1{q}_3-2q_0{q}_2\\ 2q_1{q}_2-2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3+2q_0{q}_1\\ 2q_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$$
由旋转矩阵到四元数的转换如下。设矩阵 R = { m i j } , i , j ∈ [ 1 , 2 , 3 ] \boldsymbol R=\{m_{ij}\},i,j\in [1,2,3] R={mij​},i,j∈[1,2,3]，其对应的四元数如下：
$$q_0=\frac{\sqrt{\text{tr}(R)+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_0},q_2=\frac{m_{31}-m_{13}}{4q_0},q_3=\frac{m_{12}-m_{21}}{4q_0}$$
由于$\mathbf{q}$和$-\mathbf{q}$表示同一个旋转，故$\mathbf{R}$对应的四元数不唯一。当$q_0$接近于0时，其余三个分量会很大，导致解不稳定。此时再考虑其他方法进行转换。

3.4 相似、仿射、射影变换

欧氏变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。而其他几种变换则会改变它的外形。它们都拥有类似的矩阵表示。

3.4.1 相似变换

相似变换比欧氏变换多了一个自由度，其允许物体进行均匀缩放，矩阵表示为：
$${\mathbf{T}}_S=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$$
旋转部分多了一个缩放因子$s$。相似变换不保持图形的面积不变。

3.4.2 仿射变换

仿射变换矩阵如下：
$${\mathbf{T}}_A=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ 0^T& 1\end{array}\right]$$
仿射变换只要求$\mathbf{A}$是一个可逆矩阵，不必是正交矩阵。

仿射变换也叫正交投影，经过仿射变换后，立方体不再是方的，但各个面仍是平行四边形。

3.4.3 射影变换

射影变换是最一般的变换，其矩阵表示为：
$${\mathbf{T}}_P=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ {\mathbf{a}}^T& v\end{array}\right]$$
其左上为可逆矩阵$\mathbf{A}$，右上为平移$\mathbf{t}$，左下为缩放${\mathbf{a}}^T$。当$v\ne 0$时，可以对整个矩阵除以$v$得到一个右下角为1的矩阵，否则得到右下角为0的矩阵。

2D射影变换有8个自由度，3D有15个自由度。

变换名称	矩阵形式	自由度	不变性质
欧氏变换	$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$	6	长度、夹角、体积
相似变换	${\mathbf{T}}_S=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$	7	体积比
仿射变换	${\mathbf{T}}_A=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ 0^T& 1\end{array}\right]$	12	平行比、体积比
射影变换	${\mathbf{T}}_P=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ {\mathbf{a}}^T& v\end{array}\right]$	15	接触平面的相交和相切

3.5 Eigen几何模块

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}