Android: алгоритмы для SensorManager.getRotationMatrix и SensorManager.getOrientation()

Question

Чтобы получить ориентацию от углов эйлера (например, тангажа, крена, азимута) в Android, необходимо выполнить следующее:

1. SensorManager.getRotationMatrix (float [] R, float [] I, float [] гравитация, float [] геомагнитная);
   
2. SensorManager.getOrientation (float [] R, float [] ориентация);
   

В первом я понимаю, что он использует своего рода алгоритмы TRIAD; Матрица вращения (R[]) состоит из гравитации, геомагнитной гравитации X, гравитации X (геомагнитной гравитации X) --- X является перекрестным произведением.
Смотрите коды ниже:

    float Ax = gravity[0];
    float Ay = gravity[1];
    float Az = gravity[2];
    final float Ex = geomagnetic[0];
    final float Ey = geomagnetic[1];
    final float Ez = geomagnetic[2];
    float Hx = Ey*Az - Ez*Ay;
    float Hy = Ez*Ax - Ex*Az;
    float Hz = Ex*Ay - Ey*Ax;
    final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
    if (normH < 0.1f) {
        // device is close to free fall (or in space?), or close to
        // magnetic north pole. Typical values are  > 100.
        return false;
    }
    final float invH = 1.0f / normH;
    Hx *= invH;
    Hy *= invH;
    Hz *= invH;
    final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
    Ax *= invA;
    Ay *= invA;
    Az *= invA;
    final float Mx = Ay*Hz - Az*Hy;
    final float My = Az*Hx - Ax*Hz;
    final float Mz = Ax*Hy - Ay*Hx;
    if (R != null) {
        if (R.length == 9) {
            R[0] = Hx;     R[1] = Hy;     R[2] = Hz;
            R[3] = Mx;     R[4] = My;     R[5] = Mz;
            R[6] = Ax;     R[7] = Ay;     R[8] = Az;
        } else if (R.length == 16) {
            R[0]  = Hx;    R[1]  = Hy;    R[2]  = Hz;   R[3]  = 0;
            R[4]  = Mx;    R[5]  = My;    R[6]  = Mz;   R[7]  = 0;
            R[8]  = Ax;    R[9]  = Ay;    R[10] = Az;   R[11] = 0;
            R[12] = 0;     R[13] = 0;     R[14] = 0;    R[15] = 1;
        }
    }

Тем не менее, я не могу понять SensorManager.getOrientation().

 azimuth = (float)Math.atan2(R[1], R[4]);
 pitch = (float)Math.asin(-R[7]);
 roll = (float)Math.atan2(-R[6], R[8]);

Каковы точные алгоритмы получения эйлеровых углов?

Answer 1

Позвольте мне попытаться объяснить: getRotationMatrix составляет матрицу вращения на основе гравитации и вектора меганетики.

Наша главная цель - построить каркас NED.

Мы предполагаем, что гравитация указывает на центр Земли, а магнит - на северный полюс. Но в реальных случаях эти векторы неперпендикулярны, поэтому мы сначала вычисляем вектор H, который ортогонален E и A и принадлежит тангенциальной плоскости. H является перекрестным произведением (E x A) и ортогонально к E и A.

float Hx = Ey*Az - Ez*Ay;
float Hy = Ez*Ax - Ex*Az;
float Hz = Ex*Ay - Ey*Ax;
final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);

нормализовать ускорение и вектор H (потому что эти векторы будут составлять основу системы координат ENU)

final float invH = 1.0f / normH;
    Hx *= invH;
    Hy *= invH;
    Hz *= invH;
final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
    Ax *= invA;
    Ay *= invA;
    Az *= invA;

Найдите последний базисный вектор (M) как перекрестное произведение H и A:

double Mx = Ay * Hz - Az * Hy;
double My = Az * Hx - Ax * Hz;
double Mz = Ax * Hy - Ay * Hx;

Координаты произвольного вектора (a) в кадре тела выражаются через NED-координаты в виде a = Ra' R - матрица преобразования матрицы, столбцы которой являются координатами новых базисных векторов в старом базисе

Но координаты в кадре NED вычисляются как a' = T^(-1) * a. Для ортогонального преобразования матрица обратная равна транспонированной матрице. Таким образом, мы имеем:

