Question

我正在使用Eigen C ++库。将四元数与它的倒数相乘应该总是给出零旋转，对吗？

我有一个特定的测试用例，而不是。

// This case works fine:
OrientationTestsEigen(1.7118, 0.8036, 1.5977);
// This fails:
OrientationTestsEigen(1.7118679426016505, 0.80361404966200567, 1.5977764119637190);

测试代码：

bool approxEqual(double a, double b)
{
    return fabs(a - b) < 0.0001;
}

void QuaternionToYPR(const Quaterniond& quat, double &y, double &p, double &r)
{
    Eigen::Vector3d rpy = quat.toRotationMatrix().eulerAngles(2, 0, 1);
    r = rpy.x();
    p = rpy.y();
    y = rpy.z();
}

void OrientationTestsEigen(double yaw, double pitch, double roll)
{
    // Convert Yaw, Pitch, Roll to Quaternion
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Quaterniond quaternionRepresentation = rollAngle *  pitchAngle * yawAngle;

    // quat * quat.inverse() should result in zero rotation
    Quaterniond shouldBeZero = quaternionRepresentation * quaternionRepresentation.inverse();
    double y3, p3, r3;
    QuaternionToYPR(shouldBeZero, y3, p3, r3);
    assert(fabs(y3) < 0.001);
    assert(fabs(p3) < 0.001);
    assert(fabs(r3) < 0.001);
}

我发现第一种情况很有意义，但第二种情况（值只有很小的差别）失败了。我得到的是：

测试用例1：正如预期的那样，偏航，俯仰和滚动为零。
测试用例2：我得到了偏航的PI，俯仰的PI，滚动的PI。间距仅上升到90°，因此有效地这是偏航= 0，俯仰= 0和滚动= 180°。

为什么我们在第二种情况下得到180°翻滚，而不是零？

Answer 1

相同的旋转可以用多个欧拉角（偏航，俯仰，滚动）表示。所以你的比较是不正确的。

您可以比较旋转矩阵或估计结果四元数和标识四元数之间的旋转角度。

shouldBeZero.angularDistance（Quaterniond ::身份（））;

quaternion.inverse（）*四元数不为零

1 个答案: