Неоднозначность при разложении высоты (вращения оси Yaxis) от матрицы вращения - PullRequest
Новая
       43

Неоднозначность при разложении высоты (вращения оси Yaxis) от матрицы вращения

0 голосов
/ 25 января 2019

Мне дают позу робота 4x4, где ось z направлена ​​вперед, ось x направлена ​​на восток (вправо), а ось y направлена ​​вниз.

Теперь, чтобы извлечь заголовокробот, я использовал следующую функцию 

void inline mat2xyh(Matrix4f& pose, float &x, float &y, float &heading){
    heading = atan2(-pose(2, 0),  sqrt(pose(2, 1) * pose(2, 1) +  pose(2,2) * pose(2,2)) );
    x = pose(0, 3);
    y = pose(2, 3); 
};

Чтобы проверить, я установил код ниже.Когда я исследовал угол разложенной матрицы и угол, который я использовал для построения матрицы вращения, они не совпадают!

Это было, когда я узнал, что есть 2 уникальных решения для высоты тона, или что диапазон для высоты тона равен -PI/2 < pitch < PI/2.

Теперь я понятия не имею, что робот повернут на 45 градусов или 135 градусов, есть ли какие-нибудь пути к этому? 

    Vector4f v(0, 0, 1, 0);

    MatrixXf rotation = AngleAxisf(135 * M_PI/180., Vector3f::UnitY()).toRotationMatrix();
    float x,y,h;
    Matrix4f pose1 = Matrix4f::Identity();
    pose1.topLeftCorner<3,3>() = rotation;

    mat2xyh(pose1, x,y,h);

    cout << pose1 << endl;
    cout << rad2deg(h) << endl;
    cout << "Pose: \n" << pose1 * v << endl;

    cout << "====================" << endl;

    MatrixXf rotation2 = AngleAxisf(45 * M_PI/180., Vector3f::UnitY()).toRotationMatrix();
    Matrix4f pose2 = Matrix4f::Identity();
    pose2.topLeftCorner<3,3>() = rotation2;

    mat2xyh(pose2, x,y,h);

    cout << pose2 << endl;
    cout << rad2deg(h) << endl;
    cout << pose2 * v << endl;

Результаты 

-0.707107         0  0.707107         0
        0         1         0         0
-0.707107         0 -0.707107         0
        0         0         0         1
heading: 45
Pose: 
 0.707107
        0
-0.707107
        0
====================
 0.707107         0  0.707107         0
        0         1         0         0
-0.707107         0  0.707107         0
        0         0         0         1
heading:45
Pose: 
0.707107
       0
0.707107
       0

1 Ответ

0 голосов
/ 25 января 2019

Если второй аргумент atan2 всегда неотрицателен, вы получите только углы между -pi/2 и pi/2. Если вам нужны углы между -pi и pi, вы можете рассчитать, например, 

heading = atan2(pose(0, 2), pose(2,2));

Кстати: как уже сказал @Neil в комментариях, «заголовок» также известен как «рыскание». «Pitch» и «roll» также упоминаются как «отношение» и «банк».

Кроме того: Нередки случаи, когда ось z направлена ​​вверх (или вниз), а ось x вперед, особенно в областях, которые обычно перемещаются только на поверхности. В компьютерном зрении ось z направлена ​​вперед, а ось x направлена ​​вправо. Т.е., действительно убедитесь, что вы знаете, какое соглашение использует ваш набор данных!

Чтобы узнать больше об углах Эйлера, я рекомендую прочитать:

...