【视觉SLAM十四讲学习笔记】第三讲——Eigen库

专栏系列文章如下：
【视觉SLAM十四讲学习笔记】第一讲——SLAM介绍
【视觉SLAM十四讲学习笔记】第二讲——初识SLAM
【视觉SLAM十四讲学习笔记】第三讲——旋转矩阵
【视觉SLAM十四讲学习笔记】第三讲——旋转向量和欧拉角
【视觉SLAM十四讲学习笔记】第三讲——四元数

本章将介绍视觉SLAM的基本问题之一：如何描述刚体在三维空间中的运动？

这一篇文章将原书第三讲中有关Eigen的部分跟官方教程进行了一定程度上的整合。

Eigen简介与安装

安装

Eigen是一个C++开源线性代数库。它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。许多上层的软件库也使用Eigen进行矩阵运算，包括g2o、Sophus等。

请输入以下命令进行安装：

sudo apt-get install libeigen3-dev

CMakeLists.txt编写

与其他库相比，Eigen的特殊之处在于，它是一个纯用头文件搭建起来的库，这意味着你只能找到它的头文件，而没有类似.so或者.a的二进制文件。在使用时，只需引入Eigen的头文件即可，不需要链接库文件（因为它没有库文件）。用cmake管理项目的时候，只需要在CMakeLists.txt里面头文件的路径即可：

#添加头文件
include_directories("/usr/include/eigen3")

因为Eigen库只有头文件，所以不需要再用target_link_libraries语句将程序链接到库上。

Eigen每个头文件的作用

Eigen所有的头文件及头文件里面的类的作用见下表：

模块	头文件	内容
Core	#include <Eigen/Core>	矩阵和数组类，基本线性代数（包括三角积和自伴随积），数组操作
Geometry	#include <Eigen/Geometry>	变换、平移、缩放、旋转 2D 和 3D 旋转（四元数、角度轴)
LU	#include <Eigen/LU>	使用求解器（FullPivLU、PartialPivLU)
Cholesky	#include <Eigen/Cholesky>	使用求解器的 LLT 和 LDLT Cholesky 因式分解
Householder	#include <Eigen/Householder>	该模块由多个线性代数模块使用
SVD	#include <Eigen/SVD>	使用最小二乘求解器（JacobiSVD、BDCSVD）进行 SVD 分解)
QR	#include <Eigen/QR>	使用求解器进行 QR 分解（HouseholderQR、ColPivHouseholderQR、FullPivHouseholderQR)
Eigenvalues	#include <Eigen/Eigenvalues>	特征值、特征向量分解（EigenSolver、SelfAdjointEigenSolver、ComplexEigenSolver)
Sparse	#include <Eigen/Sparse>	稀疏矩阵存储和相关基本线性代数（SparseMatrix、SparseVector）
——	#include <Eigen/Dense>	包括 Core、Geometry、LU、Cholesky、SVD、QR 和 Eigenvalues 头文件
——	#include <Eigen/Eigen>	包括 Dense 和 Sparse 头文件（整个 Eigen 库）

一般为了省事，可以直接导入#include <Eigen/Dense> 或者#include <Eigen/Eigen>

矩阵类（The Martix class）

在Eigen中，所有matrices和vectors都是Matrix模板类的对象。vectors只是matrices的一种特殊情况，具有1行或1列。

Matrix的前三个模板参数

该矩阵类需要六个模板参数，但一般只需了解前三个参数即可。剩下的三个参数具有默认值：

Matrix<typename Scalar, int RowsAtCompileTime, int ColsAtCompileTime>

• Scalar 是标量类型，即系数的类型。如果要使用浮点数矩阵，请在此处选择float。
• RowsAtCompileTime和ColsAtCompileTime是在编译时已知的矩阵的行数和列数。

Eigen提供了许多方便的typedef来覆盖通常的情况。例如，Matrix4f是一个4x4的浮点矩阵。这是Eigen定义的：

typedef Matrix <float，4，4> Matrix4f;

Eigen里面用到了很多的typedef简化名称长度，例如下面：

