视觉SLAM ch3—三维空间的刚体运动

如果对于某些线性代数的知识不太牢固，可以看一下我的另一篇博客，写了一些基础知识并推荐了一些视频。

旋转矩阵单元所需的线代基础知识https://blog.csdn.net/Johaden/article/details/141023668

一、旋转矩阵

1.点、向量、坐标系

在数学中，特别是在线性代数领域，一个三维空间中的坐标可以被视为一个向量与线性空间基的乘积，这是因为线性空间的基提供了一种方式来表示该空间中的所有向量。为了理解这一点，首先需要明确什么是线性空间的基。

线性空间的基是一组向量，这组向量满足两个条件：第一，它们线性无关，即没有向量可以通过其他向量的线性组合来表示；第二，它们能够生成整个线性空间(向量的数量不能少于空间的维数、秩与维数相等)，即线性空间中的任何向量都可以通过这组基向量的线性组合来唯一表示。在三维空间中，基通常由三个线性无关的向量组成，这三个向量分别对应于空间中的三个坐标轴。若三个向量两两垂直就是一个标准正交基（下面的就是），组成如下的矩阵为正交矩阵（A’A=E）（两个基矩阵各自是正交矩阵，相乘后仍是正交矩阵）。

已知正交矩阵后，可以推出转置=逆，即 $A'=A^{-1}$ 。

坐标的取值与向量本身有关也与坐标系（线性空间的基）选取有关。

机器人中有很多种坐标系：①世界系/惯性系（World） ②机体系（body） ③传感器参考系（Sensor）。不同坐标系存在变换关系。传感器捕获的数据都在Sensor系里，但是分析位姿时不关注传感器的移动，更多关注机体的移动，所以需要进行Sensor=>body=>world的变换。第一个变换我们已知（机器人在制作过程中就已知了，人为给定。以及制作时传感器相对于机体的位置也已知，可以进行转换）。第二个translate需要估计，需要在运动过程中估计机器人在世界的位置和姿态（SLAM本质）。

运算：

内积定义：

外积定义(在几何代数中，“∧”符号表示把向量变为矩阵，形式为反对称矩阵)：

计算用到了行列式的拆分知识（注意，i，j，k都为单位向量，模为1）。写成相乘的格式利于后面的编程和推导。

几何意义：方向满足右手定则，大小为 $|a||b|\sin \theta$ ，为平行四边形的有向面积。

坐标：

当我们说一个三维空间中的坐标是一个向量乘以线性空间的基时，实际上是在说任何一个点（或向量）在这个三维空间中的位置都可以通过其在三个基向量方向上的分量（即坐标）来唯一确定。换句话说，如果我们用v表示一个向量，用 $e_{1}$ , $e_{2}$ , $e_{3}$ 表示三维空间的一组基，那么该向量可以表示为：

其中，x,y,z是该向量相对于基 $e_{1}$ , $e_{2}$ , $e_{3}$ 的坐标。这种表示方法说明了向量与基之间的关系，即向量可以看作是其坐标与基向量的线性组合。

2.位姿变换和旋转矩阵

（1）位姿变换：

旋转变换

坐标系（e1，e2，e3）发生一次旋转变为（e1'，e2'，e3'）

对于一个固定向量a，它的坐标如何变化？

坐标关系：

