第六章 相机及其应用 6.3欧拉角、旋转矩形、四元数、应用于Eigen的示例

欧拉角

**
**

一、顺规定义

**
欧拉角也可以描述三维刚体旋转，它将刚体绕过原点的轴(i,j,k)旋转θ，分解成三步（蓝色是起始坐标系，而红色的是旋转之后的坐标系。）。

1. 绕z轴旋转α，使x轴与N轴重合，N轴是旋转前后两个坐标系x-y平面的交线
2. 绕x轴（也就是N轴）旋转β，使z轴与旋转后的z轴重合
3. 绕z轴旋转γ，使坐标系与旋转后的完全重合

按照旋转轴的顺序，该组欧拉角被称为是“zxz顺规”的。对于顺规的次序，学术界没有明确的约定。

角值范围
α:[0，2pi]
β:[0，pi]
γ:[0，2pi]**

二、轴向定义

pitch、yaw，roll：

三、旋转矩阵

**

绕x，y，或z轴旋转θ的矩阵为：



**

在旋转矩阵一节中，最先进行的旋转其矩阵在最右侧，说明该矩阵最先与点的齐次坐标相乘，旋转矩阵按照旋转的次序从右向左排列。而在欧拉角中，最先进行的旋转其旋转矩阵在最左边。这是因为，**对于前者（旋转矩阵），我们始终是以绝对参考系为参照来的，对于后者（欧拉角），我们每一次旋转的刻画都是基于刚体的坐标系。**比如，在欧拉角中的第2步，绕x轴旋转β，这里的x轴实际上是N轴了（而不是蓝色的x轴）。

四、万向节死锁（Gimbal Lock）问题

1.陀螺仪

中间有一根竖轴，穿过一个金属圆盘。金属圆盘称为转子，竖轴称为旋转轴。转子用金属制成，应该是了增加质量，从而增大惯性。竖轴外侧是三层嵌套的圆环，它们互相交叉，带来了三个方向自由度的旋转。

其中Gimbal只代表陀螺仪装置中的平衡环，显然维基百科上将它解释成“平衡环架”更为合理。

2.陀螺仪的工作原理

• 现在假设，船体发生了摇晃，是沿着前方进行旋转的摇晃，也就是桶滚。由于转子和旋转轴具有较大的惯性，只要没有直接施加扭矩，就会保持原有的姿态。
• 再次假设，船体发生了pitch摇晃，也就是俯仰。同样，由于存在相应方向的可以相对旋转的连接头），转子和旋转轴将仍然保持平衡。
• 最后假设，船体发生了yaw摇晃，也就是偏航，此时船体在发生水平旋转。

3.陀螺仪中的万向节死锁

红色连接头：可以给予一个相对俯仰的自由度。
绿色连接头：可以给予一个相对偏航的自由度。
蓝色连接头：可以给予一个相对偏航的自由度。
三个连接头，提供的自由度只对应了俯仰和偏航两个自由度，桶滚自由度丢失了。这就是陀螺仪上的“万向节死锁”问题。

欧拉角的X轴进动造成最后的变化结果，受到到了预先执行的Z轴进动的影响，它仍然会造成某个相对自身的轴向的变化，但是结果不唯一；同样，欧拉角的Y轴进动，则受到了Z轴和X轴的影响，结果更加不唯一。

欧拉角Z轴的进动，最先执行，造成桶滚，这个没问题。
欧拉角Y轴的进动，最后执行，造成沿着欧拉旋转前的Y轴旋转，这也是根据定义执行。然而现在这种沿着Y轴的旋转，同样也被映射到了物体的桶滚变化。

小结：
总结来说，欧拉角的“万向节死锁”问题，是由于欧拉旋转定义本身造成的。这种围绕选旋转前固定轴的先Z、再X、再Y的旋转操作，与其最终所预期的三个轴向可以旋转的结果并非一定是一对一的映射。某些情况下是多对一的映射，造成一些旋转自由度的缺失，也就是“死锁”。

