三维空间刚体运动1：旋转矩阵与变换矩阵（详解加代码示例）

序：本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动，为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解，其中四元数部分较多较复杂，又分为四部分。如果读者急于实践，可直接阅读第五部分的机器人运动轨迹，此部分详细讲解了安装准备工作。此系列总体目录如下：

1. 旋转矩阵和变换矩阵；
2. 旋转向量表示旋转；
3. 欧拉角表示旋转；
4. 四元数包括以下部分：
   4-1. 四元数表示变换；
   4-2. 四元数线性插值方法：LinEuler/LinMat/Lerp/Nlerp/Slerp；
   4-3. 四元数多点插值方法：Squad；
   4-4. 四元数多点连续解析解插值方法：Spicv；
   4-5. 四元数多点离散数值解插值方法：Sping。
5. 实践：SLAM中显示机器人运动轨迹及相机位姿。

在正式开始之前，我想先分享学习体会。之前看SLAM，看到第六讲放弃了，无他，前边理解的不深刻，后边的越来越难以理解，学了一本强化学习之后，才静下心继续学SLAM。所以在此建议SLAM小伙伴们，高翔博士该讲的都在书里，只不过太过精简，不怕各位笑话，第三讲和第四讲反反复复来回看了四遍。所以学习SLAM的关键，就是温故而知新，多多体会总结，串联起前后相关的知识点，融会贯通才能理解后边的内容。

本博文首先介绍向量及其坐标表示，并介绍了向量间的运算；然后，使用欧式变换描述坐标系之间的运动，它由旋转和平移组成，旋转由旋转矩阵$SO(3)$描述，而平移直接由一个${\mathbb{R}}^3$向量描述；最后，如果将旋转和平移放在一个矩阵中，就形成了变换矩阵$SE(3)$，陌生符号会在下文讲解。最后在欧氏变换基础上，讲解了相似、仿射和射影变换。

1. 点、向量和坐标系

这里讲一下刚体、点、向量、坐标和坐标系、内积和外积的概念，为了引出$a^{\wedge }$。

刚体：刚体是形状和大小不发生变化的物体，我们日常生活的空间是三维的，所以一个空间点的位置可以由3个坐标指定，而刚体不光有位置，还有自身的姿态，姿态是指物体的朝向。
点：点是空间中的基本元素，没有长度没有体积，两个点连接起来，构成了向量。
向量：可以看成从某点指向另一点的箭头，他是空间中的一样东西，向量在坐标系中表示为坐标，同一向量在不同坐标系中的坐标不同。
坐标：假设在线性空间中，找到了该空间的一组基（就是张成这个空间的一组线性无关的向量，也称为基底），记为$(e_1,e_2,e_3)$，那么任意向量$a$在这组基下就有一个坐标：
$$a=[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{e}_1+a_2{e}_2+a_3{e}_3.\text{\hspace{1em}(1.1)}$$这里$(a_1,a_2,a_3{)}^T$称为$a$在此基下的坐标。坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。注意：本文的向量均为列向量，与一般数学书籍相同。
坐标系：通常由3个正交的坐标轴组成，当给定$x$和$y$轴，$z$轴就可以通过右手（或左手）法则由$x\times y$定义出来。根据定义方式不同，又分为左手系和右手系。右手系中，大拇指指向$x$轴正向，食指指向$y$轴正向，中指所指方向即为$z$轴方向。大部分3D程序库使用右手系（如OpenGL、3D Max等），也有部分库使用左手系（如Unity、Direct3D等）。
内积：向量的数乘、加减法不再赘述。通常意义下的内积可以写成：$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_{}i=\left|a\right|\left|b\right|cos⟨a,b⟩.\text{\hspace{1em}(1.2)}$$其中$⟨a,b⟩$指向量$a,b$的夹角。内积也可以描述向量间的投影关系。
外积：外积是这个样子：$$a\times b=\Vert \begin{array}{ccc}{e}_1& e_2& e_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert =\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b\stackrel{def}{=}{a}^{\wedge }b.\text{\hspace{1em}(1.3)}$$外积的结果是一个向量，它的方向垂直于这两个向量，大小为$\left|a\right|\left|b\right|sin⟨a,b⟩$，是两个向量张成的四边形的有向面积。对于外积运算，引入${}^{\wedge }$符号，可以把$a$写成一个矩阵，它是一个反对称矩阵（$A^T=-A$）。你可以将${}^{\wedge }$记成一个反对称符号，读作hat，这样就把外积$a\times b$写成了矩阵与向量的乘法$a^{\wedge }b$，把它变成了线性运算。这个符号非常重要，会经常用到，并且此符号是一个一一映射，意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然：$$a^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right].\text{\hspace{1em}(1.4)}$$

