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]));
pitch = atan2( -r20, sqrt(r21*r21+r22*r22) ); yaw = atan2( r10, r00 ); roll = atan2( r21, r22 );
As in accelerometer one can use the X, Y and Z magnetometer readings to calculate yaw. mag_x = magReadX*cos(pitch) + magReadY mag_y = magReadY * cos(roll) - magReadZ yaw = 180 * atan2(-mag_y,mag_x)/M_PI; Now the most common question asked is, why can't we calculate yaw using accelerometer itself?
Pitch is a counterclockwise rotation of m about the y-axis. Roll is a counterclockwise rotation of s about the x-axis. Source publication. A strategy for heterogeneous multi-robot self-localization system.
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)
Yaw, pitch and roll correspond to Euler angles. You can convert a transformation matrix to Euler angles pretty easily:
Sensor Manager provides a SensorManager.getOrientation to get all the three angle.
If you love us? You can donate to us via Paypal or buy me a coffee so we can maintain and grow! Thank you!
Donate Us With