《视觉SLAM十四讲》读书笔记（一）

本着重温知识和巩固基础的目的，我最近重新精读一次《十四讲》。考虑到脑子不好使，每次看完就忘记，因此开一个专栏专门用于记录重要知识点。由于本身具备一定的SLAM系统开发经验和理论知识，因此一些太过基础的知识会适当省略过。

3 三维空间刚体运动

知识点：

1. 三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角
2. Eigen库的矩阵、几何模块的使用

3.1 旋转矩阵

3.1.1 点、向量和坐标系

基$[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3]$与坐标$[a_1,a_2,a_3{]}^T$组成向量$\mathbf{a}$：
$$\mathbf{a}=\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{\mathbf{e}}_1+a_2{\mathbf{e}}_2+a_3{\mathbf{e}}_3\text{\hspace{1em}(3.1)}$$

向量内积（点乘）：

$$\mathbf{a}\cdot \mathbf{b}={\mathbf{a}}^{\mathrm{T}}\mathbf{b}=\sum _{i=1}^3{a}_i{b}_i=|\mathbf{a}||\mathbf{b}|\cos ⟨\mathbf{a},\mathbf{b}⟩\text{\hspace{1em}(3.2)}$$

向量外积（叉乘）：
$$\mathbf{a}\times \mathbf{b}=\Vert \begin{array}{lll}{\mathbf{e}}_1& e_2& e_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert =\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]\mathbf{b}\stackrel{\text{ def }}{=}{\mathbf{a}}^{\wedge }\mathbf{b}\text{\hspace{1em}(3.3)}$$
外积的结果是一个向量，它的方向垂直于这两个向量，大小为$|\mathbf{a}||\mathbf{b}|\sin ⟨\mathbf{a},\mathbf{b}⟩$，是两个向量张成的四边形的有向面积。

基于外积，一种向量到矩阵的一一映射关系被定义，反对称矩阵：
$${\mathbf{a}}^{\wedge }={\left[\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]\right]}^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\text{\hspace{1em}(3.4)}$$
注：务必务必记住(3.4)的矩阵形式，在后面公式推导中十分重要。

3.1.2 坐标系间的欧式变换

刚体运动的解释：两个坐标系之间，由一个旋转和一个平移组成的运动。

欧式变换的解释：由旋转和平移组成的一种变换关系。