$[e_{1},e_{2},e_{3}]\begin{bmatrix} a_{1}\\ a_{2} \\ a_{3} \end{bmatrix}=[e_{1}',e_{2}',e_{3}']\begin{bmatrix} a_{1}'\\ a_{2}' \\ a_{3}' \end{bmatrix}$

左乘 $\begin{bmatrix} e_{1}^{T}\\ e_{2}^{T} \\ e_{3}^{T} \end{bmatrix}$ 得：

我们把这样一个3×3的矩阵R称为旋转矩阵。

旋转矩阵有以下两个特性：①正交矩阵 ②行列式为+1（充要）

。 n为3时为三维空间的旋转，特殊正交群可以看作一个集合。

同理， $a'=R^{-1}a=R^{T}a$ （R为正交矩阵，转置就是逆，出于复杂性考虑，常用转置）

示例，从1位置以 $R_{12}$ 为旋转矩阵旋转到2位置（R12表示把姿态由2系对齐到1系的旋转矩阵 ）：

$a_{1}=R_{12}a_{2}$

$a_{2}=R_{21}a_{1}$

$R_{21}=R_{12}^{-1}=R_{12}^{T}$

进一步，可以有三个坐标系的递推：

欧式变换

$a_{1}=R_{12}a_{2}+t_{12}$ （ $t_{12}$ 表示把2系的坐标原点对齐到1系的平移量）

在多层坐标系的变换时，不得不写成

$c=R_{2}(R_{1}a+t_{1})+t_{2}$

直观来看就是：①旋转 ②平移

$a'=Ra+t$

3.变换矩阵与齐次坐标

上面的形式在变换多次之后会过于复杂。因此，我们要引入齐次坐标和变换矩阵重写

式。

中间红框的矩阵称为变换矩阵T。记 $\widetilde{a}=\begin{bmatrix} a\\1 \end{bmatrix}$ 为齐次坐标（四维）。

这样变换利于连续坐标系转换，如：

特殊欧式群(旋转+平移)：

此处不是简单的取反，需要排除旋转的影响，因为是先旋转后平移，平移量受旋转量影响。类比y=kx+b，左右移动时不是直接调整b的值，而是调整y=k（x+b/k）中b/k的值。

若世界坐标系为 $T_{w}$ ，机器人坐标系为 $T_{R}$ ，一个点的世界坐标为 $P_{w}$ ，机器人坐标系下为 $P_{R}$ ，那么满足： $P_{R}=T_{Rw}P_{w}$ （注意T的下标与R的下标一致，“就近原则”）

其中的 $T_{Rw}$ 意思就是从w到R进行平移，如果把 $T_{Rw}$ 后面乘一个0向量（与T一样，三个0一个1），结果就是矩阵的平移t（很容易，直接稍微算一下，把R全消掉了，只剩t了）， $T_{Rw}$ 是世界坐标系原点在机器人坐标系下的表示，反之，可以自己思考一下。

4.旋转向量(角轴)

旋转向量由一个方向向量和一个标量组成，其中方向向量定义了旋转的轴，而标量则定义了绕该轴旋转的角度。旋转向量的长度等于旋转角度的弧度值。

旋转向量可以用一个三维向量 $r=[\Theta _{x},\Theta _{y},\Theta _{z}]^{T}$ 来表示，其中 θx, θy, 和 θz 分别是绕 x, y, z 轴旋转的弧度数。但是，实际上旋转向量的表示是规范化的，即它的方向表示旋转轴的方向，而它的模长表示旋转角度。所以一个旋转向量 r 可以表示为：

其中，θ 是旋转角度（以弧度为单位）， $\widehat{u}$ 是单位向量，指向旋转轴的方向。

转化:

旋转向量到旋转矩阵的过程由罗德里格斯公式（Rodrigues’s Formula ）表明

符号 ∧ 是向量到反对称的转换符

旋转矩阵到旋转向量的转换:

在三维空间中，任何非平凡的旋转矩阵 R（即不是恒等矩阵的旋转）都有一个特殊的属性：存在至少一个方向（向量），当这个向量被 R 作用时，它仅仅可能改变长度，但不会改变方向。这个方向就是旋转轴，而旋转轴上的向量是旋转矩阵的一个特征向量，对应于特征值 1。

二、实践部分-EIGEN

1.安装eigen（通过eigen在C++中进行矩阵运算）

eigen安装在如下图路径中（eigen的头文件库）

2. Eigen 不是一个预先编译好的库，而是许多头文件，因此通常不需要在 CMake 或其他构建系统中显式地链接到任何库。只需要通过include_directories（“目录”）添加进来即可。

补充拓展：`CMAKE_BUILD_TYPE与CMAKE_CXX_FLAGS`

CMAKE_BUILD_TYPE

Release：如果你将 CMAKE_BUILD_TYPE 设置为 "Release"，编译器会在编译过程中应用各种优化技术，这可能会显著提高程序的运行速度。同时，它会移除一些不必要的调试信息，使得可执行文件更小，加载更快。但是，这也意味着在程序崩溃或出现错误时，调试起来会比较困难，因为你没有那么多的上下文信息。
Debug：与此相反，设置为 "Debug" 会禁用大部分优化，并包含大量的调试信息。这意味着程序运行时可能不会像 Release 版本那样快，但是它会提供更多的信息来帮助开发者在出现问题时进行调试。对于开发阶段，这是非常有用的。

CMAKE_CXX_FLAGS

这些标志可以用来指定编译器在编译时应该遵循的特定规则或应用的优化。

-O0：这是默认的优化等级，实际上不开启任何优化，仅做最基本的处理，保留所有的调试信息，适合于调试。
-O1：开启基本的优化，比如常量传播、死代码消除等，这些优化可以提高代码的执行效率，但不会进行大规模的代码重组。
-O2：在 -O1 的基础上增加了更多的优化，包括函数内联、循环优化等，它试图在编译时间和执行效率之间找到一个平衡。
-O3 会启用所有 -O2 优化，加上其他更激进的优化，比如循环展开、内联函数、寄存器重命名等，这些都可以减少指令的数量和提高代码的执行速度。
-Os：类似于 -O2，但更注重于生成较小的二进制文件，这在嵌入式系统或资源有限的环境下很有用。
-Ofast：类似于 -O3，但可能还会启用一些可能不遵守标准或可能产生错误结果的优化，比如假设浮点运算满足结合律等，因此在科学计算等领域使用时要谨慎。

选择哪个优化等级取决于你的具体需求。在开发阶段，你可能更倾向于使用 -O0 或 -O1 来便于调试；而在发布或性能关键的应用中，-O2 或 -O3 会是更好的选择，尽管它们可能会延长编译时间。-Os 适用于需要节省存储空间的情况，而 -Ofast 则是在追求极致性能且可以接受潜在不精确计算时的选择。

——————————————————————————————

3.编程，头文件如下：

要生成一个矩阵，如下图，第二行的提示Matrix3f意思是生成一个3*3的float精度的矩阵：

而定义向量如下，同理使用Vector：

若不知道矩阵大小，需要定义动态大小矩阵：

其中的Dynamic的值为-1，写-1也可以，但是最好写Dynamic。MatrixXd也代表未知大小矩阵，也就是动态矩阵。

Ⅰ.定义 Eigen 矩阵的常见方法：

1. 使用 Eigen::Matrix 模板类

Eigen 的 Matrix 类是一个通用模板类，允许你指定元素类型、行数、列数和可选的存储选项。这是定义矩阵的最通用方式。

#include <Eigen/Dense>using namespace Eigen;// 定义一个动态大小的双精度矩阵
Matrix<double, Dynamic, Dynamic> mat1;
mat1.resize(3, 4); // 设置矩阵大小为 3x4// 定义一个固定大小的单精度矩阵
Matrix<float, 2, 3> mat2;

2. 使用类型别名

Eigen 提供了一些类型别名，简化了固定大小矩阵的定义。

Matrix2f mat2x2; // 2x2 浮点矩阵
Matrix3d mat3x3; // 3x3 双精度矩阵
VectorXd vec;    // 动态大小的列向量，元素类型为双精度
MatrixXd mat;    // 动态大小的双精度矩阵
MatrixXf matXf;  // 动态大小的单精度矩阵

3. 使用初始化列表

Eigen 支持使用初始化列表来定义矩阵，这在初始化小型矩阵时特别方便。

Matrix2f mat2x2 = { {1, 2}, {3, 4} };

4. 使用 setZero, setIdentity, setRandom 等方法初始化

Eigen 提供了一系列方法来快速初始化矩阵。

MatrixXd mat = MatrixXd::Zero(3, 3);       // 创建一个 3x3 的零矩阵
MatrixXd mat2 = MatrixXd::Identity(3, 3);  // 创建一个 3x3 的单位矩阵
MatrixXd mat3 = MatrixXd::Random(3, 3);    // 创建一个 3x3 的随机矩阵

Ⅱ.eigen阵的操作（10个）

1. 基本矩阵创建和初始化

#include <Eigen/Dense>
using namespace Eigen;MatrixXd mat(2,3); // 动态大小矩阵 2x3  OR  Matrix <float,2,3> mat; 非动态确定矩阵
mat << 1, 2, 3, 4, 5, 6;Matrix3f mat3f; // 固定大小 3x3 浮点矩阵
mat3f.setRandom(); // 随机初始化Vector3d vec; // 3维向量
vec << 1, 2, 3;

PS：使用（）取出第i行第j列，如mat(i,j)以及v_3d[0],v_3d[1]；

2. 矩阵和向量的数学运算

不能混合两种不同的矩阵，包括维度、数字类型等，比如float和double就不能往一起乘。

MatrixXd A = MatrixXd::Random(3,3);
MatrixXd B = MatrixXd::Random(3,3);MatrixXd C = A + B; // 加法
MatrixXd D = A - B; // 减法
MatrixXd E = A * B; // 矩阵乘法VectorXd v1 = VectorXd::Random(3);
VectorXd v2 = VectorXd::Random(3);double dot_product = v1.dot(v2); // 向量点积
VectorXd v_sum = v1 + v2; // 向量加法//Matrix::Identity()‌：创建一个单位矩阵，其中主对角线上的元素为1，其余元素为0。
//Matrix::Zero()‌：创建一个零矩阵，所有元素都初始化为0。
//Matrix::Ones()‌：创建一个所有元素都设置为1的矩阵。

::运算符在这里表示作用域解析运算符，它用于访问类或命名空间的成员。在这个上下文中，MatrixXd::Random意味着Random是MatrixXd类的一个静态成员函数。

Eigen 的矩阵和向量运算要求参与运算的对象具有相同的数据类型。例如，你不能直接将一个 float 类型的矩阵与一个 double 类型的矩阵相乘，因为它们的数据类型不匹配（不遵循C++的类型提升规则，遵循Eigen的类型安全的原则）。如果你需要将不同数据类型的矩阵进行运算，通常需要先进行类型转换。例如：

MatrixXf matF(2,2); // 浮点型矩阵
MatrixXd matD(2,2); // 双精度矩阵// 错误：类型不匹配
// MatrixXd result = matF * matD;// 正确：将 float 矩阵转换为 double 矩阵
MatrixXd converted = matF.cast<double>();
MatrixXd result = converted * matD;

PS：上图里有几个关键点：

[1,2,3;4,5,6] 表示的是一个2x3的矩阵，其中第一行为1,2,3，第二行为4,5,6。
[3,2,1] 如果被视为一个列向量，则它是一个3x1的矩阵。

假设result是计算上述矩阵和向量相乘得到的结果，那么result.transpose()就是将这个结果矩阵的行和列进行互换，即转置。

几种运算

3. 访问和修改矩阵元素

MatrixXd mat = MatrixXd::Random(2,2);
double element = mat(0,1); // 访问第一行第二列元素
mat(0,1) = 5.0; // 修改第一行第二列元素

4. 矩阵的转置和逆

MatrixXd A = MatrixXd::Random(3,3);
MatrixXd At = A.transpose(); // 转置
MatrixXd A_inv = A.inverse(); // 逆矩阵

5. 行列和子矩阵提取

MatrixXd A = MatrixXd::Random(3,3);
RowVectorXd row = A.row(0); // 提取第一行
VectorXd col = A.col(0); // 提取第一列
MatrixXd sub = A.block(0,0,2,2); // 提取左上角的 2x2 子矩阵

6. 矩阵的拼接

MatrixXd A = MatrixXd::Random(2,2);
MatrixXd B = MatrixXd::Random(2,2);
MatrixXd C = MatrixXd::Random(2,2);
MatrixXd D = MatrixXd::Random(2,2);MatrixXd AB = A.colwise() + B; // 按列拼接
MatrixXd CD = C.rowwise() + D; // 按行拼接
MatrixXd full = AB.colwise().append(CD); // 上下拼接

7. 矩阵的行列式和秩

MatrixXd A = MatrixXd::Random(3,3);
double det = A.determinant(); // 行列式
int rank = A.fullPivLu().rank(); // 矩阵秩

LU分解是一种将矩阵分解为一个下三角矩阵（Lower Triangular Matrix）和一个上三角矩阵（Upper Triangular Matrix）的乘积的技术。对于一个n×n的矩阵A，如果它可以被分解为A=LU

A.fullPivLu().rank()方法的原理是检查分解后的上三角矩阵U的对角线，统计非零元素的数量，从而计算出矩阵的秩。

8. 矩阵的特征值和特征向量

MatrixXd A = MatrixXd::Random(3,3);
SelfAdjointEigenSolver<MatrixXd> es(A*A.transpose());
VectorXd eigenvalues = es.eigenvalues(); // 特征值
MatrixXd eigenvectors = es.eigenvectors(); // 特征向量

A*A.transpose()计算的是矩阵A与其转置矩阵 $A^{T}$ 的乘积。对于一般的矩阵 A， $A*A^{T}$ 的结果将是一个对称矩阵，所以A*A.transpose()的结果是一个对称矩阵，可以作为SelfAdjointEigenSolver的输入。SelfAdjointEigenSolver是Eigen库中的一个类，用于求解自伴矩阵（self-adjoint matrix）的特征值问题（类中包含特征值、特征向量、其他内部计算数据等）。对于实数矩阵，自伴矩阵就是对称矩阵，即满足 $A=A^{T}$ 。

9. 矩阵的比较和条件判断

MatrixXd A = MatrixXd::Random(3,3);
MatrixXd B = MatrixXd::Random(3,3);
bool isEqual = (A == B).all(); // 判断 A 和 B 是否完全相等

10. 矩阵的求解和分解

MatrixXd A = MatrixXd::Random(3,3);
VectorXd b = VectorXd::Random(3);
VectorXd x = A.colPivHouseholderQr().solve(b); // 解线性方程 Ax = bMatrixXd lu = A.lu(); // LU 分解
MatrixXd qr = A.householderQr(); // QR 分解
MatrixXd svd = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); // SVD 分解

