[视觉SLAM十四讲]学习笔记1-刚体运动之旋转矩阵与变换矩阵

1点、向量和坐标系

• 刚体： 形状和大小不发生变化的物体被称为刚体，在我们生活的三维空间中，一个空间点的位置可以由3个坐标指定，而刚体不光有位置，还有自身的姿态(姿态是指物体的朝向)。
• 点： 点是空间中的基本元素，没有长度没有体积。
• 向量： 可以看成从某点指向另一点的箭头，它是空间中的一样东西，向量在坐标系中表示为坐标，同一向量在不同坐标系中的坐标不同。
• 坐标： 在一个三维线性空间中，我们可以找到一组基$(e_1,e_2,e_3)$来表示任意向量a在线性空间下的坐标，称为向量a在这组基下的坐标：
  a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 . a= \begin{bmatrix} e_1,e_2,e_3 \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix}=a_1e_1+a_2e_2+a_3e_3. a=[e1​,e2​,e3​​] ​a1​a2​a3​​ ​=a1​e1​+a2​e2​+a3​e3​.
• 坐标系： 通常来说，坐标系由3个正交的坐标轴组成，当给定$x$和$y$轴后，通过右手(或左手)法则由$x\times y$定义出来$z$轴。根据定义方式不同可以分为左手系和右手系。右手系的定义如图所示：
  
  在大部分3D程序库中会使用右手系(如OpenGL、3D Max等)，也有部分库使用左手系(如Unity、Direct3D等)，左手系的第3个轴与右手系方向相反。
  相信阅读本教程的同学都学习过线性代数的知识，对于向量和向量之间及向量和数之间的运算都比较熟悉，这里数乘和加减法由于比较简单不做赘述，我们着重介绍一下内积(点乘)和外积(叉乘)。
• 内积： 表示为$a\cdot b$，在三维线性空间，定义为$a\cdot b=a^{T}b={\sum }_{i=1}^3{a}_i{b}_i=|a||b|cos⟨a,b⟩$，$⟨a,b⟩$指向量a，b的夹角。
• 外积： 表示为$a\times b$，$a\times b$定义为垂直于(正交)a和b的向量c ，方向由右手法则给出，大小等于向量所在的平行四边形的面积跨度。外积公式为：$a\times b=|a||b|sin(\theta )n$。
  θ是包含它们的平面中a和b之间的角度(因此，它介于 0° 和 180° 之间);
  |a|和|b|是向量a和b的大小;
  n是垂直于包含a和b的平面的单位向量，其方向使得有序集 (a,b,n)为正向的。
  下图为示意图
  
  外积的计算公式及形式有很多，感兴趣的同学可以点击文章末尾的链接进行学习(需要科学上网)，这里为了和视觉SLAM十四讲这本书中的例子相同，我们给出了下面的形式表示外积。
  
  标准基向量($i,j,k$，也表示为$e_1,e_2,e_3$)和$a$的向量分量($a_x,a_y,a_z$，也表示为$a_1,a_2,a_3$)
  每个向量都可以定义为平行于标准基向量的三个正交分量的总和：
  $$a=a_{1}i+a_{2}j+a_{3}k,b=b_{1}i+b_{2}j+b_{3}k$$它们的外积$a\times b$可以使用分配性展开：
  $a\times b=(a_{1}i+a_{2}j+a_{3}k)\times (b_{1}i+b_{2}j+b_{3}k)$
  $=a_1{b}_1(i\times i)+a_1{b}_2(i\times j)+a_1{b}_3(i\times k)+a_2{b}_1(j\times i)+a_2{b}_2(j\times j)+a_2{b}_3(j\times k)+a_3{b}_1(k\times i)+a_3{b}_2(k\times j)+a_3{b}_3(k\times k)=a_1{b}_{1}0+a_1{b}_{2}k-a_1{b}_{3}j-a_2{b}_{1}k+a_2{b}_{2}0+a_2{b}_{3}i+a_3{b}_{1}j-a_3{b}_{2}i+a_3{b}_{3}0$
  $=(a_2{b}_3-a_3{b}_2)i+(a_3{b}_1-a_1{b}_3)j+(a_1{b}_2-a_2{b}_1)k$
  则结果向量$s=s_{1}i+s_{2}j+s_{3}k=a\times b$的三个标量分量为$$s_1=a_2{b}_3-a_3{b}_2,s_2=a_3{b}_1-a_1{b}_3,s_3=a_1{b}_2-a_2{b}_1$$列向量形式为
  [ s 1 s 2 s 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] \begin{bmatrix} s_1 \\ s_2 \\ s_3 \end{bmatrix}=\begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \end{bmatrix} ​s1​s2​s3​​ ​= ​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​ ​这对应了书上形式的第二项。而书上第一项的形式为外积的行列式形式，
  a × b = ∣ i j k a 1 a 2 a 3 b 1 b 2 b 3 ∣ = ( a 2 b 3 i + a 3 b 1 j + a 1 b 2 k ) − ( a 3 b 2 i + a 1 b 3 j + a 2 b 1 k ) a{\times}b=\begin{vmatrix} i&j&k \\ a_1&a_2&a_3 \\ b_1&b_2&b_3 \end{vmatrix}=(a_2b_3i+a_3b_1j+a_1b_2k)-(a_3b_2i+a_1b_3j+a_2b_1k) a×b= ​ia1​b1​​ja2​b2​​ka3​b3​​ ​=(a2​b3​i+a3​b1​j+a1​b2​k)−(a3​b2​i+a1​b3​j+a2​b1​k)$$=(a_2{b}_3-a_3{b}_2)i+(a_3{b}_1-a_1{b}_3)j+(a_1{b}_2-a_2{b}_1)k$$可以看出，结果同第一种形式相同，所以书上的外积可以表示为：
  a × b = ∣ i j k a 1 a 2 a 3 b 1 b 2 b 3 ∣ = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] = d e f a ∧ b . a{\times}b=\begin{vmatrix} i&j&k \\ a_1&a_2&a_3 \\ b_1&b_2&b_3 \end{vmatrix}=\begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \end{bmatrix}=\begin{bmatrix} 0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0 \end{bmatrix} \xlongequal{\rm{def}} a^{\land}b. a×b= ​ia1​b1​​ja2​b2​​ka3​b3​​ ​= ​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​ ​= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​def a∧b.通过上述等式，我们引入符号${}^{\wedge }$，把$a$写成矩阵，$a^{\wedge }$实际上是一个反对称矩阵，可以把$a\times b$写成矩阵与向量的乘法$a^{\wedge }b$，变成线性运算。此符号是一个一一映射，意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然：
  a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] a^{\land}=\begin{bmatrix} 0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0 \end{bmatrix} a∧= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​

2 坐标系间的欧式变换

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

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

2.1 欧式变换之旋转