旋转矩阵的定义：
设单位正交基$\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]$经过一次旋转后变成了$\left[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right]$。对于同一个向量$\mathbf{a}$，它在两个坐标系下的坐标为$[a_1,a_2,a_3]$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }]$，由于向量本身没变，所以存在如下约束：
$$\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\text{\hspace{1em}(3.5)}$$
左右两边同事左乘$\left[\begin{array}{c}{\mathbf{e}}_1^{\mathrm{T}}\\ {\mathbf{e}}_2^{\mathrm{T}}\\ {\mathbf{e}}_3^{\mathrm{T}}\end{array}\right]$
$$\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{lll}{\mathbf{e}}_1^{\mathrm{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^{\mathrm{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^{\mathrm{T}}{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^{\mathrm{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^{\mathrm{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^{\mathrm{T}}{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^{\mathrm{l}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^{\mathrm{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^{\mathrm{T}}{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{l}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\stackrel{\text{ def }}{=}\mathbf{R}{\mathbf{a}}^{\prime }\text{\hspace{1em}(3.6)}$$
旋转矩阵$\mathbf{R}$各分量是两个坐标系基的内积，由于基向量的长度为1，因此其实际上是各基向量夹角的余弦值。所以这个矩阵也叫方向余弦。

旋转矩阵特性总结：

1. 矩阵各分量是各基向量夹角的余弦值；
2. 它是行列式为1的正交矩阵；
3. ${\mathbf{R}}^{-1}={\mathbf{R}}^T$

Eigen关于旋转矩阵的定义和计算

1. 初始化旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

2. 旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);


Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

3. 旋转矩阵转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

4. 旋转矩阵转四元数

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion;quaternion=rotation_matrix;

欧氏空间的坐标变换：
$${\mathbf{a}}_1={\mathbf{R}}_{12}{\mathbf{a}}_2+{\mathbf{t}}_{12}\text{\hspace{1em}(3.10)}$$
${\mathbf{R}}_{12}$：从坐标系2到坐标系1的旋转，或者把坐标系2的向量变换到坐标系1中。
${\mathbf{t}}_{12}$：从坐标系2到坐标系1的向量，或者坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标。

记忆方法：向量乘在矩阵的右边，它的下标是从右读到左的。

3.1.3 变换矩阵和齐次坐标

齐次坐标和变换矩阵的表达形式：
$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ll}R& t\\ 0^{\mathrm{T}}& 1\end{array}\right]\left[\begin{array}{l}a\\ 1\end{array}\right]\stackrel{\text{ def }}{=}\mathbf{T}\left[\begin{array}{l}a\\ 1\end{array}\right]\text{\hspace{1em}(3.11)}$$

齐次变换的逆：
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathrm{T}}& -{\mathbf{R}}^{\mathrm{T}}t\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(3.14)}$$

3.2 实践：Eigen

#include <iostream>

using namespace std;

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

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

  // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
  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;

  // 同样你不能搞错矩阵的维度
  // 试着取消下面的注释，看看Eigen会报什么错
  // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * 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;
}

3.3 旋转向量和欧拉角

3.3.1 旋转向量

旋转轴为一个单位长度的向量$\mathbf{n}$，角度为$\theta$。

旋转向量特性：旋转轴上的向量在旋转后不发生改变
$$\mathbf{R}\mathbf{n}=\mathbf{n}\text{\hspace{1em}(3.18)}$$
转轴$\mathbf{n}$是矩阵$\mathbf{R}$特征值1对应的特征向量。

求特征向量的方法：先求特征值，再一一代入方程$(\mathbf{R}-\mathbf{I})\mathbf{n}=0$求特征向量。

旋转向量$\to$旋转矩阵

$$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge }\text{\hspace{1em}(3.15)}$$

旋转矩阵$\to$旋转向量

转角：
$$\begin{array}{rl}\mathrm{tr}(\mathbf{R})=& \cos \theta \mathrm{tr}(\mathbf{I})+(1-\cos \theta )\mathrm{tr}\left(\mathbf{n}{\mathbf{n}}^{\mathrm{T}}\right)+\sin \theta \mathrm{tr}\left({\mathbf{n}}^{\wedge }\right)\\ =& 3\cos \theta +(1-\cos \theta )\\ =& 1+2\cos \theta \end{array}\text{\hspace{1em}(3.16)}$$
$$\theta =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2}\text{\hspace{1em}(3.17)}$$
转轴：
转轴$\mathbf{n}$是矩阵$\mathbf{R}$特征值1对应的特征向量。

Eigen关于旋转向量的定义和计算

一、旋转向量

1. 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))

2. 旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

3. 旋转向量转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

4. 旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion;Quaterniond quaternion;

Eigen::Quaterniond quaternion;quaternion=rotation_vector;

3.3.2 欧拉角

欧拉角是一种非常直观的描述旋转的方式。太基础的知识我们不展开说，着重说下旋转顺序的影响。

例子：XYZ轴的旋转定义：先绕X轴，再绕Y轴，最后绕Z轴旋转。同理，可以定义ZYZ，ZYX等旋转方式。如果讨论得更细一些，则还需要区分每次是绕固定轴旋转的（基于全局坐标系，计算是从右到左的乘法），还是绕旋转之后的轴旋转的（基于基体坐标系，计算是从左到右的乘法）。

实际上，这种定义方式的不确定性会给我们的计算带来许多麻烦。所幸在特定领域，欧拉角通常有统一的定义方式：偏航-俯仰-翻滚 (yaw-pitch-roll)。它等价于ZYX轴的旋转(绕旋转之后的轴)。此时，可以使用$[r,p,y{]}^T$这样一个三维向量描述任意旋转。ZYX转角相当于把任意旋转分解成以下3各轴上的转角：

1. 绕物体的Z轴旋转，得到偏航角yaw
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch
3. 绕旋转之后的X轴旋转，得到翻滚角roll

欧拉角$\to$旋转矩阵

绕不同轴旋转的基础旋转矩阵：
$$R_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right],R_y=\left[\begin{array}{ccc}\cos \varphi & 0& \sin \varphi \\ 0& 1& 0\\ -\sin \varphi & 0& \cos \varphi \end{array}\right],R_z=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$
假设有一个坐标系 $\mathbf{b}$（其上有一个点 $\mathbf{p}$（点和向量是空间中一样东西，只有当选取坐标系时才讨论它的的坐标），坐标也用$\mathbf{p}$ 表示），它要绕X轴逆时针转$\mathbf{\alpha }$ 角度，那么在旋转之后的坐标系下点$\mathbf{p}$的坐标（记为${\mathbf{p}}^{\prime }$）变成了多少？做一下数学转换得到：
$$R_x\ast p=p^{\prime }$$
转换一下得到
$$p=R_x^T\ast p^{\prime }$$

1. （绕固定轴旋转的ZYX转角，或者说是基于全局坐标系）当从$\mathbf{w}$系依次进行ZYX顺序的旋转得到$\mathbf{b}$系，那么根据上式可得$R_{bw}$
   $$\begin{array}{rl}{R}_{bw}& =[R_z\ast R_y\ast R_x{]}^T=R_x^T\ast R_y^T\ast R_z^T\\ & ={\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]}^T\ast {\left[\begin{array}{ccc}\cos \varphi & 0& \sin \varphi \\ 0& 1& 0\\ -\sin \varphi & 0& \cos \varphi \end{array}\right]}^T\ast {\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]}^T\\ & =\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & \sin \theta \\ 0& -\sin \theta & \cos \theta \end{array}\right]\ast \left[\begin{array}{ccc}\cos \varphi & 0& -\sin \varphi \\ 0& 1& 0\\ \sin \varphi & 0& \cos \varphi \end{array}\right]\ast \left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\end{array}$$
