Android：SensorManager.getRotationMatrix 和 SensorManager.getOrientation() 的算法

Question

要在 Android 中从欧拉角（例如俯仰、滚转、方位角）中获取方向，需要执行以下操作：

1. SensorManager.getRotationMatrix(float[] R, float[] I, float[] Gravity, float[] geomagnetic);
   
2. SensorManager.getOrientation(float[] R, float[]orientation);
   

在第一个中，我意识到它使用了一种 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

我将给出几何解释getOrientation并解释如何仅使用RotationMatrix. 如果你了解getOrientation它的作用，那么就没有必要使用它，如果你不了解它，那么使用它会给你带来各种麻烦。

具体来说，它将回答 Stack Exchange 上发布的许多有关方向的问题，例如

• 在纵向模式下为平板电脑编写指南针。为什么打电话后getRotation需要加上90度才能得到正确的答案。
• 为什么RemapCoordinateSystem必须在调用之前调用getOrientation以获取后置摄像头的方向（设备 z 轴的负方向）。
• 为什么在调用RemapCoordinateSystem然后getOrientation正确获取设备z轴的方向后，当设备平坦时结果不再有意义。
• 如何计算与设备位置无关的任何设备轴的旋转，即平坦或不平坦。

首先我需要RotationMatrix更详细地解释一下，在解释之前它可以做什么getOrientation

这里需要考虑 2 个坐标系。
一种是世界坐标系，x 轴指向东，y 轴指向北，z 轴指向天空。
另一个是设备坐标系，x轴是手机的短边（平板电脑的长边），y轴是手机的长边（平板电脑的短边），z轴是正交向量到指向你的屏幕。

从数学上讲，我们的对象是 3 维实向量空间，我们对这个向量空间使用以下基。

世界基础W = {E, N, SKY}其中
E是东方向
N的单位向量 北方向
SKY的单位向量 是天空方向的单位向量

设备基D = {X, Y, Z}其中
X是手机短边方向（平板电脑长边）
Y的单位向量 手机长边方向（平板电脑短边）
Z的单位向量 是垂直于屏幕指向的方向的单位向量在你面前

对于那些忘记了基是什么的人来说，这意味着3 个空间中的每个向量v都可以写成

