How to find the rotation matrix of 3 orthogonal vectors in space. My current method rotates the vectors to the wrong orientation

Question

I am looking to find the rotation matrix for getting three (almost) orthogonal vectors to be in the same orientation of the world coordinate system.

My three (almost) orthogonal vectors can be represented like this in python:

vectors = np.array([[ 0.43187079,  0.90161148,  0.02417362],
   [-0.46076794,  0.19750816,  0.86526495],
   [ 0.77535832, -0.38482109,  0.50073167]])

The code I currently use can get the vectors to be parallel to the world coordinates but the orientation is incorrect. Running this code,

xrotation = np.arctan2(vectors[2, 1], vectors[2, 2])
xRot = np.array([[1, 0, 0],
                 [0, np.cos(xrotation), -np.sin(xrotation)],
                 [0, np.sin(xrotation), np.cos(xrotation)]])
vectors_x = np.zeros((3, 3))
for i in range(3):
    vectors_x[i, :] = np.linalg.inv(xRot.transpose()) @ vectors[i, :]
yrotation = np.arctan2(vectors_x[1, 2], vectors_x[1, 0])
yRot = np.array([[np.cos(yrotation), 0, np.sin(yrotation)],
                 [0, 1, 0],
                 [-np.sin(yrotation), 0, np.cos(yrotation)]])
vectors_y = np.zeros((3, 3))
for i in range(3):
    vectors_y[i, :] = np.linalg.pinv(yRot.transpose()) @ vectors_x[i, :]

zrotation = np.arctan2(vectors_y[0, 0], vectors_y[0, 1])
zRot = np.array([[np.cos(zrotation), -np.sin(zrotation), 0],
                 [np.sin(zrotation), np.cos(zrotation), 0],
                 [0, 0, 1]])
vectors_z = np.zeros((3, 3))
for i in range(3):
    vectors_z[i, :] = np.linalg.pinv(zRot.transpose()) @ vectors_y[i, :]

Gives the three rotated orthogonal vectors:

>vectors_z
>array([[-1.11022302e-16,  1.00000000e+00,  3.19660393e-09],
       [ 1.00000000e+00, -3.70417658e-09, -2.77555756e-16],
       [ 2.12261116e-09, -1.98949113e-09, -1.00000000e+00]])

What do I need to change in the code to get it in the correct orientation which would look like:

array([[ 1, 0, 0],
       [ 0, 1, 0],
       [ 0, 0, 1]])

I know it's possible to get this by rotating the vectors 90/180 deg in the correct order but there has gotta be a more efficient way to do this by doing something else in the code above.

Thanks for your time!!!

Answer 1

Figured it out. Switched to a ZYZ rotation pattern and redid the euler angle calculation method. Hope this helps someone some day.

import numpy as np

def z_rotation(zrotation):
    z1Rot = np.array([[np.cos(zrotation), -np.sin(zrotation), 0],
                      [np.sin(zrotation), np.cos(zrotation), 0],
                      [0, 0, 1]])
    return z1Rot

def y_rotation(yrotation):
    yRot = np.array([[np.cos(yrotation), 0, np.sin(yrotation)],
                     [0, 1, 0],
                     [-np.sin(yrotation), 0, np.cos(yrotation)]])
    return yRot

def forward_rotation(Rot,vectors_in):
    vectors = np.zeros((3, 3))
    for i in range(3):
        vectors[i, :] = vectors_in[i, :] @ Rot
    return vectors


def reverse_rotation(Rot, vectors_in):
    vectors = np.zeros((3, 3))
    for i in range(3):
        vectors[i, :] = np.linalg.pinv(Rot.transpose()) @ vectors_in[i, :]
    return vectors

org_vectors = np.array([[1,0,0],[0,1,0],[0,0,1]])

z1_angle = (-.5 + np.random.random()) * 1800
y_angle = (-.5 + np.random.random()) * 1800
z2_angle = (-.5 + np.random.random()) * 1800

z1 = z1_angle*np.pi/180
y = y_angle*np.pi/180
z2 = z2_angle*np.pi/180

z1Rot = z_rotation(z1)
z1vectors = forward_rotation(z1Rot, org_vectors)


yRot = y_rotation(y)
yvectors = forward_rotation(yRot, z1vectors)

z2Rot = z_rotation(z2)
z2vectors = forward_rotation(z2Rot, yvectors)

z2angle_calc = np.arctan2(z2vectors[2,1],z2vectors[2,0])
z2rot_2 = z_rotation(z2angle_calc)

new_y = forward_rotation(z2rot_2, z2vectors)

yangle_2 = np.arctan2(new_y[2,0],new_y[2,2])
yrot_2 = y_rotation(yangle_2)

new_z1 = forward_rotation(yrot_2, new_y)


z1angle_2 = yangle_2 = np.arctan2(new_z1[0,1],new_z1[0, 0])
z1rot_2 = z_rotation(z1angle_2)

new_org_vectors = forward_rotation(z1rot_2, new_z1)
print(new_org_vectors)