R[0] = Hx;     R[1] = Hy;     R[2] = Hz;
R[3] = Mx;     R[4] = My;     R[5] = Mz;
R[6] = Ax;     R[7] = Ay;     R[8] = Az;

Если у нас есть матрица вращения, мы можем преобразовать ее в представление углов Эйлера. Формулы преобразования зависят от соглашения, которое вы используете. Ваши формулы

azimuth = (float)Math.atan2(R[1], R[4]);
pitch = (float)Math.asin(-R[7]);
roll = (float)Math.atan2(-R[6], R[8]);

верны для углов Тиата Брайана с условием YXZ. Чтобы лучше понять преобразование матрицы вращения в углы Эйлера, я бы предложил изучить статью Грегори Г. Слабо - "Вычисление углов Эйлера из матрицы вращения"

Answer 2

Я собираюсь дать геометрическую интерпретацию getOrientation и объясните, как вы можете рассчитать все виды вращений, используя только RotationMatrix, Если вы понимаете, что getOrientation тогда нет необходимости использовать его, и если вы этого не сделаете, то это может доставить вам много неприятностей.

В частности, он даст ответ на многие вопросы, касающиеся ориентации, размещенные на бирже стека, такие как

• Написание компаса для планшета в портретном режиме. Почему после звонка getRotation нужно добавить 90 градусов, чтобы получить правильный ответ.
• Почему RemapCoordinateSystem должен быть вызван перед звонком getOrientation получить направление задней камеры (отрицательное по оси Z устройства).
• Почему после звонка RemapCoordinateSystem а потом getOrientation чтобы правильно определить направление оси z устройства, когда устройство плоское, результат больше не имеет смысла.
• Как рассчитать вращение вокруг любой оси устройства, независимо от положения устройства, т.е. плоского или не плоского.

Сначала мне нужно объяснить RotationMatrix более подробно и что он может сделать, прежде чем объяснить getOrientation

Здесь необходимо рассмотреть 2 системы координат.
Одним из них является система координат мира с осью X, указывающей на восток, осью Y, указывающей на север, и осью Z, указывающей на небо.
Другая - это система координат устройства, где ось X - более короткая сторона телефона (более длинная сторона у планшета), ось Y - более длинная сторона телефона (более короткая сторона у планшета), а ось Z - вектор, ортогональный на экран, указывающий на вас.

Математически говоря, наш объект является трехмерным вещественным векторным пространством, и мы используем следующие базы для этого векторного пространства.

Мировая основа W = {E, N, SKY} где
E единичный вектор, лежащий в восточном направлении
N единичный вектор, лежащий в северном направлении
SKY единичный вектор, лежащий в направлении неба

Основа устройства D = {X, Y, Z} где
X единичный вектор, лежащий в направлении короткой стороны телефона (длинная сторона для планшета)
Y единичный вектор, лежащий в направлении длинной стороны телефона (короткая сторона для планшета)
Z единичный вектор, лежащий в направлении, перпендикулярном экрану, указывающему на вас

Для тех, кто забыл, что такое базис, это означает, что каждый вектор v в пространстве 3 можно записать в виде

В мировой основе
v = a_1 E + a_2 N + a_3 SKY, где a_1, a_2, a_3 - действительные числа
И обычно пишется как (a_1, a_2, a_3)_W или просто (a_1, a_2, a_3). Эти три кортежа называются координатой v относительно всемирного базиса или просто координатой v с неявным базисом, понимаемым как мировой.

В основе устройства
v = b_1 X + b_2 Y + b_3 Z, где b_1, b_2, b_3 - действительные числа
И обычно пишется как (b_1, b_2, b_3)_W или просто (b_1, b_2, b_3). Эти 3 кортежа называются координатой v относительно базиса Устройства или просто координатой v с базисом, неявно понимаемым как базис Устройства. Значения, возвращаемые датчиками, находятся в базе устройства, например, акселерометр, возвращенный как значения [0], значения [1] и значения [2], записанные в базе устройства, будут
acc = значения [0] X + значения [1] Y + значения [2] Z

Особенно
X = 1 X + 0 Y + 0 Z
Y = 0 X + 1 Y + 0 Z
Z = 0 X + 0 Y + 1 Z