这里首先创建了一个3x3的随机矩阵A和一个3维随机向量b。接下来，使用colPivHouseholderQr()方法对矩阵A进行QR分解，其中包含了列主元的置换，以增加分解的稳定性。solve(b)方法随后被用来解线性方程组Ax = b，这里的x就是所求的解向量。

QR分解：将矩阵A分解为A = QR，其中Q是一个正交矩阵（意味着Q^T Q = I。其中Q^T 表示矩阵Q的转置。），R是一个上三角矩阵。QR分解在求解最小二乘问题和矩阵特征值问题时非常有用，因为它保留了矩阵的秩信息，同时保证了数值稳定性。
SVD分解：将矩阵A分解为A = UΣV^T，其中U和V都是正交矩阵，Σ是对角矩阵，对角线上的元素是A的奇异值。SVD分解提供了矩阵的完整结构信息，包括矩阵的秩、特征值、奇异向量等，是处理线性代数问题的强有力工具，尤其在数据分析和降维等领域。

Ⅲ.解方程

正半定矩阵是一个对称矩阵，其所有特征值都是非负的。生成正半定矩阵的一个常见方法是将一个矩阵与其转置矩阵相乘。这是因为对于任意矩阵A，A * A^T的结果总是一个正半定矩阵。在上面的代码中，matrix_NN首先被赋值为其自身与自身转置的乘积。最后成了一个MATRIX_SIZE x 1的随机列向量v_Nd。MatrixXd::Random(MATRIX_SIZE, 1)生成的向量同样具有在[-1, 1]区间均匀分布的随机元素。

