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

1. 点、向量和坐标系
2.坐标系间的欧式变换
- 2.1 旋转
- 2.2 平移
3.齐次坐标和变换矩阵
4. 相似、仿射和射影变换
- 4.1 相似变换
- 4.2 仿射变换
- 4.3 射影变换
- 4.4 总结
5.实践：Eigen

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

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

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

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

1. 点、向量和坐标系

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

刚体：刚体是形状和大小不发生变化的物体，我们日常生活的空间是三维的，所以一个空间点的位置可以由3个坐标指定，而刚体不光有位置，还有自身的姿态，姿态是指物体的朝向。
点：点是空间中的基本元素，没有长度没有体积，两个点连接起来，构成了向量。
向量：可以看成从某点指向另一点的箭头，他是空间中的一样东西，向量在坐标系中表示为坐标，同一向量在不同坐标系中的坐标不同。
坐标：假设在线性空间中，找到了该空间的一组基（就是张成这个空间的一组线性无关的向量，也称为基底），记为(e1,e2,e3)(e_{1},e_{2},e_{3})(e1,e2,e3)，那么任意向量aaa在这组基下就有一个坐标：
a=[e1,e2,e3][a1a2a3]=a1e1+a2e2+a3e3.(1.1)a = [e_{1},e_{2},e_{3}]\begin{bmatrix} a_{1}\\ a_{2}\\ a_{3} \end{bmatrix} = a_{1}e_{1} + a_{2}e_{2} + a_{3}e_{3}. \tag{1.1}a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3.(1.1)这里(a1,a2,a3)T(a_{1},a_{2},a_{3})^{T }(a1,a2,a3)T称为aaa在此基下的坐标。坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。注意：本文的向量均为列向量，与一般数学书籍相同。
坐标系：通常由3个正交的坐标轴组成，当给定xxx和yyy轴，zzz轴就可以通过右手（或左手）法则由x×yx \times yx×y定义出来。根据定义方式不同，又分为左手系和右手系。右手系中，大拇指指向xxx轴正向，食指指向yyy轴正向，中指所指方向即为zzz轴方向。大部分3D程序库使用右手系（如OpenGL、3D Max等），也有部分库使用左手系（如Unity、Direct3D等）。
内积：向量的数乘、加减法不再赘述。通常意义下的内积可以写成：a⋅b=aTb=∑i=13aibi=∣a∣∣b∣cos⟨a,b⟩.(1.2)a\cdot b= a^{T}b= \sum_{i=1}^{3}a_{i}b_{}i= \left | a \right |\left | b \right |cos \left \langle a,b \right \rangle. \tag{1.2}a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,b⟩.(1.2)其中⟨a,b⟩\left \langle a,b \right \rangle⟨a,b⟩指向量a,ba,ba,b的夹角。内积也可以描述向量间的投影关系。
外积：外积是这个样子：a×b=∥e1e2e3a1a2a3b1b2b3∥=[a2b3−a3b2a3b1−a1b3a1b2−a2b1]=[0−a3a2a30−a1−a2a10]b=defa∧b.(1.3)a\times b= \begin{Vmatrix} e_{1} & e_{2} & e_{3}\\ a_{1} & a_{2} & a_{3}\\ b_{1} & b_{2} & b_{3} \end{Vmatrix}= \begin{bmatrix} 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{bmatrix}= \begin{bmatrix} 0 & -a_{3} & a_{2}\\ a_{3} & 0 & -a_{1}\\ -a_{2} & a_{1} & 0 \end{bmatrix}b \xlongequal[]{def} a^{\wedge }b. \tag{1.3}a×b=∥∥∥∥∥∥e1a1b1e2a2b2e3a3b3∥∥∥∥∥∥=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤bdefa∧b.(1.3)外积的结果是一个向量，它的方向垂直于这两个向量，大小为∣a∣∣b∣sin⟨a,b⟩\left | a \right |\left | b \right |sin \left \langle a,b \right \rangle∣a∣∣b∣sin⟨a,b⟩，是两个向量张成的四边形的有向面积。对于外积运算，引入∧^{\wedge }∧符号，可以把aaa写成一个矩阵，它是一个反对称矩阵（AT=−AA^{T}=-AAT=−A）。你可以将∧^{\wedge }∧记成一个反对称符号，读作hat，这样就把外积a×ba\times ba×b写成了矩阵与向量的乘法a∧ba^{\wedge}ba∧b，把它变成了线性运算。这个符号非常重要，会经常用到，并且此符号是一个一一映射，意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然：a∧=[0−a3a2a30−a1−a2a10].(1.4)a^{\wedge }= \begin{bmatrix} 0 & -a_{3} & a_{2}\\ a_{3} & 0 & -a_{1}\\ -a_{2} & a_{1} & 0 \end{bmatrix}. \tag{1.4}a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤.(1.4)

2.坐标系间的欧式变换

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

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

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

