视觉SLAM十四讲（3）：三维空间刚体运动

本章需要掌握的知识点有：旋转矩阵，变换矩阵，四元数，欧拉角定义和数学表达；同时也要掌握Eigen库关于矩阵、几何模块的使用方法。

本章对应视频为： https://www.bilibili.com/video/BV16t411g7FR?p=2

【高翔】视觉SLAM十四讲

3.1 旋转矩阵

3.1.1 点，向量和矩阵的关系

这里相对比较简单，只需要理解向量内积和外积的计算方法即可，向量内积为：
$$a\cdot b=a^{T}b=|a||b|cos\theta$$
其中$\theta$为向量$a,b$之间的夹角，向量内积的结果是一个标量。
外积相对比较复杂，公式为：
$$a\times b=\begin{array}{ccc}{e}_1& e_2& e_3\\ a_1& a_2& a_3\\ b_1& b_2& a_3\end{array}=\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=a\wedge b$$
外积的结果是一个向量，方向垂直于这两个向量。大小为$|a||b|sin\theta$。这里引入了反对称矩阵，任意向量都有着唯一的一个反对称矩阵，这里为：
$$a\wedge =\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$

---

3.1.2 坐标系间的欧式变换

这里引入了旋转矩阵$\mathbb{R}$的概念，描述了向量从一组基中如何旋转变换到另一组基，旋转矩阵是一个行列式为1的正交矩阵，反之，行列式为1的正交矩阵也是一个旋转矩阵。
除了旋转变换外，还有平移，因此世界坐标系中的向量$a$，经过一次旋转（用$\mathbb{R}$描述）和一次平移$t$后，得到了新的向量$a^{{}^{\prime }}$。
$$a^{{}^{\prime }}=Ra+t$$

---

3.1.3 变换矩阵与齐次坐标