то есть координаты X, Y и Z относительно основы устройства равны (1, 0, 0), (0, 1, 0) и (0, 0, 1) соответственно. Вы увидите, как эти векторы будут использованы для объяснения расчета в getOrientation потом.

Обратите внимание, что основание World является фиксированным, но основание Device изменяется по мере изменения положения телефона. То есть единичные векторы X, Y, Z изменяются при изменении положения устройства. Они определяются одинаково, но они являются разными векторами при изменении положения. Вы по-прежнему пишете их как X, Y, Z, но они разные X, Y, Z.
Таким образом, если телефон стоит на месте, единственной силой, действующей на него, является гравитация, и, следовательно, вектор акселерометра теоретически является вектором, лежащим на оси Мирового Неба, то есть на основе Мира координата равна (0, 0, g). В основе устройства это (a_1, a_2, a_3) для некоторых a_1, a_2 и a_3. Если устройство стоит в другой ориентации, акселерометр остается таким же, как (0, 0, g) в мире, но теперь он (b_1, b_2, b_3) в устройстве, где, по крайней мере, один a отличается от б.

Среди параметров для getRotationMatrix являются gravity а также geomagnetic, gravity считается реальным gravity то есть его координата равна (0, 0, k) в мировом базисе, и предполагается, что геомагнитный объект лежит на мировой плоскости N-Sky, то есть его координата равна (0, a, b) в мировом базисе.

С этим предположением давайте посмотрим, как Rotation Matrix рассчитывается в getRotationMatrix, В этом методе он пытается получить матрицу M такую, что при заданном любом векторе с координатами (a_1, a_2, a_3) в базе устройства произведение M (a_1, a_2, a_3) _T (transpose) дает координаты (b_1, b_2, б_3) в мировой основе. Математически говоря getRotationMatrix рассчитать изменение базисной матрицы.

Переданные параметры для gravity г должны быть значения, полученные из onSensorChanged за TYPE_GRAVITY или фильтр нижних частот TYPE_ACCELEROMETER и для geomagnetic м должны быть значения от TYPE_MAGNETIC_FIELD

g = a_1 X + a_2 Y + a_3 Z
m = b_1 X + b_2 Y + b_3 Z

Без ограничения общности предположим, что g и m уже нормализованы, то есть норма g и m равны 1. То, что мы хотим сделать сейчас, - это написать базу World. {E, N, SKY} в базе устройства.

Поскольку g предполагается гравитацией, то есть (0, 0, 1) в мировом базисе, это в точности вектор SKY.

SKY = g = a_1 X + a_2 Y + a_3 Z
То есть координата SKY в Базе устройства равна (a_1, a_2, a_3)

Теперь, поскольку предполагается, что m лежит в Мировой плоскости N-SKY, перекрестное произведение m и g является единичным вектором, ортогональным к Мировой плоскости N-SKY и указывающим направо, и, следовательно, это E.

E = m x g = b_1 X + b_2 Y + b_3 Z

То есть координата E относительно устройства является (b_1, b_2, b_3), b являются значениями, полученными в результате перекрестного произведения m и g.

Наконец, перекрестное произведение SKY и E является вектором, ортогональным к плоскости E-SKY и равным N

N = НЕБО x E = c_1 X + c_2 Y + c_3 Z

Собираем вместе у нас

E = b_1 X + b_2 Y + b_3 Z
N = c_1 X + c_2 Y + c_3 Z
SKY = a_1 X + a_2 Y + a_3 Z

Таким образом, учитывая любую координату в базе мира, мы можем найти координату в базе устройства. Например (1, 2, 3) это вектор v

v = E + 2 N + 3 SKY

записанная в координате устройства будет подстановка и умножение

v = b_1 X + b_2 Y + b_3 Z + 2 (c_1 X + c_2 Y + c_3 Z) + 3 (a_1 X + a_2 Y + a_3 Z)
v = (b_1 + 2 c_1 + 3 a_1) X + (b_2 + 2 c_2 + 3 a_2) Y + (b_3 + 2 c_3 + 3 a_3) Z