2. （绕旋转之后的轴旋转的ZYX转角，或者说是基于基体坐标系）从$\mathbf{w}$系变换到$\mathbf{b}$系所进行的ZYX顺序的旋转的含义是什么呢？我们知道$t_{wb}$ 是$\mathbf{b}$系的坐标原点在$\mathbf{w}$系下的坐标，那么此处的ZYX顺序的旋转就是指：$\mathbf{b}$系在$\mathbf{w}$系下的欧拉角，表示$\mathbf{w}$系按照此欧拉角顺序旋转即可得到$\mathbf{b}$系！既然是$\mathbf{b}$系在$\mathbf{w}$系下的欧拉角，那么我们求解的应该是$R_{wb}$，所以对$R_{bw}$做转置，得到 $R_{wb}$ ，如下：
   $$\begin{gathered} \begin{array}{rl}{R}_{wb}=R_{bw}^T& =R_z\ast R_y\ast R_x\\ & =\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{ccc}\cos \varphi & 0& \sin \varphi \\ 0& 1& 0\\ -\sin \varphi & 0& \cos \varphi \end{array}\right]\ast \left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]\end{array} \\ =\left[\begin{array}{ccc}\cos \varphi \ast \cos \psi & \cos \psi \ast \sin \varphi \ast \sin \theta -\cos \theta \ast \sin \psi & \sin \theta \ast \sin \psi +\cos \theta \ast \cos \psi \ast \sin \varphi \\ \cos \varphi \ast \sin \psi & \cos \theta \ast \cos \psi +\sin \varphi \ast \sin \theta \ast \sin \psi & \cos \theta \ast \sin \varphi \ast \sin \psi -\cos \psi \ast \sin \theta \\ -\sin \varphi & \cos \varphi \ast \sin \theta & \cos \varphi \ast \cos \theta \end{array}\right] \end{gathered}$$

旋转矩阵$\to$欧拉角

由于旋转矩阵转欧拉角的过程非常多变，而且主要是通过上式进行反三角运算，我更推荐大家细读Eigen中的旋转矩阵$\to$欧拉角转换代码

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_EULERANGLES_H
#define EIGEN_EULERANGLES_H

namespace Eigen { 

/** \geometry_module \ingroup Geometry_Module
  *
  *
  * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2)
  *
  * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.
  * For instance, in:
  * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
  * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
  * we have the following equality:
  * \code
  * mat == AngleAxisf(ea[0], Vector3f::UnitZ())
  *      * AngleAxisf(ea[1], Vector3f::UnitX())
  *      * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
  * This corresponds to the right-multiply conventions (with right hand side frames).
  * 
  * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
  * 
  * \sa class AngleAxis
  */
template<typename Derived>
EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
// a0, a1, a2指旋转轴（内部旋转），例如(2,1,0)表示旋转顺序为Z-Y-X, 
//(2,0,2)表示旋转顺序为Z-X-Z
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
{
  EIGEN_USING_STD_MATH(atan2)
  EIGEN_USING_STD_MATH(sin)
  EIGEN_USING_STD_MATH(cos)
  /* Implemented from Graphics Gems IV */
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)

  //存储结果，假设我们下面旋转顺序为Z-Y-X, 输入参数为(2,1,0)
  // 所以res存储顺序为 res = (yaw, pitch, roll)
  Matrix<Scalar,3,1> res;
  typedef Matrix<typename Derived::Scalar,2,1> Vector2;

  const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
  const Index i = a0;
  const Index j = (a0 + 1 + odd)%3;
  const Index k = (a0 + 2 - odd)%3;
  // odd=1, i=2, j=1, k=0
  
  //先不管这种情况
  if (a0==a2)
  {
    res[0] = atan2(coeff(j,i), coeff(k,i));
    if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0)))
    {
      if(res[0] > Scalar(0)) {
        res[0] -= Scalar(EIGEN_PI);
      }
      else {
        res[0] += Scalar(EIGEN_PI);
      }
      Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
      res[1] = -atan2(s2, coeff(i,i));
    }
    else
    {
      Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
      res[1] = atan2(s2, coeff(i,i));
    }
    
    // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
    // we can compute their respective rotation, and apply its inverse to M. Since the result must
    // be a rotation around x, we have:
    //
    //  c2  s1.s2 c1.s2                   1  0   0 
    //  0   c1    -s1       *    M    =   0  c3  s3
    //  -s2 s1.c2 c1.c2                   0 -s3  c3
    //
    //  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
    
    Scalar s1 = sin(res[0]);
    Scalar c1 = cos(res[0]);
    res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j));
  } 
  else
  {
    // yaw = res[0]
    res[0] = atan2(coeff(j,k), coeff(k,k));
    // 计算cos(pitch)
    Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm();
    if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) {
      if(res[0] > Scalar(0)) {
        res[0] -= Scalar(EIGEN_PI);
      }
      else {
        res[0] += Scalar(EIGEN_PI);
      }
      res[1] = atan2(-coeff(i,k), -c2);
    }
    else
      res[1] = atan2(-coeff(i,k), c2);
    // 计算sin(yaw)
    Scalar s1 = sin(res[0]);
    // 计算cos(yaw)
    Scalar c1 = cos(res[0]);
    // roll = res[2]
    res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j));
  }
  if (!odd)
    res = -res;
  
  return res;
}

} // end namespace Eigen

#endif // EIGEN_EULERANGLES_H

万向锁问题

欧拉角的重大缺点：万向锁问题（Gimbal Lock）：
在俯仰角为±90°时，第一次旋转和第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转）。欧拉角不适用于插值和迭代，往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角表示姿态（当然，LOAM用了大量的欧拉角表示姿态…，毕竟人家是大佬 ），同样不会在滤波或优化中使用欧拉角表示旋转（因为它具有奇异性）。

注意：上图的旋转方式都是绕旋转之后的轴旋转的。

Eigen关于欧拉角的定义和计算