2.坐标系间的欧式变换

此节是整篇甚至整本书的重中之重，请重点要理解掌握。博主也会极力详细讲清楚。首先，由刚体运动引出欧式变换。

我们经常在实际场景中定义各种各样的坐标系，如果考虑运动的机器人（即相机），那么常见的做法是设定一个惯性坐标系（或者叫世界坐标系），可以认为它是固定不动的。这时就会有这样的疑问：相机视野中某个向量p，它在相机坐标系下的坐标为$p_c$，而在世界坐标系下看，其坐标为$p_w$，那么，这两个坐标之间是如何转换的呢？这时，需要先得到该点针对机器人坐标系的坐标值，再根据机器人位姿变换到世界坐标系中，可以通过数学手段的变换矩阵$T$来描述它。

刚体运动：两个坐标系之间的运动变换由一个旋转加上一个平移组成，这种运动就是刚体运动。相机运动就是一个刚体运动。刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化。此时，我们说手机坐标系和世界坐标系之间，相差了一个欧氏变换（Euclidean Transform）。欧氏变换由旋转和平移组成。

2.1 旋转

我们首先考虑旋转。由旋转引出旋转矩阵和特殊正交群$SO(n)$。
旋转矩阵：设某个单位正交基$e=(e_1,e_2,e_3)$经过一次旋转变成了$e{}^{\prime }=(e_1{}^{\prime },e_2{}^{\prime },e_3{}^{\prime })$。那么，对于同一个向量$a$，它在两个坐标系下的坐标为$[a_1,a_2,a_3]$和$[a_1{}^{\prime },a_2{}^{\prime },a_3{}^{\prime }]$，因为向量本身没变，所以根据坐标定义，有：$$[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[e_1{}^{\prime },e_2{}^{\prime },e_3{}^{\prime }]\left[\begin{array}{c}{a}_1{}^{\prime }\\ a_2{}^{\prime }\\ a_3{}^{\prime }\end{array}\right].\text{\hspace{1em}(2.1)}$$为了描述两个坐标之间的关系，对上式两边同时左乘$e^T$，那么左侧系数变为单位矩阵，所以：$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1{}^{\prime }& e_1^T{e}_2{}^{\prime }& e_1^T{e}_3{}^{\prime }\\ e_2^T{e}_1{}^{\prime }& e_2^T{e}_2{}^{\prime }& e_2^T{e}_3{}^{\prime }\\ e_3^T{e}_1{}^{\prime }& e_3^T{e}_2{}^{\prime }& e_3^T{e}_3{}^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1{}^{\prime }\\ a_2{}^{\prime }\\ a_3{}^{\prime }\end{array}\right]\stackrel{def}{=}\mathbf{R}a{}^{\prime }.\text{\hspace{1em}(2.2)}$$矩阵$\mathbf{R}$由两组基的内积组成，刻画了旋转前后同一个向量的坐标变换关系，矩阵$\mathbf{R}$描述了旋转本身，因此称为旋转矩阵（Rotation Matrix）。同时，该矩阵各分量是两个坐标系基的内积，所以实际上是各基向量夹角的余弦值，故也叫方向余弦矩阵（Direction Cosine Matrix）。
同时，旋转矩阵$\mathbf{R}$也是正交矩阵，它的逆（即转置）描述了一个相反的旋转。按照上面的定义方式，有：$$a{}^{\prime }={\mathbf{R}}^{-1}a={\mathbf{R}}^{T}a.\text{\hspace{1em}(2.3)}$$显然，${\mathbf{R}}^{-1}$和${\mathbf{R}}^T$刻画了一个相反的旋转。

特殊正交群$SO(n)$：旋转矩阵$R$是一个行列式为1的正交矩阵（即$A^{-1}=A^T$），反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将$n$维旋转矩阵的集合定义如下：$$SO(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}|\mathbf{R}{\mathbf{R}}^T=\mathbf{I},det(\mathbf{R})=1\right\}.\text{\hspace{1em}(2.4)}$$$SO(n)$是特殊正交群（Special Orthogonal Group）的意思。这个集合由$n$维空间的旋转矩阵，特别的，$SO(3)$就是指三维空间的旋转。通过旋转矩阵，可以直接谈论两个坐标系之间的旋转变换，而不用再从基谈起。

2.2 平移

