Find roll, pitch, and yaw from the same vector in two different coordinate systems

If I know the original 3d vector and the roll, pitch, and yaw angles, I can use rotx, roty, and rotz to calculate the vector in the rotated coordinate system:

v2 = rotz(a) * roty(b) * rotx(c) * v1

How do I determine the yaw, pitch, and roll angles (a, b, & c) from the original and transformed 3d vectors (v1 & v2)? I have the original vector (v1) and the result of the transformation (v2), but need to determine the yaw, pitch, and roll angles (a, b, & c).

https://en.wikipedia.org/wiki/Rotation_matrix#General_3D_rotations shows that the rotation matrix is 3x3 with 3 variables (roll, pitch, and yaw). So this is a system of equations with three equations and three unknowns, so there should be sufficient information.

Answer 1

[Matt J]

So this is a system of equations with three equations and three unknowns, so there should be sufficient information.

No, there isn't. Even though you have 3 equations in 3 unknowns, they are not independent equations. The easiest way to see this is to recognize that there are infinitely many 3x3 rotation matrices R satisfying v2=R*v1, each with their own yaw,pitch,roll decompositions. For example, given any fixed solution R0, you can obtain a different solution R according to R=R0*Rv1(theta), where Rv1(theta) is an arbitrary rotation by angle theta about the v1 axis.

That said, of the infinitely many R satisfying v2=R*v1, there is a particularly natural choice, namely the rotation that rotates v1 toward v2 about their common normal n=cross(v1,v2). This R, and its yaw,pitch,roll decomposition can be calculated in a variety of ways, but a quick way would be to download this,

https://www.mathworks.com/matlabcentral/fileexchange/87584-object-oriented-tools-to-fit-plot-conics-and-quadrics?s_tid=srchtitle

and then do,

R=quadracFit.vecrot(v1,v2);

yaw_pitch_roll = quadricFit.rot2ypr(R);

(Possibly you would only need to download quadricFit, not the whole package.)

Answer 2

[James Tursa]

If you just want a "minimum angle rotation" solution, you can form a rotation quaternion from the vectors and then convert the quaternion to Euler Angles. E.g.,

v1 = (rand(3,1) - 0.5) * 100 % arbitrary vector

v1 = 3×1

44.8938 -45.8910 44.2119

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

v2 = rand(3,1) - 0.5; v2 = v2 * norm(v1) / norm(v2) % arbitrary vector same norm as v1

v2 = 3×1

35.8542 -17.8508 66.8726

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

v1n = v1 / norm(v1) % normalized v1

v1n = 3×1

0.5759 -0.5887 0.5672

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

v2n = v2 / norm(v2) % normalized v2

v2n = 3×1

0.4600 -0.2290 0.8579

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

e = cross(v1n,v2n) % eigen-axis of rotation, not yet normalized

e = 3×1

-0.3752 -0.2332 0.1389

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

sa = norm(e) % sine of rotation angle

sa = 0.4631

ca = dot(v1n,v2n) % cosine of rotation angle

ca = 0.8863

ang = atan2(sa,ca) % angle of rotation in radians

ang = 0.4815

e = e / norm(e) % normalized eigen-axis of rotation

e = 3×1

-0.8102 -0.5036 0.3000

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

q = quaternion( [cos(ang/2) ; e * sin(ang/2)]' ) % quaternion of rotation, MATLAB is scalar-first convention

q = quaternion
      0.97116 -  0.19316i -  0.12006j + 0.071514k

ea = q.euler( 'XYZ', 'point' ) * (180/pi) % convert to Euler angles in degrees with specified order

ea = 1×3

-23.6354 -11.8633 10.9137

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

c = ea(1); b = ea(2); a = ea(3); % pick off your c, b, a values in degrees
% check results
disp(v2) % display the desired rotated vector result v2

   35.8542
  -17.8508
   66.8726

disp(rotz(a) * roty(b) * rotx(c) * v1) % rotate v1 to see if it matches v2

   35.8542
  -17.8508
   66.8726

A more robust version of this algorithm would check to ensure that the Eigen axis calculation (e) was not zero. If it is, then this will end up being either a 0 or 180 degree rotation. So you could just have code to arbitrarily pick a or b or c to be the 0 or 180 value, and the other two set to 0.