typedef Matrix<float, 3, 1> Vector3f;
typedef Matrix<int, 1, 2> RowVector2i;
typedef Matrix<double, Dynamic, Dynamic> MatrixXd;
typedef Matrix<int, Dynamic, 1> VectorXi;

Vectors

在Eigen中，vectors只是Matrix的一种特殊情况，具有1行或1列。他们只有1列的情况最为常见；这样的向量称为列向量，通常缩写为向量。在另一行有1行的情况下，它们称为行向量。

列向量：

typedef Matrix<float, 3, 1> Vector3f;

行向量：

typedef Matrix<int, 1, 2> RowVector2i;

动态矩阵

动态矩阵在编译的时候不知道其大小，需要在运行的时候才能确定其大小。

typedef Matrix<double, Dynamic, Dynamic> MatrixXd;

类似地，对于动态向量：

typedef Matrix<int, Dynamic, 1> VectorXi;

例如我们可以这样定义一个动态矩阵：

MatrixXd m(3,4) ;  // 指定矩阵大小为3X4，也可以不指定

初始化

逗号初始化

Eigen提供了一种逗号初始化器语法，该语法使用户可以轻松设置矩阵，向量或数组的所有系数。只需列出系数，从左上角开始，从左到右，从上到下移动。需要预先指定对象的大小。如果列出的系数太少或太多，编译器就会报错。
此外，初始化列表的元素本身可以是向量或矩阵。通常的用途是将向量或矩阵连接在一起。例如，这是如何将两个行向量连接在一起。请记住，必须先设置大小，然后才能使用逗号初始化程序。

RowVectorXd vec1(3);
vec1 << 1, 2, 3;
std::cout << "vec1 = " << vec1 << std::endl;
 
RowVectorXd vec2(4);
vec2 << 1, 4, 9, 16;
std::cout << "vec2 = " << vec2 << std::endl;
 
RowVectorXd joined(7);
joined << vec1, vec2;
std::cout << "joined = " << joined << std::endl;

其他常用初始化方法

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

int main() {
    MatrixXd m0 = MatrixXd::Random(3, 3);           //随机初始化初始化的值在[-1,1]区间内,矩阵大小3X3
    MatrixXd m1 = MatrixXd::Constant(3, 3, 2.4);    //常量值初始化,矩阵里面的值全部为2.4 ,三个参数分别代表：行数，列数，常量值
    Matrix2d m2 = Matrix2d::Zero();                 //零初始化.矩阵里面的值全部为0
    Matrix3d m3 = Matrix3d::Ones();                 // 矩阵里面的值全部初始化为1
    Matrix4d m4 = Matrix4d::Identity();             //初始化为单位矩阵
    Matrix3d m5;                                    //逗号初始化
    m5 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    cout << "m0 =" << endl << m0 << endl;
    cout << "m1 =" << endl << m1 << endl;
    cout << "m2 =" << endl << m2 << endl;
    cout << "m3 =" << endl << m3 << endl;
    cout << "m4 =" << endl << m4 << endl;
    cout << "m5 =" << endl << m5 << endl;

    MatrixXf mat = MatrixXf::Ones(2, 3);
    std::cout << "before: " << endl << mat << std::endl << std::endl;
    mat = (MatrixXf(2, 2) << 0, 1, 2, 0).finished() * mat;    //此处使用了临时变量，然后使用逗号初始化，在此必须使用finish（）方法来获取实际的矩阵对象。
    std::cout << "after: " << endl << mat << std::endl;
}

输出：

m0 =
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
m1 =
2.4 2.4 2.4
2.4 2.4 2.4
2.4 2.4 2.4
m2 =
0 0
0 0
m3 =
1 1 1
1 1 1
1 1 1
m4 =
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
m5 =
1 2 3
4 5 6
7 8 9
before: 
1 1 1
1 1 1

after: 
1 1 1
2 2 2

调整矩阵大小

矩阵的当前大小可以通过rows（），cols（）和size（）检索。这些方法分别返回行数，列数和系数数。调整动态大小矩阵的大小是通过resize（）方法完成的。
动态矩阵可以随意调整矩阵大小，固定尺寸的矩阵无法调整大小。

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main() {
    MatrixXd m(2, 5);
    m.resize(4, 3);
    std::cout << "The matrix m is of size "
              << m.rows() << "x" << m.cols() << std::endl;
    std::cout << "It has " << m.size() << " coefficients" << std::endl;
    VectorXd v(2);
    v.resize(5);
    std::cout << "The vector v is of size " << v.size() << std::endl;
    std::cout << "As a matrix, v is of size "
              << v.rows() << "x" << v.cols() << std::endl;
}

输出：

The matrix m is of size 4x3
It has 12 coefficients
The vector v is of size 5
As a matrix, v is of size 5x1

代码示例

#include <iostream>

using namespace std;

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

using namespace 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<double, 3, 1>，即三维向量
  Vector3d v_3d;
  // 这是一样的
  Matrix<float, 3, 1> vd_3d;

  // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
  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;
  }

  return 0;
}

输出结果：

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6
print matrix 2x3: 
1	2	3	
4	5	6	

矩阵运算

加法和减法

当然，左侧和右侧必须具有相同的行数和列数。它们也必须具有相同的类型，因为 Eigen 不执行自动类型升级。

#include <iostream>
#include <Eigen/Dense>
 
int main()
{
  Eigen::Matrix2d a;
  a << 1, 2,
       3, 4;
  Eigen::MatrixXd b(2,2);
  b << 2, 3,
       1, 4;
  std::cout << "a + b =\n" << a + b << std::endl;
  std::cout << "a - b =\n" << a - b << std::endl;
  std::cout << "Doing a += b;" << std::endl;
  a += b;
  std::cout << "Now a =\n" << a << std::endl;
  Eigen::Vector3d v(1,2,3);
  Eigen::Vector3d w(1,0,0);
  std::cout << "-v + w - v =\n" << -v + w - v << std::endl;
}

输出：

a + b =
3 5
4 8
a - b =
-1 -1
 2  0
Doing a += b;
Now a =
3 5
4 8
-v + w - v =
-1
-4
-6

标量乘除法

#include <iostream>
#include <Eigen/Dense>
 
int main()
{
  Eigen::Matrix2d a;
  a << 1, 2,
       3, 4;
  Eigen::Vector3d v(1,2,3);
  std::cout << "a * 2.5 =\n" << a * 2.5 << std::endl;
  std::cout << "0.1 * v =\n" << 0.1 * v << std::endl;
  std::cout << "Doing v *= 2;" << std::endl;
  v *= 2;
  std::cout << "Now v =\n" << v << std::endl;
}

输出：

a * 2.5 =
2.5   5
7.5  10
0.1 * v =
0.1
0.2
0.3
Doing v *= 2;
Now v =
2
4
6

转置与共轭

对矩阵的转置、共轭和共轭转置由成员函数transpose(),conjugate(),adjoint()实现

MatrixXcf a = MatrixXcf::Random(2,2);
cout << "Here is the matrix a\n" << a << endl;
cout << "Here is the matrix a^T\n" << a.transpose() << endl;
cout << "Here is the conjugate of a\n" << a.conjugate() << endl;
cout << "Here is the matrix a^*\n" << a.adjoint() << endl;

输出：

Here is the matrix a
 (-0.211,0.68) (-0.605,0.823)
 (0.597,0.566)  (0.536,-0.33)
Here is the matrix a^T
 (-0.211,0.68)  (0.597,0.566)
(-0.605,0.823)  (0.536,-0.33)
Here is the conjugate of a
 (-0.211,-0.68) (-0.605,-0.823)
 (0.597,-0.566)    (0.536,0.33)
Here is the matrix a^*
 (-0.211,-0.68)  (0.597,-0.566)
(-0.605,-0.823)    (0.536,0.33)

代码示例

#include <iostream>

using namespace std;