Но нас действительно интересует обратное, которое дает любую координату в базисе устройства и находит координату в базисе мира. Хорошо для тех, кто не помнит линейную алгебру, в вышеупомянутом у нас есть 3 уравнения в 3 неизвестных, таким образом, мы должны быть в состоянии решить это уравнение для X, Y и Z в терминах E, N и SKY.

Математически говоря, матрица, полученная путем помещения вышеупомянутых a, b и c в столбцы, является изменением базовой матрицы от Мировой основы к Базе Устройства, и, таким образом, нам нужно найти ее обратную. Но эта матрица является ортонормированной матрицей, и, следовательно, ее обратная является просто ее транспонированием.

Написано в коде

a[0]  a[1]  a[2]
a[3]  a[4]  a[5]
a[6]  a[7]  a[8]

учитывая любую координату (a_1, a_2, a_3) в основе устройства, мы можем найти координату (b_1, b_2, b_3) в координате мира, взяв произведение матрицы выше и транспонируя (a_1, a_2, a_3).

Особенно

a[0]  a[1]  a[2]      1       a[0]
a[3]  a[4]  a[5]  x   0   =   a[3]
a[6]  a[7]  a[8]      0       a[6]

Таким образом, координаты X в мировом базисе (a[0], a[3], a[6])

a[0]  a[1]  a[2]      0       a[1]
a[3]  a[4]  a[5]  x   1   =   a[4]
a[6]  a[7]  a[8]      0       a[7]

Таким образом, координаты Y в мировом базисе есть (a[1], a[4], a[7])

a[0]  a[1]  a[2]      0       a[2]
a[3]  a[4]  a[5]  x   0   =   a[5]
a[6]  a[7]  a[8]      1       a[8]

Таким образом, координаты Z в мировом базисе (a[2], a[5], a[8])

Теперь давайте посмотрим, что getOrientation делает.

Код для этого

azimuth = (float)Math.atan2(R[1], R[4]);
pitch = (float)Math.asin(-R[7]);
roll = (float)Math.atan2(-R[6], R[8]);

И в документе говорится

значения [0]: азимут, вращение вокруг оси -Z, то есть противоположное направление оси Z.
значения [1]: шаг, вращение вокруг оси -X, то есть в противоположном направлении оси X.
значения [2]: крен, вращение вокруг оси Y.

Воспринимайте вышеприведенный документ буквально, не считая положение устройства неправильным. Документ верный для azimuth только если устройство плоское.

Прежде чем мы продолжим, позвольте мне отметить, что все вычисления должны быть сделаны с координатами относительно того же самого основания Например, если вы хотите найти угол между 2 векторами, координаты этих 2 векторов должны быть относительно одного и того же базиса.

Теперь, когда вы вычисляете вращение, вы как бы неявно понимаете, что вращение - это отклонение от фиксированной позиции. То есть, если ответ 90 градусов, то это 90 градусов от чего? В случае с azimuth это 90 градусов от магнитного севера, то есть вы рассчитываете отклонение от вектора N. Если вы не можете разобрать эту неявную фиксированную позицию, у вас много неприятностей. Например, каков фиксированный вектор для шага? рулон? ориентация устройства (портрет-пейзаж)?

Давайте посмотрим на каждый расчет и посмотрим, что он действительно рассчитывает.

Для azimuth официальный расчет

azimuth = (float)Math.atan2(R[1], R[4]); 

Вернемся к тому, как написано Y в координате устройства. Как упоминалось ранее, координата Y равна (0, 1, 0) в базисе устройства, а координата Y в базисе мира равна (a[1], a[4], a[7])

Координата в Мировой основе проекции Y на мировую плоскость XY равна (a[1], a[4]). Угол между этим вектором проекции и вектором N равен (нарисуйте вектор проекции и N если не получится)

Math.atan2(R[1], R[4]);

что именно расчет для azimuth,

таким образом getOrientation рассчитать угол между проекцией оси y устройства на мировую XY-плоскость и мировой северной осью. Следовательно, если устройство удерживается вертикально, это вычисление не имеет смысла, поскольку координата y проекции всегда равна 0. Геометрически не имеет смысла вычислять направление на север, если вы указываете на небо. Кроме того, в этом случае поворот вокруг оси -Z будет означать поворот от портрета, поэтому документ будет некорректным.