解方程方法：

1.直接求逆

Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;

这里使用了直接求逆的方法来解线性方程组。inverse() 函数计算矩阵 matrix_NN 的逆矩阵，然后将逆矩阵与向量 v_Nd 相乘，得到向量 x。这种方法简单直接，但是当矩阵较大或接近奇异时，求逆可能会非常慢，且数值稳定性较差。

2.QR分解

X = matrix_NN.colPivHouseholderQr().solve(v_Nd);

这里使用了QR分解来解方程。colPivHouseholderQr() 函数执行列主元Householder QR分解，这是一种更稳定和更快的方法来解线性方程组，尤其是对于非正交或接近奇异的矩阵。solve() 函数接收右侧向量 v_Nd 并返回解向量 X。QR分解将矩阵分解为一个正交矩阵Q和一个上三角矩阵R的乘积，使得求解线性系统变得更为简单和快速。

原理：一旦得到了矩阵 A 的QR分解 A=QR，解线性方程组 Ax=b 变得相当直接。由于 Q 是正交矩阵，我们可以两边同时左乘 $Q^{T}$ ，得到：

$Q^{T}Ax=Q^{T}b$

$Rx=Q^{T}b$

由于 R 是上三角矩阵，我们可以通过回代（back substitution）的方法很容易地解出 x。这就是 solve() 函数所做的工作，它接收向量 v_Nd，然后返回解向量 X。

PS：回代

上三角矩阵的回代

假设我们有一个上三角矩阵 U 和相应的线性方程组 Ux=b，其中 U 是一个上三角矩阵，x 是未知向量，b 是常数向量。上三角矩阵 U 的形式如下：

回代的过程从最后一行（即 nn 行）开始，逐步向上进行：

最后一行只有一个未知数 xn，所以可以直接求解：
然后移动到倒数第二行，将已知的 xn 值代入，解出 xn−1：
重复这个过程直到第一行，每次都将已解出的未知数代入更高行的方程中，最终解出所有的未知数。

————————————————————————————————————

3.Cholesky分解

X = matrix_NN.ldlt().solve(v_Nd);

对于正定矩阵，使用Cholesky分解可以提供一种快速且稳定的方法来解线性方程组。ldlt() 函数执行LDL^T分解，这是Cholesky分解的一种广义形式，适用于对称矩阵。即使矩阵不是严格正定的，LDL^T分解仍然可以执行，通过添加一个小的正数到对角线来确保正定性。solve() 函数同样接收右侧向量 v_Nd 并返回解向量 X。

如何判断正定矩阵？（尤其是下面推文中的第二条判定法则）

矩阵正定性判定——来源博主：爱学习的贝塔https://blog.csdn.net/qq_38048756/article/details/115108733

LDLT分解原理

LDLT分解是一种矩阵分解方法，主要用于对称矩阵，尤其是对称正定矩阵。对称正定矩阵是所有特征值都是正数的对称矩阵。这种分解将矩阵 A 分解为一个单位下三角矩阵 L，一个对角矩阵 D 和 L 的转置 $L^{T}$ 的乘积，即：

$A=LDL^{T}$

这里的 L 是一个单位下三角矩阵（对角线元素为1的下三角矩阵），D 是一个对角矩阵，而 $L^{T}$ 是 L 的转置。

LDLT解线性方程组原理

一旦有了 $A=LDL^{T}$ 的分解，求解线性方程组 Ax=b 变得相对简单，可以通过以下两步完成：

解 Ly=b：首先，我们需要解出 y，满足 Ly=b。这是一个下三角系统的解，可以通过前代（forward substitution）来解决，因为L 是下三角矩阵。
解 Dz=y：接着，我们解出 z，满足 Dz=y。由于 D 是对角矩阵，这个步骤非常简单，每个未知数 zi 可以直接通过 yi/dii 计算出来，其中 dii 是 D 的对角线元素。
解 $L^{T}x=z$ ：最后，我们解出 x，满足 $L^{T}x=z$ 。这是上三角系统的解，可以通过回代（back substitution）来解决，因为 $L^{T}$ 是上三角矩阵。

超级强烈推荐的视频：

如果缺少基础或不理解正交矩阵以及各种分解可以看下面的视频（很清楚！专栏里面的四个视频都很好）：

矩阵分解动画讲解，其实也很简单！https://www.bilibili.com/video/BV1mm4y1i7ex/?spm_id_from=333.337.search-card.all.click&vd_source=033a75d9cfb0b0df5347cafb8b033109

最后运行一下：

可以看看上面三种方法哪个方法用时最少？

三、欧拉角

欧拉角是由莱昂哈德·欧拉（Leonhard Euler）在18世纪提出的，用于描述任意刚体相对于固定参考系的旋转。欧拉角通常涉及三个独立的角，它们按顺序绕着三个轴旋转，以达到所需的位置。

欧拉角通常定义为三个连续的旋转，每个旋转都绕着一个坐标轴进行。最常见的欧拉角序列有三种：

Z-X-Z（或称 intrinsic Z-X-Z）: 第一次绕 Z 轴旋转，第二次绕新的 X 轴旋转，第三次再绕新的 Z 轴旋转。
X-Y-X（或称 intrinsic X-Y-X）: 类似于 Z-X-Z，只是轴的顺序不同。
Z-Y-X（或称 Tait-Bryan angles）: 第一次绕 Z 轴旋转，第二次绕新的 Y 轴旋转，第三次绕新的 X 轴旋转。这种序列在航空和航天中特别常见，其中 Z 轴通常与地球的重力方向对齐。

而X（roll），Y（pitch），Z（yaw）的方向可以类比为飞机飞行的方向，如下：

注意旋转的次序，比如，先绕x转30°，再绕y转20°，和先绕y转20°，再绕x转30°，不同的顺序会导致不同的最终结果。这是因为每次旋转都会创建一个新的局部坐标系，后续的旋转将基于这个新的坐标系进行（轴也会跟着转，所以轴变了）。定义欧拉角时要先明确旋转轴顺序。

每个旋转都可以用一个角度来描述，通常标记为 ϕ（phi）、θ（theta）和 ψ（psi）。例如，在 Z-Y-X 序列中：

ψ（航向角或Yaw角）：绕 Z 轴的旋转，决定了刚体在水平面上的方向。
θ（俯仰角或Pitch角）：绕 Y 轴的旋转，决定了刚体向上或向下倾斜的程度。
ϕ（滚转角或Roll角）：绕 X 轴的旋转，决定了刚体侧向翻滚的程度。

动轴定轴

动轴旋转（Intrinsic Rotation）（便于交流）

在动轴旋转中，每次旋转都是相对于物体当前的坐标系进行的。这意味着，一旦物体绕一个轴旋转，下一个旋转将绕着物体新的坐标轴进行。动轴旋转通常用于描述物体在其自身坐标系内的旋转，例如飞机或航天器的姿态变化。

有万向锁现象。

万向锁现象

万向锁（Gimbal Lock）现象是指在使用欧拉角描述物体旋转时，由于旋转轴的对齐，导致物体的旋转自由度减少，失去一个维度的控制能力的现象。这种情况在动轴（intrinsic）旋转中尤其常见，尤其是在航空、航天及机械工程领域中使用欧拉角描述姿态时，万向锁是一个需要特别注意的问题。

假设你有一架飞机，使用 Z-Y-X 的欧拉角（航向角、俯仰角、滚转角）来描述其姿态。当飞机的俯仰角达到 ±90°（即飞机头朝上或头朝下），Y 轴（飞机的纵向轴）与地面平行时，会发生万向锁。此时，如果再尝试改变飞机的航向角或滚转角，实际上只能改变飞机的姿态在地平面上的投影，而无法改变飞机相对于地面的实际方向。也就是说，原本独立的两个旋转操作（绕 Z 轴的航向角和绕 X 轴的滚转角）现在只能产生一个维度的效果。

定轴旋转（Extrinsic Rotation）（便于编程）

在定轴旋转中，每次旋转都是相对于固定的外部坐标系进行的，即使物体已经发生了旋转。这意味着，不管物体如何旋转，旋转轴始终保持在空间中的固定位置。定轴旋转通常用于描述物体相对于固定参考系的变化，比如地球坐标系。

没有万向锁现象

奇异性：

奇异性（Singularity）在数学和物理学中通常指的是一个点或状态，在那里某些物理量或数学函数的行为变得不寻常，通常表现为无穷大、不连续或无法定义。

比如在旋转向量中，存在0到2π的跳变，可以给定义区间打补丁等。

欧拉角中也存在0到2π的跳变。另外有万向锁，理解万向锁具体可以看下面的视频：
无伤理解欧拉角中的“万向死锁”现象https://www.bilibili.com/video/av771397545/?vd_source=033a75d9cfb0b0df5347cafb8b033109 根据上面的视频我们可知，变换是由初始状态进行的，即使在调整y90°后继续调整x，当整体过程再次从初始状态开始时，会在第一步直接调整x的值（x：10，y：90，z：0，现在继续x旋转1度=>从初始开始x：11，y：90，z：0），由于y调整为90°，与初始的x“重合”，所以视觉上是调整了z轴，而实际调整的仅为初始的x轴。

整体的一个现象可以抽象为一个规律：前面的轴运动带动后面的轴运动，而后面的轴无法带动前面的轴运动。每次旋转必须在初始状态下旋转，而不是叠加旋转！！

例子：如图，我第一次以10，30，50变换飞机，第二次以11，30，50变换。问：能否在第一次后直接变换1，0，0？答：不行，因为轴会被带动，后面转的x并不是之前最初的x轴。

无论r是多少，只要p转了90度，就一定会使x轴和z轴重合，出现死锁现象。

定轴不存在死锁现象，但是定轴的欧拉角也不能增量式旋转！

虽然旋转向量和欧拉角这些量可以表示某个位姿，但是在全局范围或者特定情况下，它们的表示可能会遇到不连续性的问题，难以描述连续变化的运动（不能增量式调整，每次都需要从头开始）。

在死锁状态下，物体仍然可以旋转，但这并不意味着在数学模型中旋转的描述是连续的。在死锁状态下，如果绕Z轴和X轴的旋转角连续变化，物体的实际旋转状态可能会经历不连续的变化。这是因为，在死锁点附近，小的欧拉角变化可能会导致物体旋转状态的大变化，或者相反，大的欧拉角变化只引起微小的旋转状态变化。这种不连续性是由于欧拉角参数化与旋转状态空间之间映射的非线性造成的。导致无法进行插值或求导等操作。

四、四元数

四元数类似于复数，由爱尔兰数学家威廉·哈密顿于1843年提出，是复数的拓展，用于表示三维空间中的旋转。四元数由一个实部和三个虚部组成，通常表示为 q=a+bi+cj+dk，其中 a,b,c,d实数，而 i,j,k 是四元数的虚数单位，它们满足特定的乘法规则。

