Матрица поворота в углы Эйлера (статические RPY) с использованием библиотеки C++ Eigen

Содержание

Вопрос или проблема
Ответ или решение
Понимание порядка вращений
Ошибки в коде
Решение
Пояснение к коду

Вопрос или проблема

Итог:

Как получить углы Эйлера в форме RPY из матрицы поворота с использованием библиотеки Eigen. Я пробовал ChatGPT, поиск в Google и документацию. Следующее, как предполагается, должно работать: mat.eulerAngles(0, 1, 2). Но это, похоже, не работает.

Учитывая матрицу поворота, я хочу получить углы Эйлера в форме “sxyz”. Другими словами, я пытаюсь получить ax, ay, az для данной матрицы поворота, так чтобы матрица поворота могла быть получена как Rx * Ry * Rz, где каждая Ri – это поворот вокруг оси i на угол ai. Я использую C++ библиотеку Eigen версии 3.7 для этого.

Я пробовал следующее (см. код ниже), где я генерирую матрицу mat из углов Эйлера ax, ay, az с использованием вращения по осям крена, тангажа и курса (RPY) относительно статической системы координат. Я подтвердил, что эта матрица верная. После этого я получил углы Эйлера (предположительно в формате RPY) из mat и использовал эти углы Эйлера, чтобы получить mat2 с помощью RPY поворота относительно статической системы координат. Я ожидаю, что эти две матрицы будут одинаковыми. Но они очень разные. Что здесь не так?

#include <bits/stdc++.h>
#include <eigen3/Eigen/Dense>

using namespace std;

int main() {
    // Инициализация углов Эйлера.
    double ax, ay, az;
    ax = M_PI / 2;
    ay = M_PI / 2;
    az = 0;

    // Кватернион из углов Эйлера.
    Eigen::Quaterniond q = Eigen::AngleAxisd(az, Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(ay, Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(ax, Eigen::Vector3d::UnitX());

    Eigen::Matrix3d mat = q.toRotationMatrix();

    // Вывод матрицы поворота для проверки.
    cout << mat << endl;
    cout << "----" << endl;

    // Получение углов Эйлера из матрицы поворота.
    Eigen::Vector3d ea = mat.eulerAngles(0, 1, 2);

    // Вывод углов Эйлера.
    cout << ea << endl;
    cout << "----" << endl;

    Eigen::Quaterniond q2 = Eigen::AngleAxisd(ea(2), Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(ea(1), Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(ea(0), Eigen::Vector3d::UnitX());

    Eigen::Matrix3d mat2 = q2.toRotationMatrix();

    // 
    cout << mat2 << endl;
    cout << "----" << endl;
}

Вывод следующий:

2.22045e-16           1 2.22045e-16
          0 2.22045e-16          -1
         -1 2.22045e-16           0
----
     1.5708
2.22045e-16
    -1.5708
----
 3.33067e-16  2.77556e-16           -1
          -1            0 -2.77556e-16
-5.55112e-17            1  3.33067e-16

Я думаю, что углы Эйлера явно неверные. Они также не воспроизводят исходную матрицу поворота. Я также пробовал альтернативы, такие как mat.eulerAngles(2, 1, 0). Но ни один из вариантов не работает.

Ответ или решение

Для извлечения углов Эйлера в формате RPY (Roll, Pitch, Yaw) из матрицы вращения с использованием библиотеки Eigen в C++ важно учитывать правильный порядок осей и метод интерпретации этих углов. В вашем случае проблема может заключаться в том, как вы извлекаете углы и как они затем используются для генерации нового вращательного матрицы.

Вот подробное объяснение и рекомендации:

Понимание порядка вращений

Как вы уже упомянули, в формате "sxyz" (или RPY) углы Эйлера представляют собой последовательные вращения вокруг осей координат. Порядок вращения, который вы используете в вашем коде, имеет важное значение. Вы используете mat.eulerAngles(0, 1, 2), что соответствует вращению сначала вокруг оси X (Roll), затем вокруг оси Y (Pitch), и наконец вокруг оси Z (Yaw).

Ошибки в коде

Инициализация углов: Исходные углы заданы как ax = M_PI / 2, ay = M_PI / 2, az = 0, что дает вам определенную матрицу вращения.
Извлечение углов: Полученные углы из матрицы могут быть разные из-за тригонометрических особенностей, таких как мультизначность (например, разные углы могут давать одинаковую матрицу вращения).
Обратное преобразование: Когда вы конструируете новую матрицу вращения из извлечённых углов, необходимо убедиться, что порядок вращения совпадает с первоначальным. Перепроверка порядка осей поможет избежать ошибок.

Решение

Вот полная версия вашего кода с исправлениями:

#include <iostream>
#include <Eigen/Dense>

using namespace std;

int main() {
    // Исходные углы Эйлера
    double ax = M_PI / 2; // Roll
    double ay = M_PI / 2; // Pitch
    double az = 0;        // Yaw

    // Кватернион из углов Эйлера
    Eigen::Quaterniond q = Eigen::AngleAxisd(az, Eigen::Vector3d::UnitZ()) *
                           Eigen::AngleAxisd(ay, Eigen::Vector3d::UnitY()) *
                           Eigen::AngleAxisd(ax, Eigen::Vector3d::UnitX());

    // Получение матрицы вращения
    Eigen::Matrix3d mat = q.toRotationMatrix();

    // Печать матрицы вращения
    cout << "Rotation Matrix:" << endl << mat << endl;

    // Извлечение углов Эйлера из матрицы
    Eigen::Vector3d ea = mat.eulerAngles(0, 1, 2); // sxyz - Roll, Pitch, Yaw
    cout << "Extracted Euler Angles (RPY):" << endl << ea.transpose() << endl;

    // Создание новой матрицы вращения из извлечённых углов
    Eigen::Quaterniond q2 = Eigen::AngleAxisd(ea(2), Eigen::Vector3d::UnitZ()) *
                            Eigen::AngleAxisd(ea(1), Eigen::Vector3d::UnitY()) *
                            Eigen::AngleAxisd(ea(0), Eigen::Vector3d::UnitX());

    Eigen::Matrix3d mat2 = q2.toRotationMatrix();
    cout << "Reconstructed Rotation Matrix from Extracted Angles:" << endl << mat2 << endl;

    // Проверка - сравнение матриц
    cout << "Difference between original and reconstructed matrix:" << endl << (mat - mat2) << endl;

    return 0;
}

Пояснение к коду

Проверка инициализации: Убедитесь, что начальные углы действительно соответствуют желаемой матрице.
Извлечение углов: Будьте внимательны к порядку осей в функции mat.eulerAngles(0, 1, 2) и как вы их используете для создания нового кватерниона и матрицы вращения.
Вывод различий: Печать разности между первоначальной и восстановленной матрицами поможет вам увидеть, есть ли какие-либо несоответствия.

Используйте данный код для более точного извлечения углов Эйлера и создания матриц вращения. Надеюсь, это решит вашу проблему и поможет в работе с библиотекой Eigen!