五、四元数

**

六、Eigen旋转矩阵，旋转向量，欧拉角，四元数互相转换的示例

**
/*
 * 本程序演示旋转向量，欧拉角，旋转矩阵，四元数之间的相互转换
 * Created By OuyangZizhou on 2019.5.7
 */
 
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
using namespace std;
 
int main()
{
    /*************分割线******************/
    // 初始化旋转向量，绕z轴旋转45°
    Eigen::AngleAxisd Rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));
 
    // 将旋转向量转化为旋转矩阵，用罗德里格斯公式
    Eigen::Matrix3d Rotation_matrix = Rotation_vector.matrix();
 
    // 旋转向量转换为四元数
    Eigen::Quaterniond q(Rotation_vector);
//    Eigen::Quaterniond q;
//    q = Rotation_vector;
 
    // 旋转向量转为欧拉角，顺序为z-y-x,也即yaw pitch roll
    Eigen::Vector3d eulerAngle = Rotation_vector.matrix().eulerAngles(2,1,0);
 
    /*************分割线******************/
    // 初始化旋转矩阵
    Eigen::Matrix3d R_matrix;
    // 以下为一种初始化方法，先直接采用上面的旋转矩阵
//    R_matrix << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    R_matrix = Rotation_matrix;
 
    // 旋转矩阵转化为旋转向量
    Eigen::AngleAxisd R_vector(R_matrix);
//    R_vector = R_matrix;
//    R_vector.fromRotationMatrix(R_matrix);
 
    // 旋转矩阵转化为四元数
    Eigen::Quaterniond q2(R_matrix);
//    q2 = R_matrix;
 
    // 旋转矩阵转化为欧拉角表示，欧拉角只能通过matrix表示出来
    Eigen::Vector3d eulerAngle1 = R_matrix.eulerAngles(2,1,0);
 
    /*************分割线******************/
    // 初始化欧拉角, yaw, pitch, roll
    Eigen::Vector3d eulerAngle2(M_PI/4,M_PI/5, M_PI/6);
 
    // 欧拉角转换为旋转向量，分别计算三个分量，然后将其相乘起来
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));
 
    Eigen::AngleAxisd rotation_vectorX;
    rotation_vectorX = yawAngle*pitchAngle*rollAngle;
 
    // 欧拉角转换为旋转矩阵，和旋转向量的计算同理
//    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
//    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
//    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));
 
    Eigen::Matrix3d R_matrix1;
    R_matrix1 = yawAngle*pitchAngle*rollAngle;
 
    // 欧拉角转换为四元数，和旋转向量的计算同理
    //    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
//    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
//    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));
    Eigen::Quaterniond q3;
    q3 = yawAngle*pitchAngle*rollAngle;
 
 
    /*************分割线******************/
    //初始化四元数 (w,x,y,z)
    Eigen::Quaterniond q4(0.1, 0.2, 0.3, 0.4);
    q4.normalize();// 要做这一步归一化，才能用来表示旋转
 
    // 四元数到旋转向量
    Eigen::AngleAxisd rotation_vector3(q4);
//    rotation_vector3 = q4;
//    cout << rotation_vector3.matrix() << endl;
 
 
    // 四元数到旋转矩阵
    Eigen::Matrix3d R_matrix4(q4);
    R_matrix4 = q4;
//    cout << R_matrix4;
//    R_matrix4 = q4.matrix();
//    R_matrix4 = q4.toRotationMatrix();
 
    // 四元数到欧拉角（Z-Y-X）
    Eigen::Vector3d eulerAngle3 = q4.matrix().eulerAngles(2,1,0);
 
    return 0;
}

引文：
https://www.cnblogs.com/yiyezhai/p/3176725.html
https://blog.csdn.net/weixin_38213410/article/details/89892336
https://blog.csdn.net/AndrewFan/article/details/60981437