Кватернионы, как мне объединить одну ось вращения с другой кватернионом? - PullRequest
Новая
       19

Кватернионы, как мне объединить одну ось вращения с другой кватернионом?

0 голосов
/ 14 марта 2020

У меня есть два кватерниона, которые составляют вращение, которое мне нужно. Тем не менее, один из них содержит вращение Yaw, а другой Pitch and Roll.

Что мне нужно сделать, это извлечь Yaw из Quaternion A, извлечь Pitch and Roll из Quaternion B, а затем объединить их в кватернион C.

У меня есть: 

   #include <Eigen/Dense>

#define M_PI 3.14159265358979323846264338327950288

int main()
{

    Eigen::Quaternionf quatA(0.775, 0.321, -0.208, 0.503); 

    Eigen::Quaternionf quatB(0.364, 0.606, 0.364, 0.606); 

    //A
    auto eulerA = quatA.toRotationMatrix().eulerAngles(0, 1, 2);

    float rollA = eulerA.x();
    float yawA = eulerA.y();
    float pitchA = eulerA.z();

    std::cout << "rotation A" << std::endl;
    std::cout << eulerA.x() * 180 / M_PI << " " << eulerA.y() * 180 / M_PI << " " << eulerA.z() * 180 / M_PI << std::endl;


    //B
    auto eulerB = quatB.toRotationMatrix().eulerAngles(0, 1, 2);

    float rollB = eulerB.x();
    float yawB = eulerB.y();
    float pitchB = eulerB.z();

    std::cout << "rotation B" << std::endl;
    std::cout << eulerB.x() * 180 / M_PI << " " << eulerB.y() * 180 / M_PI << " " << eulerB.z() * 180 / M_PI << std::endl;




    //create a new quat from just the Yaw.

    Eigen::Quaternionf QuatYaw = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX())
        * Eigen::AngleAxisf(yawB, Eigen::Vector3f::UnitY())
        * Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

    //Test the yaw rotation value

    auto testYaw = QuatYaw.toRotationMatrix().eulerAngles(0, 1, 2);
    std::cout << "Yaw value: " << testYaw.x() << " " << testYaw.y() * 180 / M_PI << " " << testYaw.z() << std::endl;

    //A back to quat
    Eigen::Quaternionf quatA_back = Eigen::AngleAxisf(rollA, Eigen::Vector3f::UnitX())
        * Eigen::AngleAxisf(yawA, Eigen::Vector3f::UnitY())
        * Eigen::AngleAxisf(pitchA, Eigen::Vector3f::UnitZ());

    Eigen::Quaternionf resultQuat = quatA_back * QuatYaw;


    //result
    auto eulerResult = resultQuat.toRotationMatrix().eulerAngles(0, 1, 2);

    std::cout << "result" << std::endl;
    std::cout << eulerResult.x() * 180 / M_PI << " " << eulerResult.y() * 180 / M_PI << " " << eulerResult.z() * 180 / M_PI << std::endl;



}

Это дает мне: 

rotation A
44.976 0.0301393 65.955
rotation B
-0 89.9693 117.99
Yaw value: -0 89.9693 0
result
134.996 155.955 -89.9807

Когда я ожидаю результат: 

44.976 89.9693 65.955

Как я могу объединить кватернионные оси таким образом? Куда я иду не так?

1 Ответ

0 голосов
/ 03 мая 2020

На первый взгляд, у вашего подхода есть две проблемы:

  1. первая техническая - вы смешиваете здесь порядок углов Эйлера: 

    float rollA = eulerA.x();
float yawA = eulerA.y();
float pitchA = eulerA.z();

  2. второй является концептуальным - углы Эйлера, как правило, взаимозависимы, и неправильно выделять и объединять из двух разных ориентаций.

...