2.1 旋转

我们首先考虑旋转。由旋转引出旋转矩阵和特殊正交群SO(n)SO(n)SO(n)。
旋转矩阵：设某个单位正交基e=(e1,e2,e3)e=(e_{1},e_{2},e_{3})e=(e1,e2,e3)经过一次旋转变成了e′=(e1′,e2′,e3′)e{}'=(e_{1}{}',e_{2}{}',e_{3}{}')e′=(e1′,e2′,e3′)。那么，对于同一个向量aaa，它在两个坐标系下的坐标为[a1,a2,a3][a_{1},a_{2},a_{3}][a1,a2,a3]和[a1′,a2′,a3′][a_{1}{}',a_{2}{}',a_{3}{}'][a1′,a2′,a3′]，因为向量本身没变，所以根据坐标定义，有：[e1,e2,e3][a1a2a3]=[e1′,e2′,e3′][a1′a2′a3′].(2.1)[e_{1},e_{2},e_{3}]\begin{bmatrix} a_{1}\\ a_{2}\\ a_{3} \end{bmatrix} = [e_{1}{}',e_{2}{}',e_{3}{}']\begin{bmatrix} a_{1}{}'\\ a_{2}{}'\\ a_{3}{}' \end{bmatrix} . \tag{2.1}[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤.(2.1)为了描述两个坐标之间的关系，对上式两边同时左乘eTe^{T}eT，那么左侧系数变为单位矩阵，所以：[a1a2a3]=[e1Te1′e1Te2′e1Te3′e2Te1′e2Te2′e2Te3′e3Te1′e3Te2′e3Te3′][a1′a2′a3′]=defRa′.(2.2)\begin{bmatrix} a_{1}\\ a_{2}\\ a_{3} \end{bmatrix} = \begin{bmatrix} 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}{}' \end{bmatrix}\begin{bmatrix} a_{1}{}'\\ a_{2}{}'\\ a_{3}{}' \end{bmatrix} \xlongequal{def} \mathbf{R}a{}'. \tag{2.2}⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤defRa′.(2.2)矩阵R\mathbf{R}R由两组基的内积组成，刻画了旋转前后同一个向量的坐标变换关系，矩阵R\mathbf{R}R描述了旋转本身，因此称为旋转矩阵（Rotation Matrix）。同时，该矩阵各分量是两个坐标系基的内积，所以实际上是各基向量夹角的余弦值，故也叫方向余弦矩阵（Direction Cosine Matrix）。
同时，旋转矩阵R\mathbf{R}R也是正交矩阵，它的逆（即转置）描述了一个相反的旋转。按照上面的定义方式，有：a′=R−1a=RTa.(2.3)a{}'=\mathbf{R}^{-1}a=\mathbf{R}^{T}a. \tag{2.3}a′=R−1a=RTa.(2.3)显然，R−1\mathbf{R}^{-1}R−1和RT\mathbf{R}^{T}RT刻画了一个相反的旋转。

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

2.2 平移

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

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

3.齐次坐标和变换矩阵

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

齐次坐标：这里使用一个数学技巧：我们在一个三维向量的末尾添加1，将其变为四维向量a~=[a1]\tilde{a}= \begin{bmatrix} a\\ 1 \end{bmatrix}a~=[a1]，称为齐次坐标。齐次坐标表示法就是用n+1n+1n+1维向量表示一个nnn维向量。
nnn维空间中的点的位置向量用非齐次坐标表示为(P1,P2...Pn)(P_{1}, P_{2}...P_{n})(P1,P2...Pn)，它具有nnn个分量且唯一。使用齐次坐标表示时，表示为(hP1,hP2...hPn,h)，(hP_{1}, hP_{2}...hP_{n},h)，(hP1,hP2...hPn,h)，该向量有n+1n+1n+1个坐标分量且不唯一。
对于h，通常使h=1h=1h=1。如果h≠1h\neq 1h=1且h≠0h\neq 0h=0，使用h除以齐次坐标各分量，这一方法称为齐次坐标的规范化。如果h=0h=0h=0，该点表示一个无穷远点。三元组(0,0,0)(0,0,0)(0,0,0)不表示任何点。原点表示为(0,0,0,1)(0,0,0,1)(0,0,0,1)。

变换矩阵：对于齐次坐标，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系：a~=[a′1]=[Rt0T1][a1]=defT[a1]=[Ra+t1].(3.3)\tilde{a}= \begin{bmatrix} a{}'\\ 1 \end{bmatrix}= \begin{bmatrix} R & t\\ 0^{T} & 1 \end{bmatrix}\begin{bmatrix} a\\ 1 \end{bmatrix} \xlongequal{def} T\begin{bmatrix} a\\ 1 \end{bmatrix} = \begin{bmatrix} Ra+t\\ 1 \end{bmatrix}. \tag{3.3}a~=[a′1]=[R0Tt1][a1]defT[a1]=[Ra+t1].(3.3)在该式中，矩阵TTT称为变换矩阵（Transform Matrix）。

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

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

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

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

