代码为14讲中slambook2/ch3/useGeometry
#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();
Matrix3d rotation_matrix = Matrix3d::Identity();
类型定义与初始化部分：
Matrix3d 是 Eigen 库中用于表示 3×3 大小的双精度（double 类型）矩阵的类型。这里定义了一个名为 rotation_matrix 的变量，其类型为 Matrix3d。
通过 Matrix3d::Identity() 对 rotation_matrix 进行初始化。::Identity() 是 Matrix3d 类型的一个静态成员函数，它的作用是创建并返回一个 3×3 的单位矩阵。
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));
类型定义与参数初始化部分：
AngleAxisd 是 Eigen 库中用于表示旋转向量的类型，它通过一个旋转角度和一个旋转轴来描述三维空间中的旋转操作。这里声明了一个名为 rotation_vector 的变量，其类型为 AngleAxisd。在构造 rotation_vector 时，传递了两个参数：M_PI / 4 和 Vector3d(0, 0, 1)。其中 M_PI 是 C++ 中定义的数学常量（通常在 <cmath> 头文件中有定义，这里可能已经包含或者 Eigen 库内部有相关依赖引用），代表圆周率 π，M_PI / 4 表示旋转角度为 π/4 弧度，也就是 45 度；
Vector3d(0, 0, 1) 则构造了一个三维向量，其 x、y 分量都为 0，z 分量为 1，这个向量代表了旋转轴的方向，即沿着 Z 轴方向旋转。所以整体这行代码创建了一个沿 Z 轴旋转 45 度的旋转向量对象。
cout.precision(3);
这行代码用于设置输出流 cout 的浮点数精度。调用 cout 的 precision() 成员函数并传入参数 3，意味着后续使用 cout 输出浮点数时，将会显示小数点后 3 位数字。
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;
首先输出字符串 "rotation matrix =\n" 到标准输出（屏幕），其中 \n 表示换行符，起到先输出提示信息并换行的作用，方便后续矩阵内容能另起一行清晰显示。
然后调用 rotation_vector.matrix() 函数，由于 rotation_vector 本身是 AngleAxisd 类型的旋转向量对象，它通过 matrix() 函数可以将自身所表示的旋转信息转换为一个等价的 3×3 矩阵形式（也就是旋转矩阵）。
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; Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_vector * v;
创建了一个向量V，再对V向量进行绕Z轴旋转45°的操作，把值赋给v_rotated。再输出v_rotated的转置。
// 欧氏变换矩阵使用 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 即可，略
Isometry3d T = Isometry3d::Identity();
Isometry3d 是 Eigen 库中用于表示三维欧氏变换的类型。这里定义了一个名为 T 的变量，其类型为 Isometry3d。通过 Isometry3d::Identity() 对 T 进行初始化。
::Identity() 是 Isometry3d 类型的静态成员函数，它返回一个 4×4 的单位矩阵形式的欧氏变换矩阵。尽管类型名称里带有 “3d”，但在实际表示上，欧氏变换需要考虑齐次坐标，所以采用 4×4 的矩阵来同时包含旋转和平移信息，在这个单位矩阵初始化状态下，表示还未进行实际的旋转和平移变换，是一种初始的 “无变换” 状态。
T.rotate(rotation_vector);
这行代码调用了 T（Isometry3d 类型对象）的 rotate 成员函数，将之前定义的 rotation_vector（AngleAxisd 类型，表示绕 Z 轴旋转 45 度的旋转操作）作为参数传入。该函数的作用是让欧氏变换矩阵 T 按照 rotation_vector 所描述的旋转方式进行旋转操作，也就是在这个 4×4 的矩阵中更新相应元素，使其包含了指定的旋转信息，用于后续对坐标等进行相应的旋转变换。
T.pretranslate(Vector3d(1, 3, 4));
这里调用了 T 的 pretranslate 成员函数，传入一个 Vector3d(1, 3, 4) 作为参数。pretranslate 函数用于设置欧氏变换矩阵中的平移部分信息。
Vector3d(1, 3, 4) 构造了一个三维向量，表示在 X、Y、Z 方向上的平移量分别为 1、3、4。通过调用这个函数，会在欧氏变换矩阵 T 中更新相应元素，使得 T 除了包含之前设置的旋转信息外，还融入了这个指定的平移信息，这样 T 就完整地表示了一个既有旋转又有平移的欧氏变换了。
Vector3d v_transformed = T * v; // 相当于R*v+t
坐标变换操作部分：
这里进行了一个乘法运算 T * v，其中 T 是前面已经构建好的包含旋转和平移信息的欧氏变换矩阵（Isometry3d 类型，本质是 4×4 矩阵），v 是一个 Vector3d 类型的三维向量（在 Eigen 库中会隐式地将其转换为齐次坐标形式的 4×1 向量，即在其末尾添加一个元素 1，以适应 4×4 矩阵的乘法运算规则）。
从线性代数和几何意义上来说，这个乘法操作相当于先对向量 v 进行旋转（对应矩阵中的旋转部分，也就是 R*v，R 是旋转矩阵部分），然后再进行平移（加上平移向量 t，这里的平移信息包含在 T 矩阵中），最终得到经过欧氏变换后的新向量 v_transformed，并将其赋值给 v_transformed 变量，完成坐标变换操作。
// 四元数
// 可以直接把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;
}Quaterniond q = Quaterniond(rotation_vector);
q = Quaterniond(rotation_matrix);
分别把旋转向量和旋转矩阵赋值给四元数，最后输出结果，显然结果都是一样的，只是赋值的形式不同罢了。
cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;
这行代码按照四元数旋转向量的完整数学形式 qvq^{-1} 进行计算展示。
首先构造一个四元数 Quaterniond(0, 1, 0, 0)，它对应着向量 (1, 0, 0)（因为按照前面提到的将向量转换为四元数的方式，实部为 0，虚部为向量分量），然后依次进行乘法运算 q * Quaterniond(0, 1, 0, 0) * q.inverse()，即先将表示向量的四元数与旋转四元数 q 相乘，再乘上 q 的逆四元数，得到旋转后的四元数结果。
接着调用 .coeffs().transpose() 获取这个旋转后四元数的系数并转置为行向量，最后将其输出到屏幕上（前面还输出了提示字符串 "should be equal to "），并输出 endl 换行，理论上这个结果应该和前面通过简化的 q * v 方式得到的 v_rotated 的数值是一致的，这样的对比展示有助于理解四元数旋转向量的原理以及 Eigen 库中相关操作的实现方式。