Eigen中eulerAngles()得到欧拉角的奇异问题

Eigen中得到欧拉角的奇异问题

1.1.关于欧拉角范围的定义

欧拉角的实际范围是:

$roll:[-\pi ,\pi ];pitch:[-\pi /2,\pi /2];yaw:[-\pi ,\pi ]$

Eigen库中欧拉角范围是：

$0:[0:\pi ];1:[-\pi :\pi ];2:[-\pi :\pi ]$

1.2.Eigen中确保第一个角度的转换

Eigen库中所有均是右手坐标系的右乘，即内在旋转，输出欧拉角第一个范围[0, pi]，第二个范围[-pi, pi]，第三个范围[-pi, pi]，所有的输出结果以保证第一个值的范围
在Eigen，把轴角、旋转矩阵和四元数转换成欧拉角具体为：

// 轴角 -> 欧拉角(Z-Y-X，即RPY) （确保第一个值的范围在[0, pi]）
Eigen::Vector3d eulerAngle_aa = angleAxis_ea.matrix().eulerAngles(2,1,0);
cout << "EulerAngle(y,p,r) <-- AngleAxis: " << eulerAngle_aa.transpose()*180/M_PI << endl;

// 旋转矩阵 -> 欧拉角(Z-Y-X，即RPY)（确保第一个值的范围在[0, pi]）
Eigen::Vector3d eulerAngle_rm = rotationMatrix_ea.eulerAngles(2,1,0);
cout << "EulerAngle(y,p,r) <-- RotationMatrix: " << eulerAngle_rm.transpose()*180/M_PI << endl;

// 四元数 -> 欧拉角(Z-Y-X，即RPY)  （确保第一个值的范围在[0, pi]）
Eigen::Vector3d eulerAngle_qu = quaternion_ea.matrix().eulerAngles(2,1,0);
cout << "EulerAngle(y,p,r) <-- Quaternion: " << eulerAngle_qu.transpose()*180/M_PI << endl;

以上方法转换得到的欧拉角输出原则为：
Eigen库中所有均是右手坐标系的右乘，即内在旋转，不论输出顺序如何，输出欧拉角第一个值范围[0, pi]，第二个范围[-pi, pi]，第三个范围[-pi, pi]，所有的输出结果以保证第一个值的范围为基础。例如:

四元数$xyzw(0.00392036,-0.00511095,-0.613622,0.789573)$；
在Eigen中转换为欧拉角$ypr(1.82026,-3.13833,-3.12913)$;

如果要确保输出的pitch角满足范围$[-\pi /2,\pi /2]$，

即得到欧拉角$ypr(-1.32133,-0.00325971,0.0124636)$，

则需要单独处理。

1.3.确保pitch角度范围的转换

为了获得满足pitch角度范围的欧拉角，

• 可以使用 Conversion between quaternions and Euler angles 中给出的从四元数转换欧拉角的算法（基于Eigen进行了修改测试）：

//四元数 --> 欧拉角(Z-Y-X，即RPY)（确保pitch的范围[-pi/2, pi/2]）
Eigen::Vector3d Quaterniond2EulerAngles(Eigen::Quaterniond q) {
    Eigen::Vector3d angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
    angles(2) = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w() * q.y() - q.z() * q.x());
    if (std::abs(sinp) >= 1)
        angles(1) = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles(1) = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
    angles(0) = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

• 也可以基于旋转矩阵转换欧拉角，引用自eigen-eulerangles-returns-incorrect-values：

//旋转矩阵 --> 欧拉角(Z-Y-X，即RPY)（确保pitch的范围[-pi/2, pi/2]）
Eigen::Vector3d eulerAngle_mine;
Eigen::Matrix3d rot = rotationMatrix_ea;
eulerAngle_mine(2) = std::atan2(rot(2, 1), rot(2, 2));
eulerAngle_mine(1) = std::atan2(-rot(2, 0), std::sqrt(rot(2, 1) * rot(2, 1) + rot(2, 2) * rot(2, 2)));
eulerAngle_mine(0) = std::atan2(rot(1, 0), rot(0, 0));

尾巴

【1】Conversion between quaternions and Euler angles
【2】eigen-eulerangles-returns-incorrect-values