4.1 相似变换

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

4.2 仿射变换

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

4.3 射影变换

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

4.4 总结

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

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

变换名称	矩阵形式	自由度	不变性质	变换形态
欧氏变换	TE=[Rt0T1]T_{E}=\begin{bmatrix} R & t\\ 0^{T} &1 \end{bmatrix}TE=[R0Tt1]	6	长度、夹角、体积	位置，方向改变
相似变换	TS=[sRt0T1]T_{S}=\begin{bmatrix} sR & t\\ 0^{T} &1 \end{bmatrix}TS=[sR0Tt1]	7	体积比	按比例缩放
仿射变换	TA=[At0T1]T_{A}=\begin{bmatrix} A & t\\ 0^{T} &1 \end{bmatrix}TA=[A0Tt1]	12	平行性、体积比	正交投影，平行性不变
射影变换	TP=[AtaTv]T_{P}=\begin{bmatrix} A & t\\ a^{T} &v \end{bmatrix}TP=[AaTtv]	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 50int 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_N1dMatrix<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.5364590.566198 -0.604897 -0.444451
transpose: 0.680375 -0.211234  0.5661980.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.364595.66198 -6.04897 -4.44451
inverse:
-0.198521   2.22739    2.83571.00605 -0.555135  -1.41603-1.62213   3.59308   3.28973
det: 0.208598
Eigen values=
0.02428990.9921541.80558
Eigen vectors=
-0.549013 -0.735943  0.3961980.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》编写，但相对于原文会适当精简，同时为便于全面理解，会收集其他网络好文，根据作者理解，加入一些注解和扩展知识点，如果您觉得还不错，请一键四连（点赞关注收藏评论），让更多的人看到。

参考文献：

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

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

java集合框架的结构_集合框架(Collections Framework)详解及代码示例
简介集合和数组的区别: 数组存储基础数据类型,且每一个数组都只能存储一种数据类型的数据,空间不可变. 集合存储对象,一个集合中可以存储多种类型的对象.空间可变. 严格地说,集合是存储对象的引用,每个 ...
wait 和 waitpid 详解及代码示例
wait 和 waitpid 详解及代码示例 1. 父子进程处理历史及父进程处理方法 2. wait 2.1 wait 功能 2.2 wait 接口 2.3 wait 原理 2.3.1 wait 源码 ...
Java 泛型（generics）详解及代码示例、Java 类型通配符详解及代码示例
Java 泛型(generics)详解及代码示例.Java 类型通配符详解及代码示例 - 概念 Java 泛型(generics)是 JDK 5 中引入的一个新特性, 泛型提供了编译时类型安全检测机制 ...
pdo mysql limit_PHP mysql中limit用法详解（代码示例）
在MySQL中,LIMIT子句与SELECT语句一起使用,以限制结果集中的行数.LIMIT子句接受一个或两个offset和count的参数.这两个参数的值都可以是零或正整数. offset:用于指定要 ...
Pylon SDK的C语言使用流程详解及代码示例
目录前言 1.Pylon SDK简介与基本运行流程 2.载入相机 3.流抓取器抓取对象 4.单帧或连续抓图过程 5.收尾工作 5.1卸载流抓取器 5.2卸载相机对象前言笔者采用的Pylon版本为 ...
html渐变线条代码,css3线性渐变语法的详解（代码示例）
本篇文章给大家带来的内容是css3线性渐变语法的详解(代码示例).有一定的参考价值,有需要的朋友可以参考一下,希望对你们有所帮助. 线性渐变的完整语法:.demo { background: line ...
Huffman 编码原理详解（代码示例）
1.概述 huffman编码是一种可变长编码( VLC:variable length coding))方式,于1952年由huffman提出.依据字符在需要编码文件中出现的概率提供对字符的唯一编码 ...
mid函数怎么用mysql_MySQL MID()函数的用法详解（代码示例）-mysql教程-学派吧
在MySQL中,MID()函数返回从指定位置开始的子字符串. MID()和SUBSTR()都是SUBSTRING()的同义词. 基本语法是这样的: MID(str,pos,len) 这里,str是字符 ...
mysql中函数mid_MySQL MID()函数的用法详解（代码示例）
在MySQL中,MID()函数返回从指定位置开始的子字符串. MID()和SUBSTR()都是SUBSTRING()的同义词. 基本语法是这样的:MID(str,pos,len) 这里,str是字符串 ...

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

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

1. 点、向量和坐标系

2.坐标系间的欧式变换

2.1 旋转

2.2 平移

3.齐次坐标和变换矩阵

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

4.1 相似变换

4.2 仿射变换

4.3 射影变换

4.4 总结

5.实践：Eigen

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

最新文章

热门文章