извлекать рыскание, тангаж и крен из RotationMatrix

У меня есть диспетчер датчиков, который возвращает rotationMatrix на основе устройств Магнитометр и Акселерометр. Я пытался также рассчитать угол рыскания и крен устройства пользователя, но обнаружил, что тангаж и крен мешают друг другу и дают неточные результаты. Есть ли способ извлечь YAW PITCH и ROLL устройства из rotationMatrix?

РЕДАКТИРОВАТЬ Пытаясь интерпретировать ответ блендера ниже, за что я благодарен, но еще не совсем понял, я пытаюсь получить угол от матрицы вращения следующим образом:

       float R[] = phoneOri.getMatrix();
       double rmYaw = Math.atan2(R[4], R[0]);
       double rmPitch = Math.acos(-R[8]);
       double rmRoll = Math.atan2(R[9], R[10]);

Я не знаю, ссылаюсь ли я на неправильные части матрицы или нет, но я не получаю результатов, которые, как мне кажется.

Я надеялся получить значения в градусах, но получаю странные целые числа.

моя матрица исходит от моего sensorManager, который выглядит так:

public void onSensorChanged(SensorEvent evt) {
            int type=evt.sensor.getType();
            if(type == Sensor.TYPE_ORIENTATION){
                yaw = evt.values[0];
                pitch = evt.values[1];
                roll = evt.values[2];
            }
            if (type == Sensor.TYPE_MAGNETIC_FIELD) {
                orientation[0]=(orientation[0]*1+evt.values[0])*0.5f;
                orientation[1]=(orientation[1]*1+evt.values[1])*0.5f;
                orientation[2]=(orientation[2]*1+evt.values[2])*0.5f;
            } else if (type == Sensor.TYPE_ACCELEROMETER) {
                acceleration[0]=(acceleration[0]*2+evt.values[0])*0.33334f;
                acceleration[1]=(acceleration[1]*2+evt.values[1])*0.33334f;
                acceleration[2]=(acceleration[2]*2+evt.values[2])*0.33334f;
            }
            if ((type==Sensor.TYPE_MAGNETIC_FIELD) || (type==Sensor.TYPE_ACCELEROMETER)) {
                float newMat[]=new float[16];

                SensorManager.getRotationMatrix(newMat, null, acceleration, orientation);
                if(displayOri==0||displayOri==2){
                    SensorManager.remapCoordinateSystem(newMat,SensorManager.AXIS_X*-1, SensorManager.AXIS_MINUS_Y*-1,newMat);
                }else{
                    SensorManager.remapCoordinateSystem(newMat,SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X,newMat);
                }

                matrix=newMat;

образец матрицы, когда устройство кладется на стол лицевой стороной вверх

0.9916188,  -0.12448014, -0.03459576,  0.0
0.12525482,  0.9918981,   0.021199778, 0.0
0.031676512,-0.025355382, 0.9991765,   0.0
0.0,         0.0,         0.0,         1

ОТВЕТ

double rmPitch = Math.toDegrees( Math.acos(R[10]));

person erik    schedule 16.07.2012    source источник


Ответы (3)


Рыскание, тангаж и крен соответствуют углам Эйлера. Вы можете легко преобразовать матрицу преобразования в углы Эйлера:

введите описание изображения здесь

person Blender    schedule 16.07.2012
comment
Спасибо, но, честно говоря, для меня это греческий язык. Не могли бы вы перевести это для меня .. хотелось бы на java? - person erik; 17.07.2012
comment
Ну, это греческие буквы;) A_31 означает запись в 3-й строке и 1-м столбце. В Java есть эти встроенные функции, поэтому вы можете их переводить. - person Blender; 17.07.2012
comment
пожалуйста, посмотрите мою правку выше .. моя матрица вращения имеет размер 16 значений. не знаю, правильно ли я делаю это .. - person erik; 17.07.2012
comment
Ссылка, которую вы прикрепили к своему ответу, похоже, предназначена для матрицы 3x3, и поскольку я использую open gl, моя матрица 4x4 ... снова, можете ли вы взглянуть на диспетчер датчиков, который я добавил в свое редактирование вместе с моей попыткой реализовать ваше решение и подскажите что я делаю не так? Я думал, что углы Эйлера будут между 0 и 360 - person erik; 17.07.2012
comment
Я имею в виду, опубликуйте образец. Я не занимаюсь Java или Android, но знаком с линейной алгеброй. - person Blender; 18.07.2012
comment
только что сделал .. и еще раз спасибо .. Насколько я понимаю, 4-я строка и 4-й столбец предназначены для openGL. - person erik; 18.07.2012
comment
Вы можете игнорировать четвертую строку и четвертый столбец. Матрица имеет однородные координаты. - person Blender; 18.07.2012
comment
поэтому сейчас я пытаюсь сделать это так: double rmYaw = Math.atan2 (R [8], R [9]); двойной rmPitch = Math.acos (R [10]); двойной rmRoll = Math.atan2 (R [2], R [6]); но это все еще не дает мне правильных результатов .. я все еще в таком же замешательстве - person erik; 18.07.2012
comment
Будьте осторожны, это не углы Эйлера XYZ, а углы Эйлера в преобразовании 3-1-3, как сказано в источнике: en.wikipedia.org/wiki/ - person Emiswelt; 02.01.2015
comment
Как получить матрицу выше 3x3 из SensorManager, чтобы использовать приведенные выше формулы? - person Rahul Rastogi; 08.08.2017
comment
Однако углы Эйлера - это не то же самое, что крен, тангаж, рыскание! Приведенное выше решение дает вам углы Эйлера, а не RPY. - person Alex; 05.04.2020
comment
Хочу спросить, может ли расчет измениться при смене порядка поворота? Интересно, в ссылках на ARkit вычислить рыскание из: atan2 (r11, r12) в мире YXZ ??? - person lady; 05.09.2020

Я считаю, что ответ Блендера неверен, так как он дал преобразование из матрицы вращения в углы Эйлера (z-x-z внешние), а Roll Pitch Yaw - это разные углы Эйлера (z-y-x внешние).

Фактическая формула преобразования будет выглядеть так:

yaw=atan2(R(2,1),R(1,1));
pitch=atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2)));
roll=atan2(R(3,2),R(3,3));

