23

I have a sensor manager that returns a rotationMatrix based on the devices Magnetometer and Accelerometer. I have been trying to also calculate the yaw pitch and roll of the user's device but am finding that pitch and roll interfere with each other and give inaccurate results. Is there a way to extract YAW PITCH and ROLL of a device from the rotationMatrix?

EDIT Trying to interpret blender's answer below, which i am thankful for but not quite there yet, i am trying to get the angle from a rotaion matrix like this:

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

i don't know if i am referencing the wrong parts of the matrix or not but i am not getting the results i would think.

i was hoping to get values in degrees, but am getting weird integers.

my matrix is coming from my sensorManager which looks like this:

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;

sample matrix when device is laying face up on table

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

ANSWER 

double rmPitch = Math.toDegrees( Math.acos(R[10]));
erik
  • 4,698
  • 11
  • 64
  • 111

3 Answers3

28

I believe Blender's answer is not correct, since he gave a transformation from Rotation matrix to Euler angles (z-x-z extrinsic), and Roll Pitch Yaw are a different kind of Euler angles (z-y-x extrinsic).

The actual transformation formula would rather be:

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

Feedback : this implementation revealed to lack numerical stability near the singularity of the representation (gimbal lock). Therefore on C++ I recommend using Eigen library with the following line of code:

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

(More details here)

Aquadarius
  • 420
  • 4
  • 5
  • 1
    This response should have a million +1. It's the perfect formula for transforming correctly the rotation matrix obtained from SensorManager.TYPE_ROTATION_VECTOR. – Fabio Marcolini Jul 13 '16 at 10:01
  • 1
    What is "R" here? If it's a rotation matrix taken from SensorManager.getRotationMatrix() then, in this case, R is a single dimen array but in above answer it seems like a 2D array. – Rahul Rastogi Aug 08 '17 at 12:41
  • 1
    I am sorry, don't know anything about Android programming, my code was just an implementation of the source's equations. – Aquadarius Aug 14 '17 at 09:02
  • hey atan() will gives values between 90 to -90 only if i need values between -180 to 180 then?? – Mirza Ahmed Baig May 28 '18 at 11:23
  • It is atan2 and not atan function, it takes two arguments and provide the angles in the four quadrants. Check for this function in your programming language. https://en.wikipedia.org/wiki/Atan2 – Aquadarius May 29 '18 at 01:17
19

Yaw, pitch and roll correspond to Euler angles. You can convert a transformation matrix to Euler angles pretty easily:

enter image description here

Blender
  • 257,973
  • 46
  • 399
  • 459
  • 9
    Thanks but to be honest that's, Greek to me. Would you be so kias to translate that for me.. like to java? – erik Jul 17 '12 at 03:18
  • 17
    Well, those *are* Greek letters ;) `A_31` means the entry in the 3rd row and the 1st column. Java has these functions built-in so you can translate it. – Blender Jul 17 '12 at 03:26
  • please see my edit above.. my rotation matrix is 16 values in size. don't know if i am doing this right.. – erik Jul 17 '12 at 12:21
  • The link you attached to your answer seems to be for a 3x3 matrix and because I'm using open gl my matrix is 4x4.. again can you take a look at the sensor manager that I added in my edit along with my attempt to implement your solution and tell me what I'm doing wrong? I thought Euler angles would be between 0 and 360 – erik Jul 17 '12 at 15:15
  • I mean, post a sample one. I don't deal with Java or Android, but I am familiar with Linear Algebra. – Blender Jul 18 '12 at 16:09
  • just did.. and thanks again.. my understanding that the 4'th row and 4th column are for openGL. – erik Jul 18 '12 at 16:56
  • 1
    You can ignore the fourth row and fourth column. The matrix is in [homogeneous coordinates](http://en.wikipedia.org/wiki/Homogeneous_coordinates) – Blender Jul 18 '12 at 17:01
  • so currently i am trying to do it like this: double rmYaw = Math.atan2(R[8], R[9]); double rmPitch = Math.acos(R[10]); double rmRoll = Math.atan2(R[2], R[6]); but that is still not giving me the right results.. i am still just as confused – erik Jul 18 '12 at 17:43
  • 3
    Take care, those are not XYZ Euler angles, but Euler angles in the 3-1-3 conversion, as the source states: https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Rotation_matrix_.E2.86.94_Euler_angles – Emiswelt Jan 02 '15 at 10:45
  • How to get above 3x3 matrix from SensorManager to use above formulae? – Rahul Rastogi Aug 08 '17 at 12:49
  • Euler angles are not the same as roll, pitch, yaw, though! The solution above gives you Euler angles, not RPY. – Alex Apr 04 '20 at 21:15
  • I want to ask about the calculation could be different when order of rotation replaced? I wonder in ARkit references calculate yaw from: atan2(r11, r12) in YXZ world ??? – lady Sep 05 '20 at 20:08
2

Sensor Manager provides a SensorManager.getOrientation to get all the three angle.

Pikanshu Kumar
  • 71
  • 3
  • 2
    I'm finding that the "yaw" provided by `getOrientation()` isn't quite right; the javadoc describes it as the "azimuth". It's all beyond my mortal brain though. – Steve Smith Sep 08 '17 at 10:32