欧拉公式（二维）

二维空间中的旋转可以用一个单位复数表示：

$e^{i\theta }=\cos \theta +i\sin \theta$

欧拉公式证明：

四元数的基本形式

四元数可以被视为扩展复数的概念，但它们具有四个分量而不是两个。一个四元数 q 通常写作：

$q=a+bi+cj+dk$

其中 a,b,c,d 是实数，而 i,j,k 是四元数单位，它们满足以下性质：

$i^{2}=j^{2}=k^{2}=ijk=-1$

这意味着 ij=k,jk=i,ki=j，同时 ji=−k,kj=−i,ik=−j。这些规则定义了四元数乘法的非交换性和非分配性。

可视化四元数（务必看完！！视频超级好！！）

四元数的在三维中的可视化与三维在二维空间的可视化（纸片人）十分相近。圆->球，线->面。

四元数的可视化https://www.bilibili.com/video/BV1SW411y7W1/?spm_id_from=333.337.search-card.all.click&vd_source=033a75d9cfb0b0df5347cafb8b033109

四元数的表示

在实际应用中，四元数通常被写作一个实部和一个虚部的组合，其中虚部由三个独立的分量组成：

$q=w+xi+yj+zk$

这里 w 称为标量部分，而 x,y,z 组成了矢量部分。四元数也可以写作一个四维向量的形式(s是实部，v就是虚部)：

$q=[s,v],s=q_{0}\in R,v=[q_{1},q_{2},q_{3}]^{T}\in R^{3}$

$q=[w,x,y,z]$

单位四元数与旋转

四元数的一个重要应用是在三维空间中表示旋转。一个单位四元数（其长度等于1）可以用来表示一个旋转：

$q=[\cos \left ( \Theta /2 \right )]+\sin \left ( \Theta /2 \right )\overrightarrow{n}$

其中 θ 是旋转角度，而 n 是一个单位向量，表示旋转轴的方向。单位四元数的一个优点是它们避免了欧拉角在某些特定旋转角度下出现的“万向节锁”（gimbal lock）问题。

四元数的运算

加减法：

$q_{a}\pm q_{b}=[s_{a}\pm s_{b},v_{a}\pm v_{b}]^{T}$

乘法：

$q_{a}q_{b}=s_{a}s_{b}-x_{a}x_{b}-y_{a}y_{b}-z_{a}z_{b}+(s_{a}x_{b}+x_{a}s_{b}+y_{a}z_{b}-z_{a}y_{b})i+(s_{a}y_{b}-x_{a}z_{b}+y_{a}s_{b}+z_{a}x_{b})j+(s_{a}z_{b}+x_{a}y_{b}-y_{a}x_{b}+z_{a}s_{b})k$

上面这个图，符合”右手定则“，也就是类似于”叉积“，i，j，k分别为三个轴的基底。

四元数没有交换律！

数乘与点乘：

$kq=[ks,kv]^{T}$

取模：以 $q=a+bi+cj+dk$ 形式

我们用来描述姿态的四元数是单位四元数，如果我们在优化运算中（如求导等）改变了四元数的模长，记得将其变为单位长度。

共轭：

逆：

矩阵相乘：

四元数的使用：

1.三维扩展到四维（实部为零）

$p=[0,x,y,z]^{T}=[0,v]^{T}$

2. 如何用一个四元数旋转一个空间点？

设点p经过一次以q表示的旋转后，得到了p'，将后三维去除就是三维坐标

(1).p的坐标用四元数表示（虚四元数）：

$p=[0,x,y,z]=[0,v]$

(2).旋转之后的关系为：

$p'=qpq^{-1}$

为什么不直接像二维中一样，是qp呢？

通过视频（下面的三维转动的视频和交互网站）可以知道，当只进行一个q的时候，会导致点 p 不仅旋转，还会平移，这是因为四元数乘法本质上是非交换的，并且不保证旋转中心在原点。具体过程如下图所示：

这一步将点 p 从其原始位置旋转到一个新的位置。但这个新的位置并不一定是在我们想要的旋转轴周围。

这一步再将点从第一步旋转后的位置“反转”回旋转轴周围。这确保了点最终在正确的旋转轴周围完成旋转。

我们知道，当q1为45度时，整体不仅仅发生了旋转发生了平移。通过这样先旋转然后“反转”旋转的过程，实际上我们是在执行一个复合操作，它确保了点围绕原点进行旋转，而不会产生任何额外的平移。这是因为 $q^{-1}$ 是 q 的逆操作，它会抵消掉 q 引入的任何非旋转效果。平移的角度变化其实是2倍的关系，后面有讲。此处只需要理解旋转的公式即可。后面推荐的视频里面会将的很清楚。

3.虽然数学实质如上所述，但是Eigen已经将四元数的旋转运算重载，平时只需要这么算就行了，也就是三维直接乘四维也可以（仅在eigen中）。

$p'=qp$

注意：旋转时候左乘和右乘是有区别的，绝大多数人习惯左乘（绕世界系旋转用左乘，绕本体轴旋转用右乘）

四元数和角轴的关系：

角轴到四元数：

四元数到角轴：

首先必须了解四元数是如何使物体旋转的，即为什么会有半角的出现？

视频推荐：理解四元数如何使三维转动的，有助于理解

看下面的视频之前务必看完上面的四元数可视化的视频：

四元数和三维转动，可互动的探索式视频https://www.bilibili.com/video/BV1Lt411U7og/?spm_id_from=autoNext&vd_source=033a75d9cfb0b0df5347cafb8b033109 视频中的交互网站如下（英文无翻译，打开后不要翻译为中文）

https://eater.net/quaternionshttps://eater.net/quaternions 因为如下图，f(p)为左乘后右乘逆，两次都是绕同一方向旋转了。按下图所示，旋转的效果不是90°而是180°，可以在上面的网址的最后一个交互视频里查看（英文无汉译），这也解释了为什么是半角的原因。

截图例子如下，注意蓝色红框点随角度的变化：

