quaternion.inverse() * quaternion не равен нулю

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: рыскание, тангаж и рулон равны нулю, как и ожидалось.
• Testcase 2: Я получаю -PI для рыскания, PI для подачи, PI для рулона. Шаг только поднимается до 90 °, поэтому эффективно это yaw = 0, pitch = 0 и roll = 180 °.

Почему мы получаем поворот на 180 ° во втором случае, а не ноль?

Answer 1

Такое же вращение можно представить с помощью углов MULtiple Euler (рыскание, шаг, рулон). Поэтому ваше сравнение неверно.

Вы можете сравнить матрицы вращения или оценить угол поворота между кватернионами результата и кватернионом идентичности.

shouldBeZero.angularDistance (Quaterniond :: Identity());