#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

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

  Vector3d v_3d;
  Matrix<float, 3, 1> vd_3d;

  // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
  Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零

  // 下面是对Eigen阵的操作
  // 输入数据（初始化）
  matrix_23 << 1, 2, 3, 4, 5, 6;

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

  // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
  // Matrix<double, 2, 1> 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;

  // 一些矩阵运算
  // 四则运算就不演示了，直接用+-*/即可。
  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;

  return 0;
}	

输出结果：

[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

解方程

#include <iostream>
#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

#define MATRIX_SIZE 50

int main(int argc, char **argv) {
  // 解方程
  // 我们求解 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;
}

结果如下：

time of normal inverse is 2.606ms
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 3.419ms
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 1.38ms
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

Eigen几何模块

这部分用于演练各种旋转表达方式。我们将在Eigen中使用四元数、欧拉角和旋转矩阵，演示它们之间的变换方式。

代码示例

#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>
// Eigen/Geometry 模块提供了各种旋转和平移的表示
using namespace Eigen;

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

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

  // 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;
}

输出结果：

rotation matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Transform matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v tranformed = 1.71 3.71    4
quaternion from rotation vector =     0     0 0.383 0.924
quaternion from rotation matrix =     0     0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707     0
should be equal to 0.707 0.707     0     0

Eigen中对各种形式的表达方式总结如下。每种类型都有单精度和双精度两种数据类型，不能由编译器自动转换。以双精度为例，把最后的d改成f，即得到单精度的数据结构。

• 旋转矩阵（3 × 3）：Eigen::Matrix3d。

• 旋转向量（3 × 1）：Eigen::AngleAxisd。

• 欧拉角（3 × 1）：Eigen::Vector3d。

• 四元数（4 × 1）：Eigen::Quaterniond。

• 欧氏变换矩阵（4 × 4）：Eigen::Isometry3d。

• 仿射变换（4 × 4）：Eigen::Affine3d。

• 射影变换（4 × 4）：Eigen::Projective3d。

举例演示坐标变化

设有机器人一号和机器人二号位于世界坐标系中。记世界坐标系为W，机器人的坐标系为 R1和R2。机器人一号的位姿为q1 = [0.35, 0.2, 0.3, 0.1]^T, t_1 = [0.3, 0.1, 0.1]^T。机器人二号的位姿为q2 = [−0.5, 0.4, −0.1, 0.2]^T, t2 = [−0.1, 0.5, 0.3]^T。这里的q和t表达的是世界坐标系到相机坐标系的变换关系。机器人一号看到某个点在自身的坐标系下坐标为pR1 = [0.5, 0, 0.2]^T，求该向量在机器人二号坐标系下的坐标。

#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>

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;
}

输出结果：

-0.0309731    0.73499   0.296108

计算过程如下：

注意：四元数使用之前需要归一化。

几种表达方式的相互转换

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