通过上图，我们发现，当两个q都设定为90°时，球体的旋转为180°。视频中还操作了一些别的旋转轴和角度，可以自己去试着玩一下，有助于理解。

旋转矩阵R的推导过程：

为了更加清晰地展示推导过程，我们将使用以下符号：

q = w + xi + yj + zk (四元数)
q⁻¹ = w - xi - yj - zk (q 的共轭，假设 q 为单位四元数)
p = [0, x, y, z] (纯虚四元数，代表三维向量)
p' = [0, x', y', z'] (旋转后的向量，也用纯虚四元数表示)

目标： 计算 p' = qpq⁻¹，并将其结果表示成矩阵形式，从而得到旋转矩阵 R。

步骤：

1.计算 qp:

qp = (w + xi + yj + zk)(0 + xi + yj + zk)= -x² - y² - z² + wxi + wyj + wzk + xwi - yk + zj + ywk + xk - zi + zwi - xj - yk= (-x² - y² - z²) + (2wx)i + (2wy)j + (2wz)k + (yz - xy)i + (zx - yz)j + (xy - zx)k= [-x² - y² - z², 2wx + yz - xy, 2wy + zx - yz, 2wz + xy - zx]

2.计算 (qp)q⁻¹:

(qp)q⁻¹ = [-x² - y² - z², 2wx + yz - xy, 2wy + zx - yz, 2wz + xy - zx] * (w - xi - yj - zk)

为了简化计算，我们将 (qp)q⁻¹ 分成四部分进行计算：

标量部分:

