文章目录
第3讲 三维空间刚体运动
本节目标：理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角
3.1 旋转矩阵
3.1.1 点和向量，坐标系
需要注意，不要把向量与它的坐标两个概念混淆。一个向量是指空间当中的一样东西，如说
a
\mathbf{a}
a。这里
a
\mathbf{a}
a并不是和若干个实数相关联的。还有当我们指定这个三维空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标，也就是熬到若干个实数对应这个向量。例如，三维空间中的某个向量的坐标可以用
R
3
\mathbb{R}^3
R3当中的三个数来描述。某个点的坐标也可以用
R
3
\mathbb{R}^3
R3来描述。怎么描述的呢？如果我们确定一个坐标系，也就是一个线性空间的基
(
e
1
,
e
2
,
e
3
)
(\mathbf{e_1}, \mathbf{e_2}, \mathbf{e_3})
(e1,e2,e3)，那就可以谈论向量
a
\mathbf{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
\mathbf{a} = \begin{bmatrix} \mathbf{e_1} & \mathbf{e_2} & \mathbf{e_3} \end{bmatrix} \begin{bmatrix} a1 \\ a2 \\ a3 \end{bmatrix} = a_1 \mathbf{e_1} + a_2 \mathbf{e_2} + a_3 \mathbf{e_3}
a=[e1e2e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3
所以，这个坐标的具体取值，一个是和向量本身有关，第二也是和坐标系的选取有关。
对于
a
,
b
∈
R
3
\mathbf{a,b} \in \mathbb{R}^3
a,b∈R3，内积可以写成：
a
⋅
b
=
a
T
b
=
∑
i
=
1
3
a
i
b
i
=
∣
a
∣
∣
b
∣
c
o
s
<
a
,
b
>
(3.2)
\mathbf{a \cdot b} = \mathbf{a^Tb} = \sum_{i=1}^{3}{a_ib_i} = \mathbf{|a||b|}cos<\mathbf{a,b}> \qquad \text{(3.2)}
a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos<a,b>(3.2)
外积可以写成
a
×
b
=
[
i
i
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
]
b
=
a
^
b
\mathbf{a \times b} = \begin{bmatrix} \mathbf{i} & \mathbf{i} & \mathbf{k} \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_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} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \mathbf{b}= \mathbf{\hat{a}} \mathbf{b}
a×b=⎣⎡ia1b1ia2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=a^b
还可以用外积表示向量的旋转。考虑两个不平行的向量 a \mathbf{a} a, b \mathbf{b} b,我们要描述 a \mathbf{a} a到 b \mathbf{b} b之间是如何旋转的。我们可以用一个向量来描述三维空间中两个向量的旋转关系。在右手法则下，我们用右手的四个指头从 a \mathbf{a} a转向 b \mathbf{b} b，其大拇指朝向就是旋转向量的方向，事实上也是 a × b \mathbf{a}×\mathbf{b} a×b的方向。它的大小由 a \mathbf{a} a和 b \mathbf{b} b的夹角决定。通过这种方式，我们构造了从 a \mathbf{a} a到 b \mathbf{b} b的一个旋转向量。这个向量同样位于三维空间中，在此坐标系下，可以用三个实数来描述它。
3.1.2 坐标系间的欧式变换
与向量间的旋转类似，我们同样可以描述两个坐标系之间的旋转关系，再加上平移，统称为坐标系之间的变换关系。在机器人运动过程中，常见的做法是设定一个惯性坐标系（或者叫世界坐标系），可以认为它是固定不动的。同时，相机或机器人则是一个移动坐标系。我们会问：相机视野中某个向量
p
\mathbf{p}
p，它的坐标为
p
c
p_c
pc，而从世界坐标系下看，它的坐标是
p
w
p_w
pw。这两个坐标之间是如何转换的呢？这时，就需要先得到该点针对机器人坐标系坐标值，再根据机器人位姿转换到世界坐标系中，这个转换关系由一个矩阵
T
T
T来描述，如图3-2所示。
.
相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。这种变换称为欧式变换。一个欧式变换一般由一个旋转和一个平移两部分组成。首先来考虑旋转。我们设某个单位正交基
(
e
1
,
e
2
,
e
3
)
\begin{pmatrix} e_1,e_2,e_3 \end{pmatrix}
(e1,e2,e3)经过一次旋转，变成了
(
e
1
′
,
e
2
′
,
e
3
′
)
\begin{pmatrix}e'_1,e'_2,e'_3\end{pmatrix}
(e1′,e2′,e3′)。那么，对于同一个向量
a
\mathbf{a}
a（注意该向量并没有随着坐标系的旋转而发生改动），它在两个坐标系下的坐标为
[
a
1
,
a
2
,
a
3
]
T
\begin{bmatrix}a_1,a_2,a_3\end{bmatrix}^T
[a1,a2,a3]T和
[
a
1
′
,
a
2
′
,
a
3
′
]
T
\begin{bmatrix}a'_1,a'_2,a'_3\end{bmatrix}^T
[a1′,a2′,a3′]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
′
]
(
3.4
)
\begin{bmatrix}e_1,e_2,e_3\end{bmatrix} \begin{bmatrix} a_1\\a_2\\a_3\end{bmatrix} = \begin{bmatrix}e'_1,e'_2,e'_3\end{bmatrix} \begin{bmatrix}a'_1\\a'_2\\a'_3\end{bmatrix}\quad(3.4)
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤(3.4)
为了描述两个坐标之间的关系，我们对上面等式左右同时左乘
[
e
1
T
e
2
T
e
3
T
]
\begin{bmatrix}e^T_1\\e^T_2\\e^T_3 \end{bmatrix}
⎣⎡e1Te2Te3T⎦⎤，那么左边的系数变成了单位矩阵，所以：
我们把中间的矩阵拿出来，定义成一个矩阵
R
R
R。这个矩阵由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么这个矩阵也是一样的。可以说，矩阵
R
R
R描述了旋转本身。因此它又称为旋转矩阵。
旋转矩阵有一些特别的性质。事实上，它是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，我们可以把旋转矩阵的集合定义如下：
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
(3.6)
SO(n)=\{R\in\mathbb{R}^{n\times n}|RR^T=I,det(R)=1\} \quad \text{(3.6)}
SO(n)={R∈Rn×n∣RRT=I,det(R)=1}(3.6)
S
O
(
n
)
SO(n)
SO(n)是特殊正交群（Special Orthogonal Group）的意思。这个集合由n维空间的旋转矩阵组成，特别的，SO(3)就是三维空间的旋转矩阵。通过旋转矩阵，我们可以直接谈论两个坐标系之间的旋转变换，而不用再从基开始谈起了。换句话说，旋转矩阵可以描述相机的旋转
由于旋转矩阵为正交阵，它的逆（即转置）描述了一个相反的旋转。按照上面的定义方式，有：
a
′
=
R
−
1
a
=
R
T
a
.
(3.7)
\mathbf{a}'=R^{-1}\mathbf{a}=R^{T}\mathbf{a}.\quad \text{(3.7)}
a′=R−1a=RTa.(3.7)
显然
R
T
R^{T}
RT刻画了一个相反的旋转
在欧式变换中，除了旋转之外还有一个平移。考虑世界坐标系中的向量
a
\mathbf{a}
a，经过一次旋转（用
R
R
R描述）和一次平移
t
\mathbf{t}
t后，得到了
a
′
\mathbf{a}'
a′，那么把旋转和平移合到一起，有：
a
′
=
R
a
+
t
.
(3.8)
\mathbf{a}'=R\mathbf{a}+\mathbf{t}.\quad \text{(3.8)}
a′=Ra+t.(3.8)
其中，
t
\mathbf{t}
t称为平移向量。通过上式，我们用一个旋转矩阵
R
R
R和一个平移向量
t
\mathbf{t}
t完整地描述了一个欧式空间的坐标变换关系。
3.1.3 变换矩阵与齐次坐标
式（3.8）完整地表达了欧式空间的旋转与平移，不过还存在一个小问题：这里的变换关系不是一个线性关系。这样的形式在变换多次后会过于复杂。因此，我们要引入齐次坐标和变换矩阵重写式（3.8）：
这是一个数学技巧：我们把一个三维向量的末尾添加
1
1
1，变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。该式中，矩阵
T
T
T称为变换矩阵（Transform Matrix）。
关于变换矩阵
T
T
T，它具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为
0
\mathbf{0}
0向量，右下角为
1
1
1。这种矩阵又称特殊欧式群（Special Euclidean Group）：
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
.
(3.12)
SE(3) = \{T=\begin{bmatrix}R&t\\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4} | R \in SO(3), \mathbf{t} \in \mathbb{R}^3 \}. \quad \text{(3.12)}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}.(3.12)
与SO(3)一样，求解该矩阵的逆表示一个反向的变换：
T
−
1
=
[
R
T
R
T
t
0
T
1
]
.
(3.13)
T^{-1}=\begin{bmatrix}R^T &R^T\mathbf{t} \\ \mathbf{0}^T & 1\end{bmatrix}. \quad \text{(3.13)}
T−1=[RT0TRTt1].(3.13)
回顾一下这部分的内容：首先，我们说了向量和它的坐标表示，并介绍了向量间的运算；然后，坐标系之间的运动由欧式变换描述，它由平移和旋转组成。旋转可以由旋转矩阵 S O ( 3 ) SO(3) SO(3)描述，而平移直接由一个 R 3 \mathbb{R}^3 R3向量描述。最后，如果将平移和旋转放在一个矩阵中，就形成了变换矩阵 S E ( 3 ) SE(3) SE(3)。
3.2 实践：Eigen
如果你的ubuntu上还没有安装Eigen。请输入以下命令来安装它：
sudo apt-get install libeigen3-dev
Eigen头文件的默认位置在“/usr/include/eigen3/”中，如果不确定，可以输入以下命令来查找它的位置。
sudo updatedb
locate eigen3
相比于其他库，Eigen特殊之处在于，它是一个纯用头文件搭建起来的库（这非常神奇！）。这意味着你只能找到它的头文件，而没有.so或.a那样的二进制文件。我们在使用时，只需引入Eigen的头文件即可，不需要链接它的库文件（因为它没有库文件）。
下面写一段代码，来实际练习一下Eigen的使用：
#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#define MATRIX_SIZE 50
/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/
int main( int argc, char** argv )
{
// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
// 声明一个2*3的float矩阵
Eigen::Matrix<float, 2, 3> matrix_23;
// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
Eigen::Vector3d v_3d;
// 这是一样的
Eigen::Matrix<float,3,1> vd_3d;
// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
// 如果不确定矩阵大小，可以使用动态大小的矩阵
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
// 更简单的
Eigen::MatrixXd matrix_x;
// 这种类型还有很多，我们不一一列举
// 下面是对Eigen阵的操作
// 输入数据（初始化）
matrix_23 << 1, 2, 3, 4, 5, 6;
// 输出
cout << matrix_23 << 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里你不能混合两种不同类型的矩阵，像这样是错的
// Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
// 应该显式转换
Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << result << endl;
Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
cout << result2 << endl;
// 同样你不能搞错矩阵的维度
// 试着取消下面的注释，看看Eigen会报什么错
// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
// 一些矩阵运算
// 四则运算就不演示了，直接用+-*/即可。
matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵
cout << matrix_33 << endl << endl;
cout << matrix_33.transpose() << endl; // 转置
cout << matrix_33.sum() << endl; // 各元素和
cout << matrix_33.trace() << endl; // 迹
cout << 10*matrix_33 << endl; // 数乘
cout << matrix_33.inverse() << endl; // 逆
cout << matrix_33.determinant() << endl; // 行列式
// 特征值
// 实对称矩阵可以保证对角化成功
Eigen::SelfAdjointEigenSolver<Eigen::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的大小在前边的宏里定义，它由随机数生成
// 直接求逆自然是最直接的，但是求逆运算量大
Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
Eigen::Matrix< double, MATRIX_SIZE, 1> v_Nd;
v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );
clock_t time_stt = clock(); // 计时
// 直接求逆
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
// 通常用矩阵分解来求，例如QR分解，速度会快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout <<"time use in Qr decomposition is " <<1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
return 0;
}
要编译它，需要在CMakeLists.txt里制定Eigen的头文件目录。CMakeLists.txt内容如下：
cmake_minimum_required( VERSION 2.8 )
project( useEigen )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
# in osx and brew install
# include_directories( /usr/local/Cellar/eigen/3.3.3/include/eigen3 )
# 因为只有头文件，我们不需要再用target_link_libraries语句将程序链接到库上。
add_executable( eigenMatrix eigenMatrix.cpp )
输出结果如下，代码中有详细注释，就不一一解释每行输出了。
3.3 旋转向量和欧拉角
3.3.1 旋转向量
通过3.1节，我们有了旋转矩阵来描述旋转，有了变换矩阵描述一个六自由度的三维刚体运动，是不是已经足够了呢？但是，矩阵的表示方式至少有以下几个缺点：
- S O ( 3 ) SO(3) SO(3)的旋转矩阵有九个量，但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理，变换矩阵用十六个量表达了六自由度的变换。那么，是否有更紧凑的表示呢？
- 旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为 1 1 1。变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时。这些约束会使得求解变得更困难。
因此，我么希望有一种方式能够紧凑地描述旋转和平移。例如，用一个三维向量表达旋转，用六维向量表达变换。
对于坐标系的旋转，我们知道，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量（或轴角，Axis-Angle）。这种表示法只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。
事实上，旋转向量就是第四讲准备介绍的李代数。 旋转向量和旋转矩阵之间是如何转换的呢？假设有一个旋转轴为
n
\mathbf{n}
n，角度为
θ
\theta
θ的旋转，显然，它对应的旋转向量为
θ
n
\theta\mathbf{n}
θn。由旋转向量到旋转矩阵的过程由**罗德里格斯公式（Rodriguez’s Formula）**表明，由于推到过程比较复杂，只给出转换的结果：
R
=
cos
θ
I
+
(
1
−
cos
θ
)
n
n
T
+
sin
θ
n
^
(3.14)
R=\cos\theta I+(1-\cos\theta)\mathbf{n}\mathbf{n}^T+\sin\theta\hat\mathbf{n} \quad\text{(3.14)}
R=cosθI+(1−cosθ)nnT+sinθn^(3.14).
符号^是向量到反对称的转换符。反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角
θ
\theta
θ，有：
t
r
(
R
)
=
cos
θ
t
r
(
I
)
+
(
1
−
cos
θ
)
t
r
(
n
n
T
)
+
sin
θ
t
r
(
n
^
)
=
3
cos
θ
+
(
1
−
cos
θ
)
(3.15)
=
1
+
2
cos
θ
.
\begin{aligned} tr(R)&=\cos\theta tr(I) + (1-\cos\theta) tr(\mathbf{n}\mathbf{n}^T) + \sin\theta tr(\hat\mathbf{n})\\ &=3\cos\theta + (1-\cos\theta) \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \text{(3.15)}\\ &=1+2\cos\theta. \end{aligned}
tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθtr(n^)=3cosθ+(1−cosθ)(3.15)=1+2cosθ.
因此：
θ
=
arccos
(
t
r
(
R
)
−
1
2
)
.
(3.16)
\theta = \arccos(\cfrac{tr(R)-1}{2}). \quad\quad \text{(3.16)}
θ=arccos(2tr(R)−1).(3.16)
关于转轴
n
\mathbf{n}
n,由于旋转轴上的向量在旋转后不发生改变，说明
R
n
=
n
R\mathbf{n} = \mathbf{n}
Rn=n
因此，转轴
n
\mathbf{n}
n是矩阵
R
R
R特征值
1
1
1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。仍然剧透几句，这里的两个转换公式在下一讲仍将出现，你会发现它们正是
S
O
(
3
)
SO(3)
SO(3)上李群与李代数的对应关系。
3.3.2 欧拉角
下面我们来说说欧拉角。
虽然旋转矩阵、旋转向量能描述旋转，但对人类非常不直观。而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。当然，由于分解方式有许多种，所以欧拉角也存在着不同的定义方法。你或许在航空、航模中听说过“俯仰角”、“偏航角”这些词。欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。大概如图3-3所示。
欧拉角一个重大缺点是会碰到著名的万向锁问题（Gimbal Lock）：在俯仰角为
±
9
0
°
\pm90^{°}
±90°时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。理论上可以证明，只要我们想用三个实数来表达三维旋转时，都会不可避免地碰到奇异性问题。由于这种原理，欧拉角不适合于插值和迭代，往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为它具有奇异性）。不过，若你想验证自己算法是否有错时，转换成欧拉角能够快速辨认结果的正确与否。
3.4 四元数
3.4.1 四元数的定义
旋转矩阵用九个量描述三自由度的旋转，具有冗余性；
欧拉角和旋转向量是紧凑的，但具有奇异性；
事实上，我们找不到不带奇异性的三维向量描述方式。这有点类似于，当我们想用两个坐标表示地球表面时（如经度、纬度），必定存在奇异性（纬度为 ± 9 0 ° \pm90^{°} ±90°时经度无意义）。三维旋转是一个三维流形，想要无奇异性地表达它，用三个量是不够的。
回忆复数，类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数(Quaternion)。四元数时Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。如果说缺点的话，四元数不够直观，其运算稍微复杂一些。
一个四元数
q
\mathbf{q}
q拥有一个实部和三个虚部。
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
,
(3.17)
\mathbf{q} = q_0 + q_1i + q_2j + q_3k, \qquad \text{(3.17)}
q=q0+q1i+q2j+q3k,(3.17)
其中
i
,
j
,
k
i,j,k
i,j,k为四元数的三个虚部。这三个虚部满足关系式：
{
i
2
=
j
2
=
k
2
=
−
1
i
j
=
k
,
j
i
=
−
k
(3.18)
j
k
=
i
,
k
j
=
−
i
k
i
=
j
,
i
k
=
−
j
\begin{cases} &i^2=j^2=k^2=-1 \\ &ij=k,ji=-k \qquad \text{(3.18)}\\ &jk=i,kj=-i\\ &ki=j,ik=-j \end{cases}
⎩⎪⎪⎪⎨⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−k(3.18)jk=i,kj=−iki=j,ik=−j
由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数：
q
=
[
s
,
v
]
,
s
=
q
0
∈
R
,
v
=
[
q
1
,
q
2
,
q
3
]
T
∈
R
3
,
\mathbf{q} = \begin{bmatrix} s, \mathbf{v} \end{bmatrix}, s=q_0 \in \mathbb{R}, \mathbf{v} = \begin{bmatrix}q_1,q_2,q_3\end{bmatrix}^T \in \mathbb{R}^3,
q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3,
这里，
s
s
s称为四元数的实部，而
v
\mathbf{v}
v称为它的虚部。如果一个四元数虚部为
0
\mathbf{0}
0，称之为实四元数。反之，若它的实部为
0
0
0，称之为虚四元数。
我们能用单位四元数表示三维空间中任意一个旋转。这种表达方式和旋转矩阵、旋转向量有什么关系呢？我们不妨先来看旋转向量。假设某个旋转是绕单位向量
n
=
[
n
x
,
n
y
,
n
z
]
T
\mathbf{n}=\begin{bmatrix}n_x,n_y,n_z\end{bmatrix}^T
n=[nx,ny,nz]T进行了角度为
θ
\theta
θ的旋转，那么这个旋转的四元数形式为：
q
=
[
cos
θ
2
,
n
x
sin
θ
2
,
n
y
sin
θ
2
,
n
z
sin
θ
2
]
T
.
(3.19)
\mathbf{q} = \begin{bmatrix}\cos{\cfrac{\theta}{2}}, n_x\sin{\cfrac{\theta}{2}}, n_y\sin{\cfrac{\theta}{2}}, n_z\sin{\cfrac{\theta}{2}} \end{bmatrix}^T. \qquad \text{(3.19)}
q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T.(3.19)
反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：
{
θ
=
2
arccos
q
0
(3.20)
[
n
x
,
n
y
,
n
z
]
T
=
[
q
1
,
q
2
,
q
3
]
T
/
sin
θ
2
\begin{cases} \theta = 2\arccos q_0 \qquad\qquad\qquad\qquad\qquad\qquad \text{(3.20)} \\ \begin{bmatrix}n_x,n_y,n_z\end{bmatrix}^T = \begin{bmatrix}q_1,q_2,q_3\end{bmatrix}^T / \sin{\cfrac{\theta}{2}} \end{cases}
⎩⎨⎧θ=2arccosq0(3.20)[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
对式
(
3.19
)
(3.19)
(3.19)的
θ
\theta
θ加上
2
π
2\pi
2π，我们得到一个相同的旋转，但此时对应的四元数变成了
−
q
-\mathbf{q}
−q。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。
3.4.2 四元数的运算
四元数和通常复数一样，可以进行一系列运算。常见的有四则运算、数乘、求逆、共轭等等。我们分别介绍它们。
现有两个四元数
q
a
,
q
b
\mathbf{q_a},\mathbf{q_b}
qa,qb，它们的向量表示为
[
s
a
,
υ
a
]
\begin{bmatrix}s_a, \mathbf{\upsilon_a}\end{bmatrix}
[sa,υa]，
[
s
b
,
υ
b
]
\begin{bmatrix}s_b, \mathbf{\upsilon_b}\end{bmatrix}
[sb,υb]，或者原始四元数表示为：
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
.
\mathbf{q_a}=s_a+x_ai+y_aj+z_ak, \mathbf{q_b}=s_b+x_bi+y_bj+z_bk.
qa=sa+xai+yaj+zak,qb=sb+xbi+ybj+zbk.
那么，它们的运算可表示如下。
-
加法和减法
-
乘法
-
共轭
-
模长
-
逆
-
数乘与点乘
3.4.3 用四元数表示旋转
3.4.4 四元数到旋转矩阵的转换
任意单元四元数描述了一个旋转，该旋转亦可用旋转矩阵或旋转向量描述。从旋转向量到四元数的转换方式已在式（3.20）中给出。因此，现在看来，把四元数转换为矩阵的最直观方法，是先把四元数
q
\mathbf{q}
q转换为轴角
θ
\theta
θ和
n
\mathbf{n}
n，然后再根据罗德里格斯公式转换为矩阵。不过那样要计算一个
arccos
\arccos
arccos函数，代价较大。实际上这个计算可以通过一定的技巧绕过的。我们省略过程中的推到，直接给出四元数到旋转矩阵的转换方式。
值得一提的是，由于
q
\mathbf{q}
q和
−
q
-\mathbf{q}
−q表示同一个旋转，事实上一个
R
R
R对应的四元数表示并不是唯一的。同时，除了上面给出的转换方式外，还存在其他几种计算方法，而本书（《视觉slam十四讲》）都省略了。实际编程中，当
q
0
q_0
q0接近
0
0
0时，其余三个分量会非常大，导致解不稳定，此时我们再考虑使用其他的方式进行转换。
最后，无论是四元数、旋转矩阵还是轴角，它们都可以用来描述同一个旋转。我们应该在实际中选择对我们最为方便的形式，而不必拘泥于某种特定的样子。
3.5 *相似、仿射和射影变换
3D空间中的变换，除了欧式变换之外，还存在其余几种，其中欧式变换是最简单的。它们一部分和测量几何有关，因为在之后的章节中可能会提到，所以我们先罗列出来。欧式变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。而其他几种变换则会改变它的外形。他们都拥有类似地矩阵表示。
- 相似变换
相似变换比欧式变换多了一个自由度，它允许物体进行均匀的缩放，其矩阵表示为：
T S = [ s R t 0 T 1 ] . ( 3.37 ) T_S = \begin{bmatrix}sR & \mathbf{t} \\ \mathbf{0}^T & 1\end{bmatrix}. \qquad (3.37) TS=[sR0Tt1].(3.37)
注意到旋转部分多了一个缩放因子 s s s，表示我们在对向量旋转之后，可以在 x , y , z x,y,z x,y,z三个坐标上进行均匀的缩放。由于含有缩放，相似变换不再保持图形的面积不变。你可以想象一个边长为1的立方体通过相似变换后，变成边长为10的样子（但仍然是立方体）。 - 仿射变换
放射变换的矩阵形式如下：
T A = [ A t 0 T 1 ] . ( 3.38 ) T_A = \begin{bmatrix}A & \mathbf{t} \\ \mathbf{0}^T & 1\end{bmatrix}. \qquad (3.38) TA=[A0Tt1].(3.38)
与欧式变换不同的是，仿射变换只要求 A A A是一个可逆矩阵，而不必是正交矩阵。放射变换也叫正交投影。经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。 - 射影变换
射影变换是最一般的变换，它的矩阵形式为：
T P = [ A t a T v ] . ( 3.39 ) T_P = \begin{bmatrix}A & \mathbf{t} \\ \mathbf{a}^T & v\end{bmatrix}. \qquad (3.39) TP=[AaTtv].(3.39)
它左上角为可逆矩阵 A A A，右上为平移 t \mathbf{t} t，左下缩放 a T \mathbf{a}^T aT。由于采用齐坐标，当 v ≠ 0 v\neq0 v=0时，我们可以对整个矩阵除以 v v v得到一个右下角为 1 1 1的矩阵；否则，则得到右下角为 0 0 0的矩阵。因此，2D的射影变换一共有8个自由度，3D则共有15个自由度。
射影变换是现在讲过的变换中，形式最为一般的。从真实世界到相机照片的变换可以看成一个射影变换。可以想象一个原本方形的地板砖，在照片当中是什么样子：首先，它不再是方形的。由于近大远小的关系，它甚至不是平行四边形，而是一个不规则的四边形。
表3-1总结了目前讲到的几种变换的性质。注意在“不变性质”中，从下到上是有包含关系的。例如，欧式变换除了保体积之外，也具有保平行、相交等性质。
我们之后会说到，从真实世界到相机照片的变换是一个射影变换。如果相机的焦距为无穷远，那么这个变换则为仿射变换。不过，在详细讲述相机模型之前，我们只要对它们有个大致的印象即可。
3.6 实践：Eigen几何模块
现在，我们来实际演练一下前面讲到的各种旋转表达方式。我们将在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;
}
CMakeLists.txt:
cmake_minimum_required( VERSION 2.8 )
project( geometry )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( eigenGeometry eigenGeometry.cpp )
编译运行
mkdir build
cd build
cmake ..
make
./eigenGeometry
输出结果：
3.7 可视化演示
#include <iostream>
#include <iomanip>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
#include <pangolin/pangolin.h>
struct RotationMatrix {
Matrix3d matrix = Matrix3d::Identity();
};
ostream& operator << (ostream& out, const RotationMatrix& r) {
out.setf(ios::fixed);
Matrix3d matrix = r.matrix;
out << "=";
out << "[" << setprecision(2) << matrix(0,0) << "," << matrix(0,1) << "," << matrix(0,2) << "],"
<< "["<<matrix(1,0)<<","<<matrix(1,1)<<","<<matrix(1,2)<<"],"
<< "["<<matrix(2,0)<<","<<matrix(2,1)<<","<<matrix(2,2)<<"]";
return out;
}
istream& operator >> (istream& in, RotationMatrix& r) {
return in;
}
struct TranslationVector {
Vector3d trans = Vector3d(0,0,0);
};
ostream& operator << (ostream& out, const TranslationVector& t) {
out << "=[" << t.trans(0) << "," << t.trans(1) << "," << t.trans(2) << "]";
return out;
}
istream& operator >> (istream& in, TranslationVector& t) {
return in;
}
struct QuaternionDraw {
Quaterniond q;
};
ostream& operator << (ostream& out, const QuaternionDraw& quat) {
auto c = quat.q.coeffs();
out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
return out;
}
istream& operator >> (istream& in, QuaternionDraw& quat) {
return in;
}
int main() {
pangolin::CreateWindowAndBind ( "visualize geometry", 1000, 600);
glEnable(GL_DEPTH_TEST);
pangolin::OpenGlRenderState s_cam (
pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),
pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY)
);
const int UI_WIDTH = 500;
pangolin::View& d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f/600.0f).SetHandler(new pangolin::Handler3D(s_cam));
// ui
pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());
pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());
pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
while(!pangolin::ShouldQuit()) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);
pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
Matrix<double, 4, 4> m = matrix;
// m = m.inverse();
RotationMatrix R;
for(int i=0; i<3; ++i) {
for(int j=0; j<3; ++j) {
R.matrix(i,j) = m(j,i);
}
}
rotation_matrix = R;
TranslationVector t;
t.trans = Vector3d(m(0,3), m(1,3), m(2,3));
t.trans = -R.matrix*t.trans; // T 求逆 之后的平移向量t
translation_vector = t;
TranslationVector euler;
euler.trans = R.matrix.transpose().eulerAngles(2,1,0);
euler_angles = euler;
QuaternionDraw quat;
quat.q = Quaterniond(R.matrix);
quaternion = quat;
glColor3f(1.0, 1.0, 1.0);
pangolin::glDrawColouredCube();
// draw the original axis
glLineWidth(3);
glColor3f(0.8f, 0.f, 0.f);
glBegin(GL_LINES);
glVertex3f(0,0,0);
glVertex3f(10,0,0);
glColor3f(0.f, 0.8f, 0.f);
glVertex3f( 0,0,0 );
glVertex3f( 0,10,0 );
glColor3f( 0.2f,0.2f,1.f);
glVertex3f( 0,0,0 );
glVertex3f( 0,0,10 );
glEnd();
pangolin::FinishFrame();
}
return 0;
}
CMakeLists.txt:
cmake_minimum_required(VERSION 3.17)
project(visualizeGeometry)
set(CMAKE_CXX_STANDARD 14)
include_directories("/usr/include/eigen3")
# 添加Pangolin依赖
find_package(Pangolin)
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(visualizeGeometry visualizeGeometry.cpp)
target_link_libraries(visualizeGeometry ${Pangolin_LIBRARIES} )
运行结果：
1718
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