在欧式变换中，除了旋转还有平移。
考虑世界坐标系中的向量$a$，经过一次旋转矩阵$R$和一个平移向量$t$后，得到$a{}^{\prime }$，那么把旋转和平移合到一起，有：$$\mathbf{a}{}^{\prime }=\mathbf{R}\mathbf{a}+\mathbf{t}.\text{\hspace{1em}(2.5)}$$通过上式，我们用一个旋转矩阵$R$和一个平移向量$t$完整的描述了一个欧式空间的坐标变换。

同时，这里对下标做一下说明。实际当中，我们会定义坐标系1，坐标系2，那么向量$a$在两个坐标系下的坐标为$a_1,a_2$，它们之间的关系应该是：$$a_1=R_{12}{a}_2+t_{12}.\text{\hspace{1em}(2.6)}$$这里的$R_{12}$是指“把坐标系2的向量变换到坐标系1”，即“从2到1的旋转矩阵”。由于向量乘在矩阵的右边，所以它的下标是从右读到左的。关于平移向量$t_{12}$，它实际对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，所以建议读者把它记作“从1到2的向量”，它的下标是从左读到右的，但它并不等于$-t_{21}$。

3.齐次坐标和变换矩阵

对于式（2.5）所表达的欧式空间的旋转和平移还存在一个问题：这里的变换关系是一个线性关系。假设我们进行了两次变换：$R_1,t_1$和$R_2,t_2$：$$b=R_{1}a+t_1,c=R_{2}b+t_2.\text{\hspace{1em}(3.1)}$$那么，从$a$到$c$的变换为：$$c=R_2(R_{1}a+t_1)+t_2.\text{\hspace{1em}(3.2)}$$这样的形式在变换多次之后会显得很啰嗦。因此引入齐次坐标和变换矩阵。

齐次坐标：这里使用一个数学技巧：我们在一个三维向量的末尾添加1，将其变为四维向量$\tilde{a}=\left[\begin{array}{c}a\\ 1\end{array}\right]$，称为齐次坐标。齐次坐标表示法就是用$n+1$维向量表示一个$n$维向量。
$n$维空间中的点的位置向量用非齐次坐标表示为$(P_1,P_2...P_n)$，它具有$n$个分量且唯一。使用齐次坐标表示时，表示为$(h{P}_1,h{P}_2...h{P}_n,h)，$该向量有$n+1$个坐标分量且不唯一。
对于h，通常使$h=1$。如果$h\ne 1$且$h\ne 0$，使用h除以齐次坐标各分量，这一方法称为齐次坐标的规范化。如果$h=0$，该点表示一个无穷远点。三元组$(0,0,0)$不表示任何点。原点表示为$(0,0,0,1)$。

变换矩阵：对于齐次坐标，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系：$$\tilde{a}=\left[\begin{array}{c}a{}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\stackrel{def}{=}T\left[\begin{array}{c}a\\ 1\end{array}\right]=\left[\begin{array}{c}Ra+t\\ 1\end{array}\right].\text{\hspace{1em}(3.3)}$$在该式中，矩阵$T$称为变换矩阵（Transform Matrix）。

那么依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式：$$\tilde{b}=T_1\tilde{a},\tilde{c}=T_2\tilde{b}\Rightarrow \tilde{c}=T_2{T}_1\tilde{a}.\text{\hspace{1em}(3.4)}$$但是区分齐次和非齐次坐标的符号令我们厌烦，所以，在不引起歧义的情况下，以后直接把它写成$b=Ta$的样子，默认其中进行了齐次坐标的转换。

特殊欧式群$SE(3)$：对于变换矩阵T，它具有比较特别的结构：左上角为旋转矩阵，右上角为平移向量，左下角为$0$向量，右下角为1。这种矩阵又称为特殊欧式群（Special Euclidean Group）:$$SE(3)=\left\{T_E=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\right\}.\text{\hspace{1em}(3.5)}$$与$SO(3)$一样，求解该矩阵的逆$T^{-1}$，表示一个反向的变换：$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right].\text{\hspace{1em}(3.6)}$$同样，我们用$T_{12}$这样的写法表示从2到1的变换。在不引起歧义的情况下，以后不可以区别齐次坐标与普通坐标的符号，默认使用的是符合运算法则的那一种，因为齐次坐标与非齐次坐标之间的转换事实上非常容易。

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

除了欧式变换，3D空间还存在其他几种变换方式，只不过欧氏变换是最简单的。它们一部分和测量几何有关，因为在之后的讲解中可能会提到，所以先罗列出来。欧氏变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。但现实中由于角度问题，总会发生畸变，所以需要相似、仿射、射影变换，它们都会改变物体的外形。它们都有类似的矩阵表示。