(-x² - y² - z²)w - (2wx + yz - xy)(-x) - (2wy + zx - yz)(-y) - (2wz + xy - zx)(-z)
= -wx² - wy² - wz² + 2wx² + xyz - x²y + 2wy² + yzx - y²z + 2wz² + xyz - xz²
= 0  (因为 p' 是纯虚四元数)

i 部分:

(-x² - y² - z²)(-x) + (2wx + yz - xy)w + (2wy + zx - yz)(-z) + (2wz + xy - zx)(y)
= x³ + xy² + xz² + 2wx² + wyz - wxy - 2wyz - z²x + yz² + 2wyz + xy² - xyz
= x³ + 2xy² + xz² + 2wx² - wxy - z²x + yz² + xy² - xyz
= (1 - 2y² - 2z²)x + (2xy - 2zw)y + (2xz + 2yw)z  = x'

j 部分:

(-x² - y² - z²)(-y) + (2wx + yz - xy)(z) + (2wy + zx - yz)w + (2wz + xy - zx)(-x)
= y³ + yx² + yz² + 2wxz + yz² - xyz + 2wy² + wzx - wyz - 2wxz - x²y + xyz
= y³ + yx² + 2yz² + 2wy² + wzx - wyz - x²y
= (2xy + 2zw)x + (1 - 2x² - 2z²)y + (2yz - 2xw)z  = y'

k 部分:

(-x² - y² - z²)(-z) + (2wx + yz - xy)(-y) + (2wy + zx - yz)(x) + (2wz + xy - zx)w
= z³ + zx² + zy² - 2wxy - y²z + xyz + 2wxy + x²z - xyz + 2wz² + wxy - wzx
= z³ + zx² + zy² - y²z + x²z + 2wz² + wxy - wzx
= (2xz - 2yw)x + (2yz + 2xw)y + (1 - 2x² - 2y²)z  = z'

3.构建旋转矩阵:

将上面计算得到的 x', y', z' 写成矩阵形式：

[ x' ]   [ 1 - 2y² - 2z²   2xy - 2zw      2xz + 2yw ]   [ x ]
[ y' ] = [ 2xy + 2zw      1 - 2x² - 2z²   2yz - 2xw ] * [ y ]
[ z' ]   [ 2xz - 2yw      2yz + 2xw      1 - 2x² - 2y² ]   [ z ]

因此，旋转矩阵 R 为：

[ 1 - 2y² - 2z²   2xy - 2zw      2xz + 2yw ]
[ 2xy + 2zw      1 - 2x² - 2z²   2yz - 2xw ]
[ 2xz - 2yw      2yz + 2xw      1 - 2x² - 2y² ]

五、实践部分-Eigen的几何模块

注意：如何区分类？

若在代码里使用了 using namespace Eigen; 之后，Eigen 命名空间中的所有类和函数都被引入到当前的命名空间中，这意味着你可以直接使用这些类和函数，而不需要在它们前面加上 Eigen:: 前缀。

要判断一个单词是否代表一个类，可以根据以下几点来判断：

是否有变量的声明：查看该单词是如何被使用的。如果是作为一个类型来声明变量，那么它很可能是一个类。例如：Matrix3d A; 这里 Matrix3d 被用来声明一个变量 A，所以 Matrix3d 很可能是一个类。
构造函数调用：如果一个单词后面跟着括号，并且可能包含参数，那么它可能是一个类的构造函数。例如：AngleAxis aa(angle, axis); 这里 AngleAxis 后面跟着括号和参数，表明它可能是一个类。
成员函数调用：如果一个单词后面跟着 . 操作符，然后是函数调用，那么这个单词可能是一个类。例如：aa.matrix(); 这里 aa 后面的 . 表明 aa 可能是一个类的实例，matrix() 是它的成员函数。

——————————————————————————————

1.创建一个旋转矩阵：

Eigen Matrix3d ratation_matrix = Eigen::Matrix3d::Identity();
//旋转矩阵直接用一个3*3的单位矩阵表示

轴角表示法用一个单位向量 n和一个角度 θ 来表示旋转。单位向量 n^表示旋转轴，角度 θ 表示绕该轴旋转的角度。

2.使用`AngleAxis`类来表示这种旋转：

// 设置旋转为绕Z轴旋转45度
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));

3.将AngleAxis转换为矩阵

cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;
rotation_matrix = rotation_vector.toRotationMatrix();

matrix()成员函数是 Eigen::AngleAxis 类提供的一个方法，它的作用是计算并返回这个旋转向量对应的旋转矩阵。当你写 rotation_vector.matrix() 时，你就是在告诉程序去执行 rotation_vector 这个对象的 matrix() 方法，并获取它返回的结果。这个结果是一个 Eigen::Matrix3d 类型的对象，即一个3x3的矩阵，它具体描述了由 rotation_vector 所定义的三维空间旋转。

matrix()函数其实就是实现罗德里格斯公式，罗德里格斯公式是旋转向量和旋转矩阵之间的转换公式。它允许我们从一个旋转向量（角度和轴）计算出对应的旋转矩阵，从而可以在三维空间中应用这个旋转矩阵来旋转任意向量。

也可以直接赋值，利用toRotationMatrix.（）成员函数赋给一个旋转矩阵。

PS：调用 cout.precision(val) 时，你设置了 cout 在后续输出中使用的浮点数精度。这里的 val 是一个小数点后要保留的位数。

4.旋转某点时的乘法运算

// 用 AngleAxis 可以进行坐标变换Eigen::Vector3d v ( 1,0,0 );Eigen::Vector3d v_rotated = rotation_vector * v;cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

用 rotation_vector旋转v。输出的结果与下图中的旋转矩阵方法得到的结果一致。

    // 或者用旋转矩阵v_rotated = rotation_matrix * v;cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

编译后运行结果如图：

注意：rotation_vector 实际上并不是一个矩阵，至少不是直接以矩阵形式存储的。在Eigen库中，AngleAxis类封装了轴角表示法，它由一个旋转轴（一个单位向量）和一个旋转角度组成。尽管AngleAxis对象并不直接存储为矩阵，但它可以隐式地转换为一个旋转矩阵，这是因为AngleAxis类重载了必要的运算符，使其可以像矩阵那样与其他几何对象（如向量）进行操作。

5.从旋转矩阵获取欧拉角（eulerAngles）

// 欧拉角: 可以将旋转矩阵直接转换成欧拉角Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

输出结果（yaw-pitch-roll：z-y-x）：

也就是说，z对应的是0.785（ $\pi /4$ ），与上面我们的绕z轴旋转45度正好对应，可以作为一个验证。

6.使用Isometry表示欧氏变换

// 欧氏变换矩阵使用 Eigen::IsometryEigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
//T(0,3)=1;T(1,3)=3;T(2,3)=4;与上一行表达是一样的。cout << "Transform matrix = \n" << T.matrix() <<endl;

Isometry3d是一个4x4的矩阵，用于表示三维空间中的欧式变换。这里初始化了一个单位变换，然后设置了旋转和平移。一般形式如下：

如下图所示，还有很多种变形，比如2d代表的就是二维空间的，且精度为double。

其中，因为上文的rotation_vector 是一个绕Z轴旋转45度的 AngleAxis 对象。所以 T.rotate ( rotation_vector ); 就是告诉 T 要绕Z轴旋转45度，输出的也就是所谓的“旋转矩阵R” 。

Eigen::Vector3d ( 1,3,4 )，表示在变换中，所有的点都将在x轴方向移动1个单位，在y轴方向移动3个单位，在z轴方向移动4个单位。T(0,3)=1; T(1,3)=3; T(2,3)=4;，它们分别对应于矩阵的第1行第4列、第2行第4列、第3行第4列的值。也就是上面对应的t的部分。

T.matrix() 方法返回一个4x4的矩阵，它完全表示了 T 中的刚体变换。这个矩阵包含了旋转和平移的信息，可以用于将一个点或向量从一个坐标系转换到另一个坐标系。当你调用 T.matrix() 并打印输出时，你会看到一个4x4的矩阵，其中3x3的左上角部分是旋转矩阵，右边一列是平移向量，最后一行是齐次坐标系统中的标准化因子，就是上文的标准形式。

输出结果：

这时候这个T就是一个变换矩阵，这个变换矩阵也可以直接去乘一个向量（即使它是一个4x4的矩阵，但是它是有定义的），拿这个矩阵乘一个向量相当于用这个矩阵去变换这个向量。

7.使用变换矩阵进行坐标变换

// 用变换矩阵进行坐标变换Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+tcout<<"v tranformed = "<<v_transformed.transpose()<<endl;

通常，一个4x4矩阵和一个3维向量直接相乘在数学上是没有定义的，但是通过运算符重载，Eigen库允许这种操作，从而简化了几何变换的编程。T是一个Isometry3d对象，它实际上是一个4x4的矩阵，而v是一个3维向量。在数学上，为了能够将一个向量与一个4x4矩阵相乘，向量需要被扩展为一个齐次坐标，也就是说，向量后面需要附加一个1，变成一个4维向量。Eigen库内部自动处理了这一点，它将3维向量v扩展为齐次坐标形式，然后与T相乘，最后再丢弃结果向量的第四个元素（即齐次坐标中的1），只保留前三个元素，得到变换后的3维向量v_transformed。

结果溯源：（相当于Rv+t）

8.使用四元数表示旋转（`Quaterniond`）

 // 四元数// 可以直接把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;

Eigen::Quaterniond是一个四元数类，它使用双精度浮点数来存储四元数的四个系数。这里，rotation_vector是一个AngleAxis对象，表示一个特定的旋转。构造函数会将AngleAxis转换成等价的四元数表示。

coeffs()是Eigen::Quaterniond类的一个成员函数，它返回一个固定大小的向量Eigen::Vector4d，这个向量包含了四元数的四个系数。在Eigen中，四元数的系数顺序是(x,y,z,w)(x,y,z,w)，即虚部在前，实部在后 。

对比下面四元数旋转的形式：

注意第一张图的顺序为X,Y,Z,W。所以注意对应。

注意，旋转矩阵也可以有相同的结果是因为这是同一个旋转。因为旋转矩阵也是通过前面的轴角生成的，所以本质来讲这是一回事。

9.使用四元数旋转向量

// 使用四元数旋转一个向量，使用重载的乘法即可v_rotated = q*v; // 注意数学上是qvq^{-1}cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

这里使用四元数来旋转向量v。在数学上，正确的旋转应该是 $qpq^{-1}$ ，但由于Eigen内部的优化重载，直接使用乘法就可以得到正确的结果。其实是先把v向量的坐标成为四元数的虚部，而实部被设定为0后，按 $qpq^{-1}$ 公式进行计算。