目录
罗德里格斯公式
https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
推导过程在这里，确实很长，没耐心看
奇异性
“奇异性”（singularity）一词在不同领域有不同的含义。在数学、物理和工程学科中，通常用来描述某个函数或矩阵的特殊点或特殊情况。
在数学中，一个函数的奇异点指的是该函数在该点处不满足某种平滑性质的点。例如，一个函数在某一点的导数不存在，或者在该点的值无限大或无限小，这都可以称为该点是函数的奇异点。
在物理学中，奇异性通常用于描述某些物理现象中的特殊点或特殊情况。例如，在黑洞物理中，黑洞的事件视界就是一个奇异面，因为在这个面内部，物理定律会出现奇异性，使得我们无法用传统的物理定律描述其中发生的事情。
在工程学科中，奇异点通常指某种矩阵或系统中的某个特殊点或特殊情况。例如，在机器人学中，机械臂的运动学方程可能涉及某些矩阵，这些矩阵在某些情况下可能会变成奇异矩阵，导致机械臂的运动出现问题。
用四元数表示旋转证明
推导好长好痛苦，不想看
四元数到其他旋转表示的变换
书中推导的很细致，笔者看了半天，才从字里行间勉强看出“飞舞”二字，摆烂了，我是飞舞
读者如果对推导不懂的话，q是上图表示的，可以将这个带入前面的各种推导中，看起来会直观一些。
cmake_minimum_required( VERSION 2.8 )
project( geometry )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable(eigenGeometry eigenGeometry.cpp)这段代码使用CMake来构建一个名为“geometry”的项目，并生成一个可执行文件名为“eigenGeometry”。它依赖于Eigen库，因此需要包含Eigen头文件的目录。在这个例子中，Eigen头文件的目录是“/usr/include/eigen3”。然后使用这些信息创建一个可执行文件“eigenGeometry.cpp”并将其链接到Eigen库。
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;iostream和cmath头文件，用于标准输入输出和数学运算。
Eigen/Core提供了矩阵、向量和其他核心功能，而Eigen/Geometry提供了旋转、平移和其他几何操作的功能。通过使用using namespace Eigen;，可以简化使用Eigen库的代码。
// 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();这段代码展示了如何使用Eigen库的几何模块进行旋转操作。
Matrix3d rotation_matrix = Matrix3d::Identity();
这里创建了一个3x3的单位矩阵，表示一个没有旋转的初始状态。
Matrix3d::Identity()是Eigen库中的一个静态成员函数，用于创建一个3x3的单位矩阵。单位矩阵是一个对角线上元素为1，其它元素为0的矩阵。在这个特定的函数中，Matrix3d表示创建一个双精度的3x3矩阵类型，.Identity()表示返回一个单位矩阵。
使用Matrix3d::Identity()可以方便地创建一个没有进行任何旋转或变换的初始矩阵，它在几何计算中经常用于表示没有进行任何操作的初始状态。
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));
这里创建了一个旋转向量rotation_vector，表示绕Z轴旋转45度。M_PI / 4是旋转的角度，Vector3d(0, 0, 1)是旋转轴的方向，表示绕Z轴旋转。
AngleAxisd是Eigen库中的一个类，用于表示旋转向量。它是通过指定旋转角度和旋转轴来定义旋转的一种方式。
具体而言，AngleAxisd类使用双精度浮点数（double）表示旋转角度，并使用Vector3d类型表示旋转轴。构造函数AngleAxisd(angle, axis)用于创建一个旋转向量对象，其中angle是旋转角度，axis是旋转轴的坐标。
在此代码中，AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1))创建了一个旋转向量对象，表示沿Z轴旋转45度的旋转。M_PI是C++中定义的表示π（圆周率）的常量，Vector3d(0, 0, 1)表示Z轴的坐标为(0, 0, 1)。
Vector3d是Eigen库中的一个类，用于表示三维向量。它是由三个双精度浮点数（double）表示的向量。
在代码中，Vector3d(0, 0, 1)创建了一个三维向量对象，表示坐标为(0, 0, 1)的向量。这里的向量可以用于表示旋转轴，其中X轴的坐标为0，Y轴的坐标为0，Z轴的坐标为1。
通过使用Vector3d类，可以方便地表示和操作三维向量，例如进行向量的加法、减法、点积、叉积等操作。
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;
这里使用matrix()将旋转向量转换为旋转矩阵，并通过cout输出旋转矩阵。precision(3)用于控制输出的精度为3位小数。
注意，旋转向量在底层并不直接是矩阵类型，但是通过重载运算符，可以将其当作矩阵来进行运算。在输出时，可以通过matrix()方法将旋转向量转换为矩阵形式。
在Eigen库中，.matrix()是一种成员函数或操作符，用于将某些类型的对象转换为矩阵形式。
对于旋转向量类AngleAxisd，.matrix()函数用于将旋转向量转换为对应的旋转矩阵形式。这是因为旋转向量本身并不是一个矩阵，而是一种更紧凑的表示方式，可以通过调用.matrix()函数将其转换为对应的旋转矩阵。
在代码中，rotation_vector.matrix()将旋转向量rotation_vector转换为旋转矩阵形式，并通过cout语句打印出来。这样可以将旋转向量表示的旋转操作转换为矩阵形式，更方便进行计算和处理。
// 也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix();在这段代码中，通过toRotationMatrix()函数将旋转向量rotation_vector转换为旋转矩阵，并将结果赋值给rotation_matrix。
toRotationMatrix()是Eigen::AngleAxis类的一个成员函数，用于将旋转向量转换为旋转矩阵表示。
在这段代码中，通过rotation_vector.toRotationMatrix()将旋转向量rotation_vector转换为对应的旋转矩阵，并将结果赋值给rotation_matrix。这个旋转矩阵表示了沿着旋转向量定义的轴进行旋转的变换。
// 用 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;在这段代码中：
-
Vector3d v(1, 0, 0);创建了一个三维向量v，表示原始坐标(1, 0, 0)。 -
Vector3d v_rotated = rotation_vector * v;将旋转向量rotation_vector应用于向量v进行坐标变换，得到变换后的向量v_rotated。 -
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;输出变换后的向量v_rotated，使用transpose()函数将行向量转置为列向量，并打印输出结果。
这段代码演示了如何使用 Eigen::AngleAxis 进行坐标变换。通过将旋转向量应用于原始向量，可以得到旋转后的新向量。在这里，通过将旋转向量 rotation_vector 应用于向量 (1, 0, 0)，展示了沿 Z 轴旋转 45 度后的新坐标。
// 或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;在这段代码中：
-
v_rotated = rotation_matrix * v;使用旋转矩阵rotation_matrix对向量v进行坐标变换，得到变换后的向量v_rotated。 -
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;输出变换后的向量v_rotated，使用transpose()函数将行向量转置为列向量，并打印输出结果。
这段代码演示了使用旋转矩阵进行坐标变换的方法。通过将旋转矩阵应用于原始向量，可以得到旋转后的新向量。在这里，通过将旋转矩阵 rotation_matrix 应用于向量 (1, 0, 0)，展示了旋转后的新坐标。
// 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;在这段代码中：
-
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);将旋转矩阵rotation_matrix转换为欧拉角表示。eulerAngles()函数接受三个参数，表示旋转顺序，这里使用的是 ZYX 顺序，即 yaw-pitch-roll 顺序。 -
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;输出欧拉角表示的旋转结果，使用transpose()函数将行向量转置为列向量，并打印输出结果。
通过将旋转矩阵转换为欧拉角，可以获得旋转的绕三个轴的角度。在这里，使用 ZYX 顺序获取欧拉角，得到的结果表示旋转的 yaw、pitch 和 roll 角度。
在函数调用eulerAngles(2, 1, 0)中，参数(2, 1, 0)表示旋转矩阵转换为欧拉角时的旋转顺序。这里的参数值(2, 1, 0)表示按照 ZYX 的顺序进行旋转，即先绕 Z 轴进行旋转（yaw），然后绕 Y 轴进行旋转（pitch），最后绕 X 轴进行旋转（roll）。
旋转矩阵转换为欧拉角时，旋转顺序的选择会影响最终得到的欧拉角的顺序和值。在这里，使用了 ZYX 顺序，即先绕 Z 轴旋转，然后绕 Y 轴旋转，最后绕 X 轴旋转。这种顺序在某些应用中是常见的，例如飞行器的姿态表示中常使用的 yaw-pitch-roll 顺序。
// 欧氏变换矩阵使用 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;在上述代码中，我们使用了Eigen::Isometry3d来表示欧氏变换矩阵，它实际上是一个4x4的矩阵。通过Isometry3d::Identity()可以创建一个单位矩阵。
Isometry3d::Identity()是Eigen::Isometry3d类的一个静态成员函数，用于创建一个单位矩阵。单位矩阵表示没有任何旋转和平移的初始状态。
Isometry3d是Eigen库中的一个类，用于表示3D欧氏变换（Euclidean transformation）。它是Eigen::Transform类的一个特化版本，用于表示没有缩放的刚体变换，包括平移和旋转。
Isometry3d类提供了各种方法和运算符，使我们能够对欧氏变换进行操作，例如旋转、平移、合并、逆变换等。
虽然Isometry3d被称为"3d"，但它实际上是一个4x4的矩阵。这是因为它包含了3D空间中的平移和旋转，并使用齐次坐标表示。
齐次坐标是一种扩展了欧几里得坐标系统的方法，它通过添加一个额外的维度来表示平移。在3D空间中，我们通常使用三个坐标轴(x、y、z)表示位置，但当需要进行平移操作时，我们需要一个额外的维度来表示平移量。
因此，为了同时表示旋转和平移，Isometry3d使用了一个4x4的矩阵。其中，前三行和前三列表示旋转矩阵（3x3），用于描述物体的方向和姿态。第四列（0, 0, 0, 1）用于表示平移向量，最后一行（0, 0, 0, 1）则是齐次坐标系统中的单位元素。
通过使用4x4的矩阵表示，Isometry3d可以同时表示旋转和平移操作，使得在3D空间中的刚体变换更加方便和统一。
T.rotate(rotation_vector)将使用给定的旋转向量rotation_vector对Isometry3d对象T进行旋转操作。这将根据旋转向量指定的旋转轴和角度对T进行旋转变换。
具体来说，rotation_vector是一个AngleAxisd类型的对象，表示绕旋转轴旋转的角度。通过调用rotate()函数，将rotation_vector应用于T，即将T绕旋转向量指定的轴进行旋转。
该操作将修改T的旋转部分，即使它已经包含了之前的旋转或平移。这意味着对T进行多次旋转操作，每次都会基于当前的旋转结果进行变换。
T.pretranslate(Vector3d(1, 3, 4))将使用给定的平移向量(1, 3, 4)对Isometry3d对象T进行平移操作。这将在原有的旋转基础上对T进行平移变换。
具体来说，pretranslate()函数将平移向量应用于T的平移部分，即将给定的平移向量添加到T的当前平移部分上。
这个操作会修改T的平移部分，即使它已经包含了之前的旋转或平移。这意味着对T进行多次平移操作，每次都会基于当前的平移结果进行变换。
最后，通过T.matrix()将欧氏变换矩阵转换为普通的4x4矩阵，并打印出来。这个变换矩阵包含了旋转和平移的信息，可以用于对三维点或向量进行变换操作。
// 用变换矩阵进行坐标变换
Vector3d v_transformed = T * v; // 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;T * v将使用变换矩阵T对向量v进行坐标变换。这个操作相当于将向量v先应用旋转变换，然后再进行平移变换。
具体来说，T * v将向量v乘以变换矩阵T，这将对向量进行旋转和平移变换。旋转部分会将向量进行旋转，而平移部分会将向量进行平移。
通过这个操作，可以将原始的向量v变换为在给定变换矩阵T下的新坐标系中的向量v_transformed。
输出语句 cout << "v tranformed = " << v_transformed.transpose() << endl; 将打印变换后的向量v_transformed的值。.transpose()函数用于将向量转置，以便更好地打印出向量的数值。
// 对于仿射和射影变换，使用 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;
Eigen::Affine3d是Eigen库中用于表示仿射变换的类。仿射变换是一种保持直线和平行关系的几何变换，包括平移、旋转、缩放和错切等操作。
Eigen::Affine3d类是一个4x4的矩阵类型，它可以表示三维空间中的仿射变换。它继承自Eigen::Transform类，提供了一系列方法和运算符来进行仿射变换的操作。
使用Eigen::Affine3d，可以进行平移、旋转和缩放等变换。例如，可以使用translate()方法来进行平移操作，使用rotate()方法进行旋转操作，使用scale()方法进行缩放操作。还可以使用乘法运算符*来组合多个变换。
以下是一些示例代码：
Eigen::Affine3d transform = Eigen::Affine3d::Identity(); // 创建一个单位矩阵的仿射变换
// 进行平移操作
transform.translation() << 1.0, 2.0, 3.0; // 平移向量为(1, 2, 3)
// 进行旋转操作
Eigen::AngleAxisd rotation(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 沿Z轴旋转45度
transform.rotate(rotation);
// 进行缩放操作
transform.scale(2.0);
// 应用变换到向量
Eigen::Vector3d v(1.0, 1.0, 1.0);
Eigen::Vector3d v_transformed = transform * v; // 应用变换到向量v
// 输出变换矩阵
std::cout << "Transform matrix:\n" << transform.matrix() << std::endl;
以上代码展示了如何创建一个仿射变换矩阵，并进行平移、旋转和缩放操作。然后，将向量v应用到变换矩阵上，得到变换后的向量v_transformed。最后，输出变换矩阵的内容。
请注意，Eigen::Affine3d类中的变换是按照乘法顺序从右到左应用的，即先进行最后一次变换，然后是倒数第二次，以此类推。
Eigen::Projective3d是Eigen库中用于表示射影变换的类。射影变换是一种更一般的几何变换，包括平移、旋转、缩放、错切以及透视投影等操作。
Eigen::Projective3d类是一个4x4的矩阵类型，它可以表示三维空间中的射影变换。它继承自Eigen::Transform类，提供了一系列方法和运算符来进行射影变换的操作。
使用Eigen::Projective3d，可以进行平移、旋转、缩放、错切和透视投影等变换。同样，可以使用translate()、rotate()、scale()、shear()和其他方法来进行相应的操作。还可以使用乘法运算符*来组合多个变换。
以下是一个简单的示例代码，展示了如何使用Eigen::Projective3d进行射影变换：
Eigen::Projective3d transform = Eigen::Projective3d::Identity(); // 创建一个单位矩阵的射影变换
// 进行平移操作
transform.translation() << 1.0, 2.0, 3.0; // 平移向量为(1, 2, 3)
// 进行旋转操作
Eigen::AngleAxisd rotation(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 沿Z轴旋转45度
transform.rotate(rotation);
// 进行缩放操作
transform.scale(2.0);
// 进行透视投影操作
double focal_length = 100.0; // 焦距
transform.matrix().col(3) << 0.0, 0.0, -1.0 / focal_length, 0.0; // 设置投影矩阵的最后一列
// 应用变换到向量
Eigen::Vector4d v_homogeneous(1.0, 1.0, 1.0, 1.0);
Eigen::Vector4d v_transformed = transform * v_homogeneous; // 应用变换到齐次坐标向量v
// 输出变换矩阵
std::cout << "Transform matrix:\n" << transform.matrix() << std::endl;
以上代码展示了如何创建一个射影变换矩阵，并进行平移、旋转、缩放和透视投影操作。然后，将齐次坐标向量v_homogeneous应用到变换矩阵上，得到变换后的向量v_transformed。最后，输出变换矩阵的内容。
请注意，射影变换中的齐次坐标是通过添加额外的维度来表示的，例如四维向量。在应用变换时，需要将普通的三维向量转换为齐次坐标向量。
// 四元数
// 可以直接把AngleAxis赋值给四元数，反之亦然
Quaterniond q = Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose()
<< endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部在代码中，通过Quaterniond q = Quaterniond(rotation_vector)这行语句，将rotation_vector（旋转向量）赋值给了一个四元数q。
Quaterniond是Eigen库中用于表示四元数的类。通过将旋转向量赋值给四元数，可以方便地进行四元数的操作和旋转变换。
在输出四元数时，使用了coeffs()方法来获取四元数的系数。请注意，四元数的系数是按照(x, y, z, w)的顺序排列，其中(x, y, z)是虚部，w是实部。通过transpose()方法将系数转置为行向量形式进行输出。
因此，cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;这行代码会输出从旋转向量构造的四元数的系数。
// 也可以把旋转矩阵赋给它
q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;在这段代码中，将旋转矩阵rotation_matrix赋值给了四元数q。
通过q = Quaterniond(rotation_matrix)这行语句，将旋转矩阵转换为对应的四元数表示。这种转换是可能的，因为旋转矩阵和四元数之间存在一对一的对应关系。
输出语句cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;用于打印从旋转矩阵构造的四元数的系数。和之前一样，通过coeffs()方法获取四元数的系数，并使用transpose()方法将系数转置为行向量形式进行输出。
这样，你可以将旋转矩阵表示的旋转转换为对应的四元数表示，并进行后续的四元数操作和旋转变换。
// 使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q * v; // 注意数学上是qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;在这段代码中，使用四元数q对向量v进行旋转操作。
通过q * v这行代码，实现了四元数对向量的旋转操作。在数学上，四元数和向量的乘法运算定义为 qvq^{-1}，其中 q^{-1} 表示四元数的逆。在Eigen库中，通过重载运算符，可以直接使用乘法运算符*对四元数和向量进行乘法操作。
输出语句cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;用于打印旋转后的向量结果。通过v_rotated.transpose()将结果转置为行向量形式，并进行输出。
这样，通过四元数对向量进行旋转，可以得到旋转后的新向量。
在这段代码中，v是一个三维向量，表示原始的向量坐标。它的值是(1, 0, 0)，即在x轴上的单位向量。
在旋转操作中，我们使用四元数q对向量v进行旋转。通过 v_rotated = q * v，将四元数q与向量v相乘，实现了向量的旋转操作。
在Eigen库中，三维向量与四元数的乘法运算是通过运算符重载实现的。
当一个三维向量与一个四元数相乘时，Eigen库会自动进行运算符重载，按照四元数的乘法规则执行乘法操作。具体而言，向量与四元数的乘法操作会将向量视为一个四元数，其中实部为零，虚部为向量的各个分量。
假设有一个三维向量 v 和一个四元数 q，它们的乘法操作 v_rotated = q * v 会将向量 v 视为四元数 (0, v_x, v_y, v_z)，其中 v_x、v_y 和 v_z 是向量 v 的分量。然后，按照四元数乘法的规则进行运算，得到旋转后的向量 v_rotated。
需要注意的是，这里的乘法运算是基于四元数的Hamilton乘法规则执行的，即先进行对应分量的乘法和加法运算，然后根据四元数的定义进行合并和调整。
总结起来，三维向量与四元数的乘法运算是通过运算符重载实现的，遵循四元数乘法的规则进行计算。
cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;这行代码用于验证四元数的旋转性质。它计算了一个向量 (0, 1, 0) 绕着旋转四元数 q 进行旋转后的结果，并将其系数（coeffs）打印输出。
具体而言，(q * Quaterniond(0, 1, 0, 0) * q.inverse()) 执行了以下操作：
- 创建一个四元数
Quaterniond(0, 1, 0, 0)，表示向量(0, 1, 0)。 - 将该四元数乘以旋转四元数
q，即将向量(0, 1, 0)绕着q进行旋转。 - 再将结果乘以旋转四元数
q的逆，实现旋转的逆过程，得到旋转后的向量。
最后，(q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs() 获取旋转后的四元数的系数，然后通过 transpose() 方法将其转置，使结果以行向量形式输出。
这行代码的目的是验证四元数旋转的正确性，通过比较旋转后的结果与手动计算的结果是否相等来进行验证。
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