我们假设某个单位正交基$(e_1,e_2,e_3)$经过一次旋转变成了$(e_1^{\prime },e_2^{\prime },e_3^{\prime })$，这时，对于同一个向量$a$，它在两个基坐标系下的坐标分别为$[a_1,a_2,a_3{]}^T$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$。因为向量本身并没有发生变化，所以下列等式成立：
[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] [e_1,e_2,e_3]\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=[e^{\prime}_1,e^{\prime}_2,e^{\prime}_3]\begin{bmatrix}a^{\prime}_1\\a^{\prime}_2\\a^{\prime}_3\end{bmatrix} [e1​,e2​,e3​] ​a1​a2​a3​​ ​=[e1′​,e2′​,e3′​] ​a1′​a2′​a3′​​ ​
为了让两个坐标之间的关系看起来更清晰，我们将上述等式两侧同时左乘$[e_1^T,e_2^T,e_3^T]$,此时左侧第一项将变为单位矩阵：
[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = d e f R a ′ \begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=\begin{bmatrix}e^T_1e^{\prime}_1&e^T_1e^{\prime}_2&e^T_1e^{\prime}_3\\ e^T_2e^{\prime}_1&e^T_2e^{\prime}_2&e^T_2e^{\prime}_3 \\ e^T_3e^{\prime}_1&e^T_3e^{\prime}_2&e^T_3e^{\prime}_3 \end{bmatrix}\begin{bmatrix}a^{\prime}_1\\a^{\prime}_2\\a^{\prime}_3\end{bmatrix}\xlongequal{\rm{def}}\bm{R}a^{\prime} ​a1​a2​a3​​ ​= ​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​ ​ ​a1′​a2′​a3′​​ ​def Ra′
矩阵$\mathbf{R}$描述了不同坐标系下同一向量的坐标变换关系。可以说，矩阵$\mathbf{R}$描述了旋转本身。所以矩阵$\mathbf{R}$称为旋转矩阵(Rotation Matrix)。显然，我们定义的矩阵$\mathbf{R}$是由两组基之间的内积组成，实际上是各基向量夹角的余弦值，所以我们也可以称矩阵$\mathbf{R}$为方向余弦矩阵(Direction Cosine Matrix)。
同时，我们可以看出，矩阵$\mathbf{R}$为正交矩阵，根据正交矩阵的性质，我们可以得到下面的关系：
$$a^{\prime }={\mathbf{R}}^{-1}a={\mathbf{R}}^{T}a$$很明显，${\mathbf{R}}^{-1}({\mathbf{R}}^T)$刻画了一个相反的旋转。
除此之外，旋转矩阵也有一些特别的性质，我们通过矩阵$R$的行列式可以看出，它是一个行列式为1的正交矩阵。反过来说，行列式为1的正交矩阵也是一个旋转矩阵。
根据这些性质，我们可以推广到$n$维旋转矩阵，可以将$n$维旋转矩阵的集合定义如下：
$$\mathrm{SO}(\mathrm{n})=\{\mathbf{R}\in {\mathbb{R}}^{\mathrm{n}\times \mathrm{n}}|{\mathbf{RR}}^{\mathrm{T}}=\mathrm{I},\det (\mathbf{R})=1\}.$$在这里，$SO(n)$表示的是特殊正交群(Special Orthogonal Group)的意思。通过上式我们可以看出，这个集合由$n$维空间的旋转矩阵组成，所以我们可以用$SO(3)$表示三维空间的旋转。我们之后可以通过旋转矩阵直接谈论两个坐标系之间的旋转变换，而不再通过基表述。

2.2 欧式变换之平移

在欧式变换中，我们可以通过直接在旋转后的坐标后加上平移向量，这可以非常简单的表示欧式变换中的平移。我们首先考虑一个世界坐标系中的向量$a$，经过一次旋转和一次平移后，得到了$a^{\prime }$，通过开头的描述，我们将旋转和平移合到一起，有$$a^{\prime }=\mathbf{R}a+\mathbf{t}$$这里的$\mathbf{t}$即为平移向量。我们可以通过上式用一个旋转矩阵$\mathbf{R}$和一个平移向量$\mathbf{t}$来完整的描述一个欧式空间的坐标变换。
这里我们通过一个例子，来规定一下角标。在现实中，我们会定义坐标系1和坐标系2，同时向量$a$在两个坐标系下的坐标分别为$a_1$和$a_2$，根据上式，他们之间的关系应该这样表示：$$a_1={\mathbf{R}}_{12}{a}_2+{\mathbf{t}}_{12}.$$这里的${\mathbf{R}}_{12}$是指“把坐标系2的向量变换到坐标系1”中，即为“从2到1的旋转矩阵”。由于向量乘在矩阵的右边(右乘)，所以它的下标是从右往左读的。而对于平移向量${\mathbf{t}}_{12}$,它实际对应的是坐标系1原点指向坐标系2原点的向量，即在坐标系1下取的坐标，这里建议把它记作“从1到2的向量”，它的下标是从左读到右的，由于两坐标系旋转的关系，它并不等于$-{\mathbf{t}}_{12}$。

3 变换矩阵与齐次坐标

根据上一节的介绍，通过式子$a^{\prime }=\mathbf{R}a+\mathbf{t}$可以完整的表达欧式空间的旋转与平移。下面假设我们进行两次变换：${\mathbf{R}}_1,{\mathbf{t}}_1$和${\mathbf{R}}_2,{\mathbf{t}}_2$：$$b={\mathbf{R}}_{1}a+{\mathbf{t}}_1,c={\mathbf{R}}_{2}b+{\mathbf{t}}_2$$$$c={\mathbf{R}}_2({\mathbf{R}}_{1}a+{\mathbf{t}}_1)+{\mathbf{t}}_2$$这里我们可以明显看出，连续两次变换的关系并不是一个线性关系，这也是欧式变换的一个问题，这样的形式在变换多次后会变得非常啰嗦和复杂。
在这里，我们引入齐次坐标和变换矩阵的概念，对式$a^{\prime }=\mathbf{R}a+\mathbf{t}$进行重新表示

• 齐次坐标： 齐次坐标简单来说就是将一个原本是$n$维的向量用一个$n+1$维向量来表示。我们在一个三维向量$a^{\prime }$的末尾添加1，将其变为四维向量$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]$，称为齐次坐标。
  关于齐次坐标引入的意义，可以参阅这三篇文章：
  关于齐次坐标的理解(经典)
  为什么要引入齐次坐标，齐次坐标的意义(一)
  为什么要引入齐次坐标，齐次坐标的意义(二)