在世界基础上
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)。这 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相对于 Device 基的坐标，或者只是v 的坐标，其基隐含地理解为 Device 基。传感器返回的值以设备为基础，例如加速度计返回为 values[0]、values[1] 和 values[2] 写入设备基础将是
acc = values[0] X + values[1] Y + values[ 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后面看到如何使用这些向量来解释计算。

请注意，世界基础是固定的，但设备基础会随着手机位置的变化而变化。即单位向量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 与b的。

的参数getRotationMatrix包括gravity和geomagnetic。gravity假设是真实的gravity，即它的坐标在世界基础上是 (0, 0, k) 并且假设地磁位于世界 N-Sky 平面上，即它的坐标在世界基础上是 (0, a, b)。

有了这些假设，让我们看看 是如何Rotation Matrix计算的getRotationMatrix。它在这种方法中尝试的是获得一个矩阵M使得给定在 Device 基础上具有坐标 (a_1, a_2, a_3) 的任何向量，乘积M (a_1, a_2, a_3)_T (transpose) 给出坐标 (b_1, b_2 , b_3) 在世界基础上。从数学上讲就是getRotationMatrix计算基矩阵的变化。

gravity g的传入参数应该是从onSensorChangedforTYPE_GRAVITY或低通滤波器获得的值，TYPE_ACCELEROMETER而geomagnetic m应该是从的值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。我们现在要做的是{E, N, SKY}在 Device 基中写入 World基。

由于g被假定为重力，即世界基础中的 (0, 0, 1) 这正是向量SKY。

SKY = g = a_1 X + a_2 Y + a_3 Z
即SKY在Device base中的坐标为(a_1, a_2, a_3)

现在，由于假设m位于 World N-SKY 平面中，因此m和g的叉积是垂直于 World N-SKY 平面并指向右侧的单位向量，因此这是 E。

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

即E相对于 Device 基的坐标是 (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天空

写在设备坐标中将通过代入和乘出

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 个方程，因此我们应该能够根据E、N和SKY解出X、Y和Z 的这个方程。

从数学上讲，将上面的a、b和c放在列中得到的矩阵是基矩阵从World基到Device基的变化，因此我们需要找到它的逆。但是这个矩阵是一个正交矩阵，因此它的逆就是它的转置。

写成代码

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

给定设备基础中的任何坐标（a_1，a_2，a_3），我们可以通过取上述矩阵的乘积和（a_1，a_2，a_3）的转置来找到世界坐标中的坐标（b_1，b_2，b_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在 World 基础上的坐标是 (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在 World 基础上的坐标是 (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]);

文件说

values[0]：方位角，绕-Z轴旋转，即Z轴的反方向。
values[1]：pitch，绕-X轴旋转，即与X轴方向相反。
values[2]：滚动，绕 Y 轴旋转。

从字面上看上面的文档，不考虑设备的位置是错误的。该文件仅适用于azimuth设备平坦的情况。

在我们继续之前，让我注意所有计算都必须使用相对于相同基础的坐标来完成。例如，如果您想找到 2 个向量之间的角度，则这 2 个向量的坐标必须相对于相同的基础。

现在，当您计算旋转时，您会隐含地理解旋转是与固定位置的偏差。也就是说，如果答案是 90 度，那么它是 90 度，从什么地方开始？在azimuth它与磁北成 90 度的情况下，即您计算与N向量的偏差。如果你不能拼出这个隐含的固定位置，你就会遇到很多麻烦。例如，音高的固定向量是多少？卷？设备方向（纵向-横向）？

让我们看一下每个计算，看看它真正计算了什么。

对于azimuth官方的计算

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

让我们回到在设备坐标中写入Y的方式。如前所述，Y的坐标在 Device 基础中是 (0, 1, 0) 而Y在 World 基础中的坐标是 (a[1], a[4], a[7])

Y投影到 World XY 平面的 World base坐标是 (a[1], a[4])。这个投影向量和N向量的夹角是（画投影向量的图，N如果没搞明白的话）

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

这正是 的计算azimuth。

因此getOrientation计算设备 y 轴投影到世界 XY 平面与世界北轴之间的角度。因此，如果设备垂直放置，则此计算没有意义，因为投影的 y 坐标始终为 0。从几何上讲，如果您指向天空，则计算正北方向是没有意义的。此外，在这种情况下，围绕 -Z 轴旋转将意味着远离纵向旋转，因此文档不正确。

对于音高，官方计算是

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

Y矢量在World N-SKY平面上的投影在World base中的坐标是(a[4], a[7])，这个投影矢量与Y矢量在世界EN平面上的投影的夹角也就是Z向量投影到YZ平面上与重力向量的夹角是一样的（自己再画图）

Math.asin(-R[7]) 

因此，间距是设备 y 轴投影到 World N-SKY 平面与Y轴投影到 World EN 平面之间的角度

类似地，Rollθ 是设备 x 轴投影到 World X-SKY 平面与X投影到 World EN 平面之间的角度。

例如我之前指出的。

• 在纵向模式下为平板电脑编写指南针。为什么打电话后getRotation需要加上90度才能得到正确的答案。

在这种情况下，需要计算 x 轴投影到 World 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 轴的方向，而不是getOrientation计算的 y 轴。因此，您必须调用remapCoordinateSystemwhich 将 Z 轴映射到 Y 轴。在几何上，您所做的是将 z 轴旋转到 y 轴，现在 y 轴变为 -z 轴。因此，您需要否定旋转矩阵中的 y 列以返回原始定义坐标系。你可以得到-Z 的World 基础坐标，即 (-a[2], -a[5], -a[8]) 然后投影到 World XY 平面得到 (-a[2] ], -a[5]) 并进行计算。

注意：我在 2011 年第一次使用 Sensor 的类，在知道了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 轴正北的方向，现在它指向天空，因此计算不再有意义。

• 如何计算与设备位置无关的任何设备轴的旋转，即平坦或不平坦。

明天我将在Get Euler Yaw angle of an Android Device 上发布关于 z 轴旋转案例的答案。

注意：如果你只是想写一个指南针应用程序并且设备总是平坦的，那么 TYPE_ORIENTATION 仍然很好。

注2：我画画很差，所以不能张贴任何图片来说明。我什至无法使用 Gimp 画一条直线（我按照说明按住 shift 键，但我的线条参差不齐）。如果有人擅长绘画并愿意帮忙，只需发表评论，我将指导我想到的图片插图以及放置它们的位置。

Answer 2

让我试着解释一下：getRotationMatrix 在重力和磁矢量的基础上组成旋转矩阵。

我们这里的主要目标是构建NED 框架

我们假设重力指向地球的中心，而磁铁指向北极。但在实际情况下，这些向量是非垂直的，这就是为什么我们首先计算与 E 和 A 正交并属于切线平面的向量 H。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;

body frame中任意向量(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 的 Tiat Bryan 角是正确的。为了更好地理解从旋转矩阵到欧拉角的转换，我建议研究 Gregory G. Slabaugh的一篇文章——“从旋转矩阵计算欧拉角”