4.1 相似变换

相似变换比欧式变换多了一个自由度，它允许物体进行均匀缩放，其矩阵表示为：$$T_S=\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(4.1)}$$
注意，旋转部分多了一个缩放因子$s$，它表示我们在对向量旋转之后，可以在$x,y,z$三个坐标上进行均匀缩放。由于含有缩放，相似变换不再保持图形的面积不变。你可以想象一个边长为1的立方体经过相似变换后，变成边长为10的立方体。
三维相似变换的集合也叫做相似变换群，记作$Sim(3)$。

4.2 仿射变换

仿射变换的矩阵形式如下：$$T_A=\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(4.2)}$$与欧氏变换不同二十，仿射变换只要求$A$是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影，经过仿射变换之后，立方体就不再是方的了，但是各个方面仍然是平行四边形。

4.3 射影变换

射影变换是最一般的变换，又称为投影变换。它的矩阵形式为：$$T_P=\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]\text{\hspace{1em}(4.3)}$$它的左上角为可逆矩阵$A$，右上角为平移$t$，左下角为缩放$a^T$，右下角为整体的变换比例$v$。由于采用了齐次坐标，当$v\ne 0$时，我们可以对整个矩阵除以$v$得到一个右下角为1的矩阵；否则当$v=0$时，得到右下角为0的矩阵。因此，2D的射影变换一共有8个自由度，3D则共有15个自由度。
射影变换是讲过的变换中，形式最一般的。从真实世界到相机照片的变换可以看成一个射影变换。读者可以想象一个原本方形的地板砖，在照片中是什么样子？首先，它不再是方形的，由于近大远小的关系，它甚至不是平行四边形，而是一个不规则的四边形。这也是位姿中常遇到的情况。

4.4 总结

下面对比总结下讲到的四种变换的性质。注意在“不变性质”中，从上到下是有包含关系的。例如，欧氏变换除了保体积，也具有保平行、相交等性质。

表4-1 常见变换的性质比较

变换名称	矩阵形式	自由度	不变性质	变换形态
欧氏变换	$T_E=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$	6	长度、夹角、体积	位置，方向改变
相似变换	$T_S=\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right]$	7	体积比	按比例缩放
仿射变换	$T_A=\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right]$	12	平行性、体积比	正交投影，平行性不变
射影变换	$T_P=\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]$	15	接触平面的相交和相切	大小、平行性均发生改变

从真实世界到相机照片的变换是一个射影变换。如果相机的焦距为无穷远，那么这个变换为仿射变换。在详细学习相机模型之前，只要对它们有个大致了解即可。

5.实践：Eigen

本节讲解如何使用Eigen表示矩阵和向量，随后引申至旋转矩阵与变换矩阵的运算。KDevelop工程形式的代码在附件中。

Eigen:Eigen是一个C++开源线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。许多上层的软件库也使用Eigen进行矩阵运算，包括g2o、Sophus等。与其他库相比，Eigen的特殊之处在于，它是一个纯用头文件搭建起来的库，这意味着你只能找到它的头文件，而没有类似.so或.a的二进制文件。在使用时，只需引入头文件即可，不需要链接库文件。例程只是介绍了基本的矩阵运算，你可以通过Eigen官网教程学习更多Eigen知识。

如果没有安装Eigen，请输入以下命令进行安装：

sudo apt install libeigen3-dev

下面写一段代码来实际练习Eigen的使用（已添加注释）：

#include<iostream>
using namespace std;

#include<ctime>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Dense>  //稠密矩阵的代数运算，如逆、特征值等
using namespace Eigen;

#define MATRIX_SIZE 50