Для Pitch официальный расчет

pitch = (float)Math.asin(-R[7]);

Координата в мировой основе проекции вектора Y в мировую плоскость N-SKY равна (a[4], a[7]), а угол между этим вектором проекции и проекцией вектора Y в мировую плоскость EN который равен углу между проекцией вектора Z на плоскость YZ и вектором гравитации (снова нарисуйте для себя)

Math.asin(-R[7]) 

Таким образом, шаг - это угол между проекцией оси y устройства на мировую плоскость N-SKY и проекцией Y на мировую плоскость EN

Точно так же Roll угол между проекцией оси x устройства в мировую плоскость X-SKY и проекцией X в мировую плоскость EN.

Например, я указал ранее.

• Написание компаса для планшета в портретном режиме. Почему после звонка getRotation нужно добавить 90 градусов, чтобы получить правильный ответ.

В этом случае нужно вычислить угол между проекцией оси x на мировую плоскость XY и, следовательно, нужно добавить 90 градусов, поскольку угол между осями x и y всегда равен 90 градусам. Вместо этого можно получить координату в Мировой основе X вектор, который (a[0], a[3], a[6]), а затем проецируется в плоскость World XY, чтобы получить (a[0], a[3]) и выполнить вычисление Math.atan2(R[0], R[3]).

• Почему RemapCoordinateSystem должен быть вызван перед звонком getOrientation получить направление задней камеры (отрицательное по оси Z устройства).

В этом случае вы хотите вычислить направление оси Z, а не оси Y, которая getOrientation рассчитывает. Таким образом, вы должны позвонить remapCoordinateSystem который отображает ось Z на ось Y. Геометрически то, что вы делаете, это поворачиваете ось z к оси y, и теперь ось y становится осью -z. Таким образом, вам нужно отменить y-столбец в Матрице вращения, чтобы вернуться к исходной системе координат определения. Вы можете просто получить координату в мировом базисе -Z, которая равна (-a[2], -a[5], -a[8]), а затем спроецировать в плоскость World XY, чтобы получить (-a[2], -a[5]) и сделать расчет.

Примечание: я впервые работаю с классами Сенсора в 2011 году, которые, узнав, что getRotationMatrix а также getOrientation до, я создал библиотеку, чтобы сделать расчет, используя только матрицу вращения. Я не думаю remapCoordinateSystem а также getOrientation нужны, но я не осознавал, что ими плохо пользоваться, пока не напишу этот ответ, так как он даст правильный azymuth но тогда дайте неправильное значение для pitch, Какие remapCoordinateSystem делает, чтобы сделать расчет azymuth в getOrientation исправить в соответствии с направлением, которое вы хотите рассчитать. Но тогда pitch неверное значение, вы должны добавить к нему -90 градусов, чтобы получить правильное значение для случая направления задней камеры и любого другого значения.

Если бы я написал этот класс, я бы создал

float getPitch(float[] rotMatrix)   
float getRoll(float[] rotMatrix) 
float getOrientation(float[] rotMatrix, int axis_you_want_to_calculate) 

Тогда не будет путаницы, и все ответы будут правильными.

• Почему после звонка RemapCoordinateSystem а потом getOrientation чтобы правильно определить направление оси z устройства, когда устройство плоское, результат больше не имеет смысла.

Вы пытаетесь вычислить направление оси -z к северу, и теперь она указывает на небо, поэтому вычисление больше не имеет смысла.

• Как рассчитать вращение вокруг любой оси устройства, независимо от положения устройства, т.е. плоского или не плоского.

Завтра я опубликую ответ для случая поворота вокруг оси z в углу Get Euler Yaw устройства Android.

Примечание. Если вы просто хотите написать приложение для компаса, а устройство всегда плоское, тогда TYPE_ORIENTATION все еще хорош.

Примечание 2: я ужасно рисую и поэтому не могу опубликовать картинку для иллюстрации. Я даже не могу нарисовать прямую линию, используя Gimp (я следовал инструкциям, чтобы удерживать клавишу Shift, но моя линия была неровной). Если кто-то хорошо рисует и хочет помочь, просто оставьте комментарий, и я дам инструкцию, какие иллюстрации на картинках я имел в виду и где их разместить.