• 变换矩阵： 通过引入齐次坐标，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系：$$\tilde{a}=\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{c}\mathbf{R}a+\mathbf{t}\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\stackrel{\mathrm{def}}{}\mathbf{T}\left[\begin{array}{c}a\\ 1\end{array}\right].$$
  在上式中，$\tilde{a}$为$a$的齐次坐标，矩阵$\mathbf{T}$称为变换矩阵(Transform Matrix)。
  通过齐次坐标和变换矩阵，上述两次变换的叠加就可以以下更好的形式：$$\tilde{b}={\mathbf{T}}_1\tilde{a},\tilde{c}={\mathbf{T}}_2\tilde{b}\Rightarrow {\mathbf{T}}_2{\mathbf{T}}_1\tilde{a}$$在视觉SLAM十四讲中，为了防止区分齐次和非齐次坐标的符号带来的复杂性和麻烦，在不引起歧义的情况下，以后直接把它写成$b=\mathbf{T}a$的样子，默认其中进行了齐次坐标的转换。
  对于变换矩阵$\mathbf{T}$，我们可以看出，矩阵的左上角是旋转矩阵$\mathbf{R}$，右上角是平移向量$\mathbf{t}$，左下角为0向量，右下角为1。我们称这种矩阵为特殊欧式群(Special Euclidean Group)，三维特殊欧式群形式如下：$$\mathrm{SE}(3)=\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\mathbf{R}\in \mathrm{SO}(3),\mathbf{t}\in {\mathbb{R}}^3\}.$$类似于特殊正交群$SO(3)$，该矩阵的逆矩阵表示的是一个反向的变换：$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^T\mathbf{t}\\ 0^T& 1\end{array}\right]$$注意： 不是常规的反向变换，由于是旋转+平移的变换，且平移在旋转后，受到旋转的影响。例如，对于方程"$y=ax+b$，若想进行平移，首先进行变形，$y=a(x+b/a)$，从而调整参数$b/a$而不是参数$a$"。
  在视觉SLAM十四讲中，同样用$T_{12}$的形式表示从2到1的变换，在后续的内容中，在不引起歧义的情况下，为了让符号看起来更简介，不再刻意区分其次坐标和普通坐标的符号，默认使用的符号符合运算法则。

4 Eigen库的简单应用

4.1 Eigen库的介绍与安装

Eigen是一个C++开源线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。许多上层的软件库也使用Eigen进行矩阵运算，包括g2o、Sophus等。Eigen库的优势在于，它是一个纯用头文件搭建起来的库，这意味着你只能找到它的头文件，而没有类似.so或.a的二进制文件。在使用时，只需引入头文件即可，不需要链接库文件。
通过Eigen官网教程，我们可以学习更多关于Eigen库的知识。
Eigen库需要我们自己进行安装才可以使用，我们可以通过在终端命令行输入以下指令来安装Eigen库。

sudo apt-get install libeigen3-dev

4.2 Eigen库应矩阵运算实例代码

示例代码地址：https://github.com/gaoxiang12/slambook2
我们可以通过以下指令，克隆SLAM十四讲的示例代码到个人电脑上

git clone https://github.com/gaoxiang12/slambook2.git

然后在终端输入下面的命令，打开对应的实例程序即可

cd slambook2/ch3/useEigen/
code .

下面是操作演示

这里给出使用Eigen库表示矩阵和向量并进行基本运算和调用的示例代码

#include 

using namespace std;

#include 
// Eigen 核心部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 

using namespace Eigen;