Источник

Обратная связь: в данной реализации обнаружена нехватка числовой стабильности вблизи особенности представления (блокировка кардана). Поэтому на C ++ я рекомендую использовать библиотеку Eigen со следующей строкой кода:

R.eulerAngles(2,1,0).reverse();

(Подробнее здесь)

person Aquadarius    schedule 01.06.2016
comment
В этом ответе должен быть миллион +1. Это идеальная формула для правильного преобразования матрицы вращения, полученной из SensorManager.TYPE_ROTATION_VECTOR. - person Fabio Marcolini; 13.07.2016
comment
Что здесь R? Если это матрица вращения, взятая из SensorManager.getRotationMatrix (), тогда в этом случае R представляет собой массив с одним размером, но в приведенном выше ответе он выглядит как 2D-массив. - person Rahul Rastogi; 08.08.2017
comment
Извините, я ничего не знаю об Android-программировании, мой код был просто реализацией уравнений источника. - person Aquadarius; 14.08.2017
comment
эй, atan () даст значения от 90 до -90, только если мне нужны значения от -180 до 180, тогда ?? - person Mirza Ahmed Baig; 28.05.2018
comment
Это функция atan2, а не функция atan, она принимает два аргумента и предоставляет углы в четырех квадрантах. Проверьте эту функцию на своем языке программирования. en.wikipedia.org/wiki/Atan2 - person Aquadarius; 29.05.2018

В диспетчере датчиков есть SensorManager. getOrientation, чтобы получить все три угла.

person Pikanshu Kumar    schedule 12.11.2016
comment
Я нахожу, что рыскание, обеспечиваемое getOrientation(), не совсем правильное; javadoc описывает это как азимут. Но это все за пределами моего смертного мозга. - person Steve Smith; 08.09.2017