int main(int argc, char **argv){
    //Eigen中所有向量和矩阵都是Eigen::Matrix，它是一个模板类，前三个参数为数据类型、行、列。下式为声明一个2*3的float矩阵
    Matrix<float, 2, 3> matrix_23f;
    //同时，Eigen通过typedef提供了许多内置类型，不过底层仍是Eigen::Matrix，例如Vector3d实质上是Eigen::Matrix<double, 3, 1>，即三维向量。
    Vector3d v_3d;
    Matrix<float, 3, 1> matrix_31f;
    
    Matrix3d matrix_33d = Matrix3d::Zero();
    //如果不确定大小，可使用动态大小的矩阵，Matrix<double, Dynamic, Dynamic>与MatrixXd相同。
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;
    MatrixXd matrix_x;
    
    //下面是对Eigen矩阵的操作
    
    //输入数据进行初始化
    matrix_23f<<1,2,3,4,5,6;
    cout<<"matrix 2*3 from 1 to 6:\n"<<matrix_23f<<endl;
    
    //用（）访问矩阵中的元素
    cout<<"print matrix 2*3:"<<endl;
    for (int i=0; i<2; i++) {
        for (int j=0; j<3; j++) {
            cout<<matrix_23f(i, j)<<"\t";
        }
        cout<<endl;
    }
    
    v_3d << 3,2,1;
    matrix_31f<<4,5,6;
    
    //在Eigen中，不能混合两种不同类型的矩阵，必须进行显式转换。同样，不能搞混维度
    Matrix<double, 2, 1> result = matrix_23f.cast<double>() * v_3d;
    cout<<"[1,2,3;4,5,6]*[3,2,1]="<<result.transpose()<<endl;
    
    Matrix<float, 2, 1> result2 = matrix_23f * matrix_31f;
    cout<<"[1,2,3;4,5,6]*[4,5,6]="<<result2.transpose()<<endl;

    //同样，不能搞混维度,下面是个错误例子。当你在编译程序，出现莫名其妙的错误时，请首先仔细检查你所进行运算矩阵的维度，这点相当重要。
    //Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23f.cast<double>()*v_31d;
    
    //一些矩阵运算
    matrix_33d = Matrix3d::Random();    //随机数矩阵
    cout<<"random matrix: \n"<<matrix_33d<<endl;
    cout<<"transpose: \n"<<matrix_33d.transpose()<<endl;    //转置
    cout<<"sum: "<<matrix_33d.sum()<<endl;    //各元素和
    cout<<"trace: "<<matrix_33d.trace()<<endl;    //迹
    cout<<"times 10: \n"<<10 * matrix_33d<<endl;    //数乘
    cout<<"inverse: \n"<<matrix_33d.inverse()<<endl;    //逆
    cout<<"det: "<<matrix_33d.determinant()<<endl;    //行列式
    
    //特征值和特征向量，实对称矩阵可保证对角化成功。
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33d.transpose()*matrix_33d);
    cout<<"Eigen values=\n"<<eigen_solver.eigenvalues()<<endl;
    cout<<"Eigen vectors=\n"<<eigen_solver.eigenvectors()<<endl;
    
    //解方程，这里求解方程matrix_NN * x = v_N1d
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose();
    Matrix<double, MATRIX_SIZE, 1> v_N1d = MatrixXd::Random(MATRIX_SIZE, 1);
    
    //计时
    clock_t time_stt = clock();
    //直接求逆，运算量大
    Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse()*v_N1d;
    cout<<"time of normal inverse is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
    
    time_stt = clock();
    //通常用矩阵分解来求解，例如QR分解，速度会快很多
    x = matrix_NN.colPivHouseholderQr().solve(v_N1d);
    cout<<"time of Qr decomposition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
    
    time_stt = clock();
    //对于正定矩阵，还可以用cholesky分解来解方程
    x = matrix_NN.ldlt().solve(v_N1d);
    cout<<"time of ldlt decomposition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
    
    time_stt = clock();
    //此外还有lu分解
    x = matrix_NN.lu().solve(v_N1d);
    cout<<"time of lu decomposition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
}

CMakeLists.txt文件内容如下：

cmake_minimum_required(VERSION 3.0)
project(rigidMotion)
add_executable(useEigen useEigen.cpp)
set(CMAKE_BUILD_TYPE "Debug")

编译好程序后，运行它，可以看到各矩阵运算结果如下：

matrix 2*3 from 1 to 6:
1 2 3
4 5 6
print matrix 2*3:
1	2	3	
4	5	6	
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]=32 77
random matrix: 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
transpose: 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 0.208598
Eigen values=
0.0242899
 0.992154
  1.80558
Eigen vectors=
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 1.967ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of Qr decomposition is 2.409ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of ldlt decomposition is 0.667ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of lu decomposition is 0.787ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734

附件包含了第三讲所有代码。
后续会介绍刚体运动第二部分：旋转向量和欧拉角，以及第三部分：四元数表示旋转。请继续学习，欢迎留言讨论，你的关注是我更新下去的动力。

本文基于《视觉SLAM十四讲：从理论到实践》和《Quaternions, Interpolation and Animation》编写，但相对于原文会适当精简，同时为便于全面理解，会收集其他网络好文，根据作者理解，加入一些注解和扩展知识点，如果您觉得还不错，请一键四连（点赞关注收藏评论），让更多的人看到。

参考文献：

1. 《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社