int main(int argc, char** argv)
{
    //旋转向量（轴角）：沿z轴旋转45度*****************************************************************************************
    Eigen::AngleAxisd rotation_vector (M_PI/4,Eigen::Vector3d(0,0,1));
    cout<<"旋转向量的旋转轴 = \n" << rotation_vector.axis()<<"\n旋转向量角度 = "<< rotation_vector.angle()<<endl;

    //旋转矩阵：沿z轴旋转45度***********************************************************************************************
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    rotation_matrix<< 0.707, -0.707, 0, 0.707, 0.707, 0, 0, 0, 1;
    cout<<"旋转矩阵 = \n"<<rotation_matrix<<endl;

    //四元数：沿z轴旋转45度************************************************************************************************
    Eigen::Quaterniond quat = Eigen::Quaterniond(0, 0, 0.383, 0.924);
    cout<<"四元数输出方法1 : 四元数 = \n"<<quat.coeffs()<<endl;
    //注意coeffs的顺序是(x,y,z,w)，w为实部，其他为虚部
    cout<<"四元数输出方法2 : 四元数 = \nx = "<<quat.x()<<"\ny = "<<quat.y()<<"\nz = "<<quat.z()<<"\nw = " <<quat.w()<<endl;
    
    //1.将旋转矩阵转换为其他形式*********************************************************************************************
    rotation_vector.fromRotationMatrix(rotation_matrix);
    cout<<"旋转矩阵转换为旋转向量方法 1: 旋转轴 = \n" << rotation_vector.axis() << "\n旋转角度 = " << rotation_vector.angle()<<endl;
    //注意: fromRotationMatrix 参数只适用于将旋转矩阵转换为旋转向量，不适用于将旋转矩阵转换为四元数
    
    rotation_vector = rotation_matrix;
    cout<<"旋转矩阵转换为旋转向量方法 2: 旋转轴 = \n" << rotation_vector.axis() << "\n旋转角度 = " << rotation_vector.angle()<<endl;

    rotation_vector = Eigen::AngleAxisd(rotation_matrix);
    cout<<"旋转矩阵转换为旋转向量方法 3: 旋转轴 = \n" << rotation_vector.axis() << "\n旋转角度 = " << rotation_vector.angle()<<endl;

    quat = Eigen::Quaterniond(rotation_matrix);
    cout<<"旋转矩阵转换为四元数方法 1: Q = \n" << quat.coeffs()<<endl;

    quat = rotation_matrix;
    cout<<"旋转矩阵转换为四元数方法 2: Q = \n" << quat.coeffs()<<endl;

    //2.将旋转向量转换为其他形式*************************************************************************************************
    cout<<"旋转向量转换为旋转矩阵方法 1: 旋转矩阵 R = \n" << rotation_vector.matrix()<<endl;
    cout<<"旋转向量转换为旋转矩阵方法 2: 旋转矩阵 R = \n" << rotation_vector.toRotationMatrix()<<endl;
    quat = Eigen::Quaterniond(rotation_vector);
    //请注意 coeffs 的顺序是(x,y,z,w),w是虚部，x,y,z是实部
    cout<<"旋转向量转换为四元数: Q = \n"<<quat.coeffs()<<endl;

    //3.将四元数转换为其他形式*************************************************************************************************
    rotation_vector = quat;
    cout<<"四元数转化为旋转向量: 旋转轴 = \n" << rotation_vector.axis()<<"\n旋转角度 = "<<rotation_vector.angle()<<endl;

    rotation_matrix = quat.matrix();
    cout<<"四元数转换为旋转矩阵方法 1: 旋转矩阵 =\n"<<rotation_matrix<<endl;

    rotation_matrix = quat.toRotationMatrix();
    cout<<"四元数转换为旋转矩阵方法 2: 旋转矩阵 =\n"<<rotation_matrix<<endl;

    return 0;
}

可视化演示

显示运动轨迹

轨迹文件记录了一个机器人的运动轨迹，存储于trajectory.txt，每一行用下面的格式存储：

其中time指该位姿的记录时间，t为平移，q为旋转四元数，均是以世界坐标系到机器人坐标系记录。
下面我们从文件中读取这些轨迹，并显示到一个窗口中。原则上，如果只是谈论“机器人的位姿”，那么可以使用T_WR或者T_RW，事实上它们也只差一个逆而已。如果想要存储机器人的轨迹，那么可以存储所有时刻的T_WR或者T_RW。

在画轨迹的时候，可以把“轨迹”画成一系列点组成的序列。这其实是机器人（相机）坐标系的原点在世界坐标系中的坐标。考虑机器人坐标系的原点，即O_R，那么，此时的O_W 就是这个原点在世界坐标系下的坐标：

O_W正是T_WR 的平移部分。因此，可以从T_WR中直接看到相机在何处，这是T_WR更为直观的原因。在可视化程序里，轨迹文件存储了T_WR而不是T_RW。

在linux 中，基于OpenGL的Pangolin库，在支持OpenGL的绘图操作基础之上还提供了一些GUI的功能。
Pangolin库的下载以及配置参考以下博客：
Ubuntu中pangolin库的安装配置及问题解决

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>

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

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
  }
}

位姿可视化结果：

程序演示了如何在Panglin中画出3D的位姿。用红、绿、蓝三种颜色画出每个位姿的三个坐标轴（计算了各坐标轴的世界坐标），然后用黑色线将轨迹连起来。

引用

Eigen库学习教程(全)

Eigen库官网