#define MATRIX_SIZE 50

/****************************
 * 本程序演示了 Eigen 基本类型的使用
 ****************************/

int main(int argc, char **argv)
{
    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个2*3的float矩阵
    Matrix<float, 2, 3> matrix_23;

    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
    // 例如 Vector3d 实质上是 Eigen::Matrix，即三维向量
    Vector3d v_3d;
    // 这是一样的
    Matrix<float, 3, 1> vd_3d;

    // Matrix3d 实质上是 Eigen::Matrix
    Matrix3d matrix_33 = Matrix3d::Zero(); // 初始化为零
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;
    // 更简单的
    MatrixXd matrix_x;
    // 这种类型还有很多，我们不一一列举

    // 下面是对Eigen阵的操作
    // 输入数据（初始化）
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 输出
    cout << "matrix 2x3 from 1 to 6: \n"
         << matrix_23 << endl;

    // 用()访问矩阵中的元素
    cout << "print matrix 2x3: " << endl;
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 3; j++)
            cout << matrix_23(i, j) << "\t";
        cout << endl;
    }

    // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
    v_3d << 3, 2, 1;
    vd_3d << 4, 5, 6;

    // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
    // Matrix result_wrong_type = matrix_23 * v_3d;
    // 应该显式转换
    Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;

    Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

    // 同样你不能搞错矩阵的维度
    // 试着取消下面的注释，看看Eigen会报什么错
    // Eigen::Matrix result_wrong_dimension = matrix_23.cast() * v_3d;

    // 一些矩阵运算
    // 四则运算就不演示了，直接用+-*/即可。
    matrix_33 = Matrix3d::Random(); // 随机数矩阵
    cout << "random matrix: \n"
         << matrix_33 << endl;
    cout << "transpose: \n"
         << matrix_33.transpose() << endl;          // 转置
    cout << "sum: " << matrix_33.sum() << endl;     // 各元素和
    cout << "trace: " << matrix_33.trace() << endl; // 迹
    cout << "times 10: \n"
         << 10 * matrix_33 << endl; // 数乘
    cout << "inverse: \n"
         << matrix_33.inverse() << endl;                // 逆
    cout << "det: " << matrix_33.determinant() << endl; // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    cout << "Eigen values = \n"
         << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n"
         << eigen_solver.eigenvectors() << endl;

    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大

    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_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_stt = clock(); // 计时
    // 直接求逆
    Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout << "time of normal inverse is "
         << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    // 通常用矩阵分解来求，例如 QR 分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time of Qr decomposition is "
         << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    // 对于正定矩阵，还可以用cholesky分解来解方程
    time_stt = clock();
    x = matrix_NN.ldlt().solve(v_Nd);
    cout << "time of ldlt decomposition is "
         << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    return 0;
}

CMakelists配置文件

cmake_minimum_required(VERSION 2.8) # 最低版本声明
project(useEigen) # 项目名称

set(CMAKE_BUILD_TYPE "Release")

# CMake预定义的内建变量，且他们是全局的。该变量可用于设置编译选项。直接使用set修改其值即可。
set(CMAKE_CXX_FLAGS "-O3") # '-O3'是一个优化选项，告诉编译器优化我们的代码。

# 添加Eigen头文件
# 方式一
# include_directories("/usr/include/eigen3")

# 方式二
find_package(Eigen3 REQUIRED)
include_directories( ${EIGEN3_INCLUDE_DIRS})

# 声明一个 C++ 可执行文件
add_executable(eigenMatrix eigenMatrix.cpp)

通过下面的指令进行程序的编译和执行

mkdir build
cd build
cmake ..
make
./eigenMatrix

下面是操作演示

程序结果如下：

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6
print matrix 2x3: 
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 0.262ms
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 0.05ms
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.02ms
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

创作不易，希望大家支持，多多点赞收藏！！！！非常感谢！！！！

---

参考资料：
https://en.wikipedia.org/wiki/Cross_product
视觉SLAM十四讲：从理论到实践(第2版)(ISBN：9787121369421)
现代机器人学：机构、规划与控制(ISBN：9787111639848)
关于齐次坐标的理解(经典)
为什么要引入齐次坐标，齐次坐标的意义(一)
为什么要引入齐次坐标，齐次坐标的意义(二)