1. 初始化欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle(yaw,pitch,roll);

2. 欧拉角转旋转向量

Eigen::AngleAxisd rollAngle(eulerAngle(2),Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(eulerAngle(1),Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(eulerAngle(0),Eigen::Vector3d::UnitZ()); 
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

3. 欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(eulerAngle(2),Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(eulerAngle(1),Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(eulerAngle(0),Eigen::Vector3d::UnitZ()); 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

4. 欧拉角转四元数

Eigen::AngleAxisd rollAngle(eulerAngle(2),Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(eulerAngle(1),Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(eulerAngle(0),Eigen::Vector3d::UnitZ()); 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

3.4 四元数

3.4.1 四元数的定义与表达方式

$$\mathbf{q}=q_0+q_1\mathrm{i}+{\mathrm{q}}_2\mathrm{j}+{\mathrm{q}}_3\mathrm{k}\text{\hspace{1em}(3.19)}$$
$$\{\begin{array}{l}{\mathrm{i}}^2={\mathrm{j}}^2={\mathrm{k}}^2=-1\\ \mathrm{i}\mathrm{j}=\mathrm{k},\mathrm{j}\mathrm{i}=-\mathrm{k}\\ \mathrm{j}\mathrm{k}=\mathrm{i},\mathrm{k}\mathrm{j}=-\mathrm{i}\\ \mathrm{k}\mathrm{i}=\mathrm{j},\mathrm{i}\mathrm{k}=-\mathrm{j}\end{array}\text{\hspace{1em}(3.20)}$$

也可以用一个标量和一个向量来表达四元数：
$$\mathbf{q}=[s,\mathbf{v}{]}^{\mathrm{T}},s=q_0\in \mathbb{R},\mathbf{v}={\left[q_1,q_2,q_3\right]}^{\mathrm{T}}\in {\mathbb{R}}^3.$$

可以用单位四元数表示三维空间中任意一个旋转。例如，乘以$i$对应着旋转180°，这样才能保证$ij=k$的性质。而$i^2=-1$，意味着绕$i$轴旋转360°后得到一个相反的东西。这个东西要旋转两周才会和它原先的样子相等。

3.4.2 四元数的运算

现有两个四元数${\mathbf{q}}_a$，${\mathbf{q}}_b$，它们的向量表示为$[s_a,{\mathbf{v}}_a{]}^{\mathrm{T}}$，$[s_b,{\mathbf{v}}_b{]}^{\mathrm{T}}$，或者原始四元数表示为：
$${\mathbf{q}}_a=s_a+x_a\mathrm{i}+{\mathrm{y}}_{\mathrm{a}}\mathrm{j}+{\mathrm{z}}_{\mathrm{a}}\mathrm{k},{\mathrm{q}}_{\mathrm{b}}={\mathrm{s}}_{\mathrm{b}}+{\mathrm{x}}_{\mathrm{b}}\mathrm{i}+{\mathrm{y}}_{\mathrm{b}}\mathrm{j}+{\mathrm{z}}_{\mathrm{b}}\mathrm{k}$$

1. 加法和减法
   $${\mathbf{q}}_a\pm {\mathbf{q}}_b={\left[s_a\pm s_b,{\mathbf{v}}_a\pm {\mathbf{v}}_b\right]}^{\mathrm{T}}\text{\hspace{1em}(3.22)}$$

2. 乘法（不等价于旋转）
   把${\mathbf{q}}_a$的每一项与${\mathbf{q}}_b$相乘，最后相加：
   $$\begin{array}{rl}{\mathbf{q}}_a{\mathbf{q}}_b=& s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +\left(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b\right)\mathrm{i}\\ & +\left(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b\right)\mathrm{j}\\ & +\left(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b\right)\mathrm{k}\end{array}\text{\hspace{1em}(3.23)}$$
   简洁形式：
   $${\mathbf{q}}_a{\mathbf{q}}_b={\left[s_a{s}_b-{\mathbf{v}}_a^{\mathrm{T}}{\mathbf{v}}_b,s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\right]}^{\mathrm{T}}\text{\hspace{1em}(3.24)}$$

3. 模长
   $$\Vert {\mathbf{q}}_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}=\sqrt{s_a^2+{\mathbf{v}}^{\mathrm{T}}\mathbf{v}}\text{\hspace{1em}(3.25)}$$
   单位四元数特点：
   $$\Vert {\mathbf{q}}_a{\mathbf{q}}_b\Vert =\Vert {\mathbf{q}}_a\Vert \Vert {\mathbf{q}}_b\Vert \text{\hspace{1em}(3.26)}$$

4. 共轭
   虚部取成相反数：
   $${\mathbf{q}}_a^{\ast }=s_a-x_a\mathrm{i}-{\mathrm{y}}_{\mathrm{a}}\mathrm{j}-{\mathrm{z}}_{\mathrm{a}}\mathrm{k}={\left[{\mathrm{s}}_{\mathrm{a}},-{\mathrm{v}}_{\mathrm{a}}\right]}^{\mathrm{T}}\text{\hspace{1em}(3.27)}$$
   四元数共轭与其本身相乘，会得到一个实四元数，实部为模长的平方：
   $${\mathbf{q}}^{\ast }\mathbf{q}=\mathbf{q}{\mathbf{q}}^{\ast }={\left[s_a^2+{\mathbf{v}}^{\mathrm{T}}\mathbf{v},0\right]}^{\mathrm{T}}\text{\hspace{1em}(3.28)}$$

5. 逆
   $${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/\Vert \mathbf{q}{\Vert }^2\text{\hspace{1em}(3.29)}$$
   四元数和自己的逆的乘积为实四元数1：
   $$\mathbf{q}{\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\mathbf{q}=1\text{\hspace{1em}(3.30)}$$
   如果${\mathbf{q}}_a$是单位四元数，有：
   $${\mathbf{q}}^{\ast }={\mathbf{q}}^{-1}\text{\hspace{1em}(3.31-a)}$$
   乘积的逆具有和矩阵相似的性质：
   $${\left({\mathbf{q}}_a{\mathbf{q}}_b\right)}^{-1}={\mathbf{q}}_b^{-1}{\mathbf{q}}_a^{-1}\text{\hspace{1em}(3.31-b)}$$

6. 数乘
   $$k\mathbf{q}=[ks,k\mathbf{v}{]}^T\text{\hspace{1em}(3.32)}$$

3.4.3 用四元数表示旋转

一个空间三位点$\mathbf{p}=[x,y,z]\in {\mathbb{R}}^3$，经过单位四元数$\mathbf{q}$旋转作用后得到${\mathbf{p}}^{\prime }$，计算过程如下：

把三维空间点用一个虚四元数来描述
$$\mathbf{p}=[0,x,y,z{]}^{\mathrm{T}}=[0,\mathbf{v}{]}^{\mathrm{T}}.$$
旋转后的点${\mathbf{p}}^{\prime }$可表示为如下四元数乘积
$${\mathbf{p}}^{\prime }=\mathbf{q}{\mathbf{p}\mathbf{q}}^{-1}\text{\hspace{1em}(3.33)}$$
${\mathbf{p}}^{\prime }$计算结果必为纯虚四元数。虚部即为旋转之后点的坐标。

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

首先，四元数乘法可以写成一种矩阵的乘法。设$\mathbf{q}=[s,\mathbf{v}{]}^T$，定义如下的符号${}^{+}$和${}^{\oplus }$为
$$q^{+}=\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right],\text{\hspace{1em}(3.34-a)}$$
$$q^{\oplus }=\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ \mathbf{v}& s\mathbf{I}-{\mathbf{v}}^{\wedge }\end{array}\right]\text{\hspace{1em}(3.34-b)}$$

这两个符号将四元数映射成为一个$4\times 4$的矩阵，四元数乘法可以写成矩阵的形式

$${\mathbf{q}}_1^{+}{\mathbf{q}}_2=\left[\begin{array}{cc}{s}_1& -{\mathbf{v}}_1^{\mathrm{T}}\\ {\mathbf{v}}_1& s_1\mathbf{I}+{\mathbf{v}}_1^{\wedge }\end{array}\right]\left[\begin{array}{l}{s}_2\\ {\mathbf{v}}_2\end{array}\right]=\left[\begin{array}{c}-{\mathbf{v}}_1^{\mathrm{T}}{\mathbf{v}}_2+s_1{s}_2\\ s_1{\mathbf{v}}_2+s_2{\mathbf{v}}_1+{\mathbf{v}}_1^{\wedge }{\mathbf{v}}_2\end{array}\right]={\mathbf{q}}_1{\mathbf{q}}_2\text{\hspace{1em}(3.35)}$$
同理亦可证
$${\mathbf{q}}_1{\mathbf{q}}_2={\mathbf{q}}_1^{+}{\mathbf{q}}_2={\mathbf{q}}_2^{\oplus }{\mathbf{q}}_1\text{\hspace{1em}(3.36)}$$
前面说到的空间三维点旋转问题可转化为：
$$\begin{array}{rl}{\mathbf{p}}^{\prime }& =\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}={\mathbf{q}}^{+}{\mathbf{p}}^{+}{\mathbf{q}}^{-1}\\ & ={\mathbf{q}}^{+}({\mathbf{q}}^{-1}{)}^{\oplus }\mathbf{p}\end{array}\text{\hspace{1em}(3.37)}$$
其中两个符号对应的矩阵：
$${\mathbf{q}}^{+}{\left({\mathbf{q}}^{-1}\right)}^{\oplus }=\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]\left[\begin{array}{cc}s& {\mathbf{v}}^{\mathrm{T}}\\ -\mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0^{\mathrm{T}}& \mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+{\left({\mathbf{v}}^{\wedge }\right)}^2\end{array}\right]\text{\hspace{1em}(3.38)}$$

四元数$\to$旋转矩阵

因为 ${\mathbf{p}}^{\prime }$ 和 $\mathbf{p}$ 都是虚四元数, 所以事实上该矩阵的右下角即给出了从四元数到旋转矩阵的变换关系：
$$\mathbf{R}=\mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+{\left({\mathbf{v}}^{\wedge }\right)}^2\text{\hspace{1em}(3.39)}$$
为了得到四元数到旋转向量的转换公式, 对上式两侧求迹, 得
$$\begin{array}{rl}\mathrm{tr}(\mathbf{R})& =\mathrm{tr}(\mathbf{v}{\mathbf{v}}^{\mathrm{T}}+3s^2+2s\cdot 0+\mathrm{tr}\left({\left({\mathbf{v}}^{\wedge }\right)}^2\right)\\ & =v_1^2+v_2^2+v_3^2+3s^2-2\left(v_1^2+v_2^2+v_3^2\right)\\ & =\left(1-s^2\right)+3s^2-2\left(1-s^2\right)\\ & =4s^2-1\end{array}\text{\hspace{1em}(3.40)}$$
由式 (3.17) 得
$$\begin{array}{rl}\theta & =\arccos \frac{\mathrm{tr}(\mathbf{R}-1)}{2}\\ & =\arccos \left(2s^2-1\right)\end{array}\text{\hspace{1em}(3.41)}$$
即
$$\cos \theta =2s^2-1=2{\cos }^2\frac{\theta }{2}-1\text{\hspace{1em}(3.42)}$$
所以有
$$\theta =2\arccos s.\text{\hspace{1em}(3.43)}$$

四元数$\to$旋转向量

总结，四元数到旋转向量的转换公式如下：
$$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^{\mathrm{T}}={\left[q_1,q_2,q_3\right]}^{\mathrm{T}}/\sin \frac{\theta }{2}\end{array}\text{\hspace{1em}(3.44)}$$

四元数$\to$欧拉角

一般是先让四元数转旋转矩阵后，再转欧拉角。

Eigen关于欧拉角的定义和计算

1. 初始化四元数

Eigen::Quaterniond quaternion(w,x,y,z);

2. 四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector;rotation_vector=quaternion;

3. 四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.toRotationMatrix();

4. 四元数转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

3.5 相似、仿射、射影变换

等距变换（欧氏变换）

$$\mathbf{T}=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]$$

Eigen关于欧氏变换的定义

欧氏变换矩阵(4 × 4) : Eigen::Isometry3d
等距变换(Isometry Transform)可以看作是维持任意两点距离不变的仿射变换，也称做欧氏变换、刚体运动，在实际场景中使用比较多。在Eigen中已经内置好了一些常用的等距变换可以直接调用，如下。

/** \ingroup Geometry_Module */
typedef Transform<float,2,Isometry> Isometry2f;
/** \ingroup Geometry_Module */
typedef Transform<float,3,Isometry> Isometry3f;
/** \ingroup Geometry_Module */
typedef Transform<double,2,Isometry> Isometry2d;
/** \ingroup Geometry_Module */
typedef Transform<double,3,Isometry> Isometry3d;

新建方法十分简单，直接类型名+变量名即可，赋值可以通过它的成员函数.rotate()和.translate()完成(须先初始化为单位阵)。三维等距变换是一个4×4的矩阵。但在构建一个变换前，你首先要想清楚两个问题：这个变换顺序 (先平移后旋转 or 先旋转后平移) 是什么？这个变换相对谁改变 (固定轴 or 自身)？不同的答案会得到不同的结果，代码也不尽相同。以下是一个标准的新建并初始化按固定轴先平移再旋转变换的例子：

# include<iostream>
# include<Eigen\Dense>
# include<Eigen\Geometry>

using namespace std;
using namespace Eigen;

int main(){
	AngleAxisd rotation(3.1415926 / 4, Vector3d(1, 0, 1).normalized());
	Vector3d translation(1, 3, 4);
	Isometry3d T= Isometry3d::Identity();

	T.translate(translation);
	T.rotate(rotation);
	
	return 0;
}

上述代码中构建了一个角轴表示的旋转、一个平移向量以及一个初始的单位变换。注意这里必须要调用Isometry3d::Identity()对它进行初始化(或者调用.setIdentity()设置)，后面的.translate()和.rotate()函数才有用。因为这些函数本质上都是矩阵乘法，而如果没初始化，Isometry3d中的元素全为0(或者非常大的负数)，这样等于是和0矩阵相乘，结果会不对。

如果你要说我就不初始化为单位阵行不行？也可以，那就不要调用.translate()和.rotate()函数，直接调用.matrix()函数获取对应的变换矩阵，然后手动自己设置(构造)，像下面这样。

# include<iostream>
# include<Eigen\Dense>
# include<Eigen\Geometry>

using namespace std;
using namespace Eigen;

int main(){
	AngleAxisd rotation(3.1415926 / 4, Vector3d(1, 0, 1).normalized());
	Vector3d translation(1, 3, 4);
	Isometry3d T;

	// 设置旋转R部分
	T.matrix().block(0, 0, 3, 3) = rotation.toRotationMatrix();
	// 设置平移t部分，.homogeneous()函数用于将平移向量变成对应的齐次表达形式
	T.matrix().col(3) = translation.homogeneous();
	// 设置最后一行
	T.matrix().row(3) = Vector4d(0, 0, 0, 1);

	cout << "rotation matrix:" << endl << rotation.toRotationMatrix() << endl << endl;
	cout << "translation vector:" << endl << translation << endl << endl;
	cout << "translation matrix:" << endl << T.matrix() << endl << endl;
	system("pause");
	return 0;
}

上面两种方式演示了基本的新建变换的步骤，可以发现很多成员函数如.translate()、.rotate()、.pretranslate()、.prerotate()等起了关键作用，这些函数是什么意思、怎么用，下面简单进行说明。

这些函数准确来说是用来“应用变换”的，变换在线代里即为矩阵相乘，这些函数将当前变换与输入的参数相乘，从而得到新的变换。有且仅当变换为单位阵时，这样等价于对变换赋值。A.translate(B)等价于A×B，而A.pretranslate(B)等价于B×A，对应于左乘和右乘的区别。

在应用角度而言，回到之前说的两个问题，这些函数就与它们有关。简单来说就是，凡是前面带pre的函数，其变化都是相对于上一步变化之前的状态进行的。这样说可能有些绕，举例说就是我要新建一个按固定轴平移旋转(世界坐标系下)、先平移后旋转的变换。但我首先设置了旋转，然后再设置平移。这个时候设置平移就不能用.translate()了，而应该用.pretranslate()。因为第一步已经对坐标系进行了旋转，后面的平移是在旋转后的坐标系中进行的，这显然和我设想的不一样。我需要在未旋转的坐标系上(世界坐标系)进行平移。下面代码演示了这种现象：

# include<iostream>
# include<Eigen\Dense>
# include<Eigen\Geometry>

using namespace std;
using namespace Eigen;

int main(){
	AngleAxisd rotation(3.1415926 / 4, Vector3d(1, 0, 1).normalized());
	Vector3d translation(1, 3, 4);
	Isometry3d T1 = Isometry3d::Identity();
	Isometry3d T2 = Isometry3d::Identity();
	Isometry3d T3 = Isometry3d::Identity();

	T1.translate(translation);
	T1.rotate(rotation);

	T2.rotate(rotation);
	T2.pretranslate(translation);

	T3.rotate(rotation);
	T3.translate(translation);

	cout << "rotation matrix:" << endl << rotation.toRotationMatrix() << endl << endl;
	cout << "translation vector:" << endl << translation << endl << endl;
	cout << "translation matrix1:" << endl << T1.matrix() << endl << endl;
	cout << "translation matrix1:" << endl << T2.matrix() << endl << endl;
	cout << "translation matrix1:" << endl << T3.matrix() << endl << endl;
	system("pause");
	return 0;
}

在代码中建了3个变换，其中1和2是等价的，而3的平移部分作为对比没有使用.pretranslate()，可以看到和我们预期的不一样。其实这等于是按自身进行变换了，这在欧拉角的旋转中倒是有用。之前说过欧拉角有两种不同的定义方式：按自身旋转和按固定轴旋转。

常用的一些成员函数列举如下：

.translation():无参数，返回当前变换平移部分的向量表示(可修改)，可以索引[]获取各分量

.translationExt():无参数，如果当前变换是仿射的话，返回平移部分(可修改)；如果是射影变换的话，返回最后一列(可修改)

.translate():有参数，用于在当前变换上应用右乘，A.translate(B)等价于A×B

.pretranslate():有参数，用于在当前变换上应用左乘，A.pretranslate(B)等价于B×A

.rotation():无参数，返回只读的当前变换的旋转部分，以旋转矩阵表示

.rotate():有参数，用于设置当前变换的旋转部分，输入参数可以为角轴、四元数、旋转矩阵等。A.rotate(B)等价于A×B

.prerotate():有参数，用于设置当前变换的旋转部分，输入参数可以为角轴、四元数、旋转矩阵等。A.prerotate(B)等价于B×A

.matrix():返回变换对应的矩阵(可修改)

.cols():变换对应矩阵的列数

.rows():变换对应矩阵的行数

.linear()&.linearExt():返回变换的线性部分，对于Isometry而言就是旋转对应的旋转矩阵

.inverse():返回当前变换的逆变换

.Identity():返回一个单位变换，一般可用于Isometry的初始化中，将变换设为一个单位阵：Isometry3d::Identity()

.setIdentity():将当前变换变成单位变换，变换对应的矩阵变成单位阵，也可用于初始化赋值

.cast():将当前类型的变换转换为其它类型的变换，如Isometry3d转Isometry3f

.data():返回一个指向变换内部矩阵的指针，矩阵按照列优先存储

.computeRotationScaling():将当前变换的线性部分分解成rotation和scaling的乘积

.computeScalingRotation():将当前变换的线性部分分解成scaling和rotation的乘积

.fromPositionOrientationScale():从一个3D对象的位置、指向以及缩放来设置一个变换

.prescale():有参数，用于在当前变换上应用右乘，A.translate(B)等价于A×B

.scale():有参数，用于在当前变换上应用右乘，A.translate(B)等价于A×B

.preshear():只针对2D情况，沙尔变换，有参数，用于在当前变换上应用左乘，A.preshear(B)等价于B×A

.shear():只针对2D情况，沙尔变换，有参数，用于在当前变换上应用右乘，A.shear(B)等价于A×B

.affine():无参数，返回当前变换的仿射部分(可修改)，对于Isometry3d而言，返回的是一个4×3的矩阵

makeAffine():将变换对应的矩阵的最后一行设为0,0,…,1

.isApprox():判断当前Isometry是否与给定Isometry近似相等，是的话返回true。判断精度由传入的第二个参数决定。

简单使用示例如下：

# include<iostream>
# include<Eigen\Dense>
# include<Eigen\Geometry>

using namespace std;
using namespace Eigen;

int main(){
	Isometry3d transform;
	transform.setIdentity();

	transform.translate(Vector3d(1.5, 2, 3));
	transform.rotate(AngleAxisd(0.1, Vector3d::UnitX()));
	cout << "transform matrix:\n" << transform.matrix() << endl << endl;

	Vector3d vec1(4.3, 5, 3.2),vec2;
	vec2 = transform*vec1;
	cout << vec1 << endl << endl;
	cout << vec2 << endl;
	system("pause");
	return 0;
}

构造了一个三维刚体变换并将其应用到了vec1这个向量上。

相似变换

相似变换的旋转部分多一个缩放因子$s$，表示我们在对向量旋转之后，可以在$x,y,z$三个坐标上进行均匀缩放。由于含有缩放，相似变换不再保持图形的面积不变。
$${\mathbf{T}}_S=\left[\begin{array}{ll}s\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]$$
一个边长未1的立方体通过相似变换后，变成边长为10的样子（但仍然是立方体）。三维相似变换的集合也叫作相似变换群，记作$Sim(3)$

仿射变换

$${\mathbf{T}}_A=\left[\begin{array}{ll}\mathbf{A}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]$$
与欧式变换不同的是，仿射变换只要求$\mathbf{A}$是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫做正交投影。经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。

Eigen关于仿射变换的定义

仿射变换(4 × 4) : Eigen::Affine3d

仿射变换是空间直角坐标变换的一种，保留线的平行性。仿射变换一般按照TRSI的顺序进行构建，T-translation，R-rotation，S-scaling，I-identity。

Transform<float,N,Affine> t = concatenation_of_any_transformations;
Transform<float,3,Affine> t = Translation3f(p) * AngleAxisf(a,axis) * Scaling(s);

为了方便Eigen中已经定义好了一些常用类型，如下：

/** \ingroup Geometry_Module */
typedef Transform<float,2,Affine> Affine2f;
/** \ingroup Geometry_Module */
typedef Transform<float,3,Affine> Affine3f;
/** \ingroup Geometry_Module */
typedef Transform<double,2,Affine> Affine2d;
/** \ingroup Geometry_Module */
typedef Transform<double,3,Affine> Affine3d;

由于仿射变换的成员函数和Isometry是一模一样的，这里就不再介绍了。下面是一个简单的使用示例。

# include<iostream>
# include<Eigen\Dense>
# include<Eigen\Geometry>

using namespace std;
using namespace Eigen;

int main(){
	Affine2d affine;
	affine.setIdentity();
	affine.translate(Vector2d(1.2, 3));
	affine.rotate(Rotation2Dd(0.1));
	cout << "affine matrix:\n" << affine.matrix() << endl << endl;
	Matrix<double, 2, 3> vecs1,vecs2;
	vecs1 << 1, 2, 3, 4, 5, 6;
	cout << "original vectors:\n" << vecs1 << endl << endl;
	vecs2 = affine*vecs1;
	cout << "affined vectors:\n" << vecs2 << endl;
	system("pause");
	return 0;
}

利用2D仿射变换对多个向量进行了批量变换。

射影变换

$${\mathbf{T}}_A=\left[\begin{array}{ll}\mathbf{A}& \mathbf{t}\\ {\mathbf{\alpha }}^{\mathrm{T}}& 1\end{array}\right]$$
它的左上角为可逆矩阵$\mathbf{A}$，右上角为平移$\mathbf{t}$，左下角为缩放${\mathbf{\alpha }}^{\mathrm{T}}$。由于采用了齐次坐标，当$v\ne 0$时，我们可以对整个矩阵除以$v$得到一个右下角为1的矩阵；否则得到右下角为0的矩阵。因此，2D的射影变换一共有8个自由度，3D则共有15个自由度。从真实世界到相机照片的变换可以看成一个射影变换。

Eigen关于射影变换的定义

射影变换(4 × 4) : Eigen::Projective3d

射影变换是最一般的变换，其保证的是接触平面的相交或相切。Eigen中预定好的一些类型如下。

/** \ingroup Geometry_Module */
typedef Transform<float,2,Projective> Projective2f;
/** \ingroup Geometry_Module */
typedef Transform<float,3,Projective> Projective3f;
/** \ingroup Geometry_Module */
typedef Transform<double,2,Projective> Projective2d;
/** \ingroup Geometry_Module */
typedef Transform<double,3,Projective> Projective3d;

它的成员函数和Isometry也是一模一样的，这里也不再介绍了。下面是一个简单的使用示例。

# include<iostream>
# include<Eigen\Dense>
# include<Eigen\Geometry>

using namespace std;
using namespace Eigen;

int main(){
	Projective3d prj;
	prj.setIdentity();
	cout << prj.matrix() << endl;
	system("pause");
	return 0;
}

下面示例图片来自：相似、仿射、射影变换区别

总结：
（1）刚性变换：只有物体的$\mathbf{R}$，$\mathbf{t}$发生改变，形状不变。
（2）等距变换：$\mathbf{R}$，$\mathbf{t}$的复合，等距变换前后长度，面积，线线之间的角度不变。自由度6。
（3）相似变换：等距变换和均匀缩放（$\mathbf{s}$）的一个复合，类似相似三角形，体积比不变。自由度7。
（4）仿射变换（正交投影）：平移变换和非均匀变换（$\mathbf{A}$）的复合，不变量是平行线。自由度12。
（5）射影变换（透视变换）：图像中的点的齐次坐标的一般非奇异线性变换。 不变量是：重合关系，长度的交比。自由度15。

实践：Eigen几何模块

#include <iostream>
#include <cmath>

using namespace std;

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

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