从旋转矩阵中提取偏航、俯仰和滚动答案

【问题标题】：extract yaw, pitch, and roll from a rotationMatrix从旋转矩阵中提取偏航、俯仰和滚动
【发布时间】：2012-07-16 23:29:17
【问题描述】：

我有一个传感器管理器，它根据设备磁力计和加速度计返回 rotationMatrix。我一直在尝试计算用户设备的偏航俯仰和滚动，但发现俯仰和滚动相互干扰并给出不准确的结果。有没有办法从rotationMatrix 中提取设备的 YAW PITCH 和 ROLL？

编辑试图在下面解释搅拌机的答案，我很感激，但还没有，我试图从这样的旋转矩阵中获得角度：

       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]));

【问题讨论】：

标签： android android-sensors rotational-matrices

【解决方案1】：

我相信 Blender 的回答是不正确的，因为他给出了从旋转矩阵到欧拉角的转换（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));

Source

反馈：此实现显示在表示的奇异点附近缺乏数值稳定性（万向节锁定）。因此，在 C++ 上，我建议使用带有以下代码行的 Eigen 库：

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

（更多详情here）

【讨论】：

这个回复应该有一百万个+1。这是正确转换从 SensorManager.TYPE_ROTATION_VECTOR 获得的旋转矩阵的完美公式。
这里的“R”是什么？如果它是从 SensorManager.getRotationMatrix() 获取的旋转矩阵，那么在这种情况下，R 是一个单维数组，但在上面的答案中，它看起来像一个二维数组。
对不起，我对安卓编程一窍不通，我的代码只是源码方程的一个实现。
嘿 atan() 只有当我需要 -180 到 180 之间的值时才会给出 90 到 -90 之间的值？？
它是 atan2 而不是 atan 函数，它需要两个参数并提供四个象限中的角度。在您的编程语言中检查此功能。 en.wikipedia.org/wiki/Atan2

【解决方案2】：

Yaw、pitch 和 roll 对应于欧拉角。你可以很容易地convert a transformation matrix to Euler angles：

【讨论】：

谢谢，但老实说，这对我来说是希腊语。你愿意为我翻译那个..喜欢java吗？
嗯，那些是希腊字母 ;) A_31 表示第 3 行和第 1 列中的条目。 Java 内置了这些函数，因此您可以对其进行翻译。
请看我上面的编辑。我的旋转矩阵大小为 16 个值。不知道我这样做对不对..
第四行第四列可以忽略。矩阵在homogeneous coordinates
请注意，这些不是 XYZ 欧拉角，而是 3-1-3 转换中的欧拉角，如来源所述：en.wikipedia.org/wiki/…

【解决方案3】：

Sensor Manager 提供SensorManager.getOrientation 来获取所有三个角度。

【讨论】：

我发现getOrientation() 提供的“偏航”不太正确； javadoc 将其描述为“方位角”。不过，这一切都超出了我的凡人大脑。