这里引入齐次坐标和变换矩阵，对于上式可得：
$$\left[\begin{array}{c}{a}^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$$
这里将旋转和平移写在了一个矩阵里，整个关系变为线性关系，矩阵$T$为变换矩阵。变换矩阵的逆矩阵为：
$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$

---

3.2 Eigen实践

下面介绍了`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.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")
add_executable(eigenMatrix eigenMatrix.cpp)

执行结果：

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.454ms
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.078ms
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.021ms
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

---

3.3 旋转向量和欧拉角（理解）

3.3.1 旋转向量

旋转矩阵使用9个变量来描述旋转，显得有些冗余；同样变换矩阵使用16个变量来描述变换，也是很冗余。因此有没有一种方式能够紧凑的描述旋转和平移呢？
一次旋转只有3个自由度，因此这里引入了旋转向量，它的方向与旋转轴一致，长度等于旋转角，这样就可以使用一个三维向量来描述旋转。
从旋转矩阵到旋转向量的过程需要罗德里格斯公式转换，这里直接给出旋转矩阵和旋转向量之间的转换关系：
$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n\wedge$$
这里，$R$为旋转矩阵，$n$为一个单位长度的向量，$\theta$为角度。对上式两边取迹，可以求出转角：
$$\theta =arccos\frac{tr(R)-1}{2}$$
因为，旋转轴上的向量在旋转后不发生变化，可得：
$$Rn=n$$
因此，$n$为矩阵$R$特征值为1对应的特征向量，再归一化，即可求出旋转向量。

3.3.2 欧拉角

欧拉角其实就是3个分离的转角，常见的有绕物体的$Z$轴旋转的偏航角(yaw)，绕$Y$旋转的俯仰角(pitch)，绕$X$轴旋转的滚转角(roll)。
工程上常会听到$rpy$角，对应的旋转顺序为$ZYX$。

---

3.4 四元数（常用）

3.4.3 用四元数表示旋转

四元数由实部和虚部组成，常见形式为：$q=q_0+q_{1}i+q_{2}j+q_{3}k$。关于四元数的运算这里不展开了，就是普通复数运算。
那么如何使用四元数来表达一个点的旋转呢？假设有一个空间三维点$p=[x,y,z]$，以及一个单位四元数$q$指定的旋转，三维点$p$经过旋转之后变为$p^{\prime }$。用矩阵描述的话，则二者关系为$p^{\prime }=Rp$，用四元数表示则为：
$$p=[0,x,y,z{]}^T=[0,v{]}^T,p^{\prime }=qp{q}^{-1}$$
最后$p^{\prime }$的虚部为旋转之后的坐标。

3.4.4 四元数到其他旋转表示的转换

这里总结四元数到旋转矩阵之间的关系，设四元数为：$q=[s,v{]}^T$，则：
$$R=v{v}^T+s^{2}I+2sv\wedge +(v\wedge {)}^2$$
四元数到旋转向量之间的关系
$$\begin{gathered} \theta =2arccos{q}_0 \\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2} \end{gathered}$$

---

3.6 Eigen几何模块

3.6.1 几何模块数据演示

这里给出如何使用Eigen库进行四元数、旋转矩阵、欧拉角、旋转向量的运算。

#include 
#include 

using namespace std;

#include 
#include 

using namespace Eigen;

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

int main(int argc, char **argv) {

	// Eigen/Geometry 模块提供了各种旋转和平移的表示
	 // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
	 Matrix3d rotation_matrix = Matrix3d::Identity();
	 // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
	 AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
	 cout.precision(3);
	 cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
	 // 也可以直接赋值
	 rotation_matrix = rotation_vector.toRotationMatrix();
	 // 用 AngleAxis 可以进行坐标变换
	 Vector3d v(1, 0, 0);
	 Vector3d v_rotated = rotation_vector * v;
	 cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
	 // 或者用旋转矩阵
	 v_rotated = rotation_matrix * v;
	 cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;
	
	 // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
	 Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
	 cout << "yaw pitch roll = " << euler_angles.transpose() << endl;
	
	 // 欧氏变换矩阵使用 Eigen::Isometry
	 Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
	 T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
	 T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
	 cout << "Transform matrix = \n" << T.matrix() << endl;
	
	 // 用变换矩阵进行坐标变换
	 Vector3d v_transformed = T * v;                              // 相当于R*v+t
	 cout << "v tranformed = " << v_transformed.transpose() << endl;
	
	 // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
	
	 // 四元数
	 // 可以直接把AngleAxis赋值给四元数，反之亦然
	 Quaterniond q = Quaterniond(rotation_vector);
	 cout << "quaternion from rotation vector = " << q.coeffs().transpose()
	      << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
	 // 也可以把旋转矩阵赋给它
	 q = Quaterniond(rotation_matrix);
	 cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
	 // 使用四元数旋转一个向量，使用重载的乘法即可
	 v_rotated = q * v; // 注意数学上是qvq^{-1}
	 cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
	 // 用常规向量乘法表示，则应该如下计算
	 cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;
	
	 return 0;
}

3.6.2 坐标转换

这是坐标转换的小例子，显示了不同坐标系下的点的坐标转换。

#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
	 Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
	 q1.normalize();
	 q2.normalize();
	 Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
	 Vector3d p1(0.5, 0, 0.2);
	
	 Isometry3d T1w(q1), T2w(q2);
	 T1w.pretranslate(t1);
	 T2w.pretranslate(t2);
	
	 Vector3d p2 = T2w * T1w.inverse() * p1;
	 cout << endl << p2.transpose() << endl;
	 return 0;
}

---

3.7 可视化演示

这是一个显示运动轨迹的程序：

#include 
#include 
#include 

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "./examples/trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {

	 vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
	 ifstream fin(trajectory_file);
	 if (!fin) {
	   cout << "cannot find trajectory file at " << trajectory_file << endl;
	   return 1;
	 }
	
	 while (!fin.eof()) {
	   double time, tx, ty, tz, qx, qy, qz, qw;
	   fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
	   Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
	   Twr.pretranslate(Vector3d(tx, ty, tz));
	   poses.push_back(Twr);
	 }
	 cout << "read total " << poses.size() << " pose entries" << endl;
	
	 // draw trajectory in pangolin
	 DrawTrajectory(poses);
	 return 0;
}
	
/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
	 // create pangolin window and plot the trajectory
	 pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
	 glEnable(GL_DEPTH_TEST);
	 glEnable(GL_BLEND);
	 glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	
	 pangolin::OpenGlRenderState s_cam(
	   pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
	   pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
	 );
	
	 pangolin::View &d_cam = pangolin::CreateDisplay()
	   .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
	   .SetHandler(new pangolin::Handler3D(s_cam));
	
	 while (pangolin::ShouldQuit() == false) {
	   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	   d_cam.Activate(s_cam);
	   glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
	   glLineWidth(2);
	   for (size_t i = 0; i < poses.size(); i++) {
	     // 画每个位姿的三个坐标轴
	     Vector3d Ow = poses[i].translation();
	     Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
	     Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
	     Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
	     glBegin(GL_LINES);
	     glColor3f(1.0, 0.0, 0.0);
	     glVertex3d(Ow[0], Ow[1], Ow[2]);
	     glVertex3d(Xw[0], Xw[1], Xw[2]);
	     glColor3f(0.0, 1.0, 0.0);
	     glVertex3d(Ow[0], Ow[1], Ow[2]);
	     glVertex3d(Yw[0], Yw[1], Yw[2]);
	     glColor3f(0.0, 0.0, 1.0);
	     glVertex3d(Ow[0], Ow[1], Ow[2]);
	     glVertex3d(Zw[0], Zw[1], Zw[2]);
	     glEnd();
	   }
	   // 画出连线
	   for (size_t i = 0; i < poses.size(); i++) {
	     glColor3f(0.0, 0.0, 0.0);
	     glBegin(GL_LINES);
	     auto p1 = poses[i], p2 = poses[i + 1];
	     glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
	     glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
	     glEnd();
	   }
	   pangolin::FinishFrame();
	   usleep(5000);   // sleep 5 ms
	 }
}