how to transform Optitrack quaternion to euler from one coodinate system to another

Question

I saw few similar post to this question, but none that provided a concrete final working solution.

I'm working with OptiTrack with python, Motive 2.2.0, NatNet SDK 4.0 using the NatNetClient from the examples provided with the SDK.

The coordinates system is such that Y is Up, X is backward and Z is left. I want to translate the quaternion to a coordinates system in which X is forward, Y is right (or left, the more simple one) and Z is up.

I'm getting the quaternion qx, qy, qz, qw values, I think that in this order but I'm not sure (if you can find it in the documentation of Motive/OptiTrack it also could help).

Now I'm trying by a plenty of similar ways that I think should work to get the Euler angles: pitch, roll and yaw, and then check for the three rotations in which I should get 0 to -180 and then to 180 and back to 0 (or vice versa), but it is always results in roll direction which goes from 0 to 90 then back to 0 (positive grow and then positive decrease) and then to -90 and then again back to 0 (negative decrease and then negative grow).

Correct me on this too, but I that is the result that serves as the sanity check for assurance the transformation to Euler was done correctly, right ?

First I take the pose and the quat and create an SO(3) matrix (just for convenience) :

def pos_quat2SE(quat_data):
    # Assumed quat_data order is (pos, quat)
    SO = R.from_quat(quat_data[3:7]).as_matrix()
    SE = np.matrix(np.eye(4))
    SE[0:3,0:3] = np.matrix(SO)
    SE[0:3,3]   = np.matrix(quat_data[0:3]).T
    return SE_motive

where quat_data is a simple concatenation of pos (3 values) and quat (4 values) as mentioned.

I tried to use scipy function:

from scipy.spatial.transform import Rotation as R

euler_transformed = R.from_matrix(SE_motive[0:3, 0:3]).as_euler('zyx', degrees=False)

but I'm not sure what should be the right argument for as_euler.

Also tried to use the following approach using this auxiliary function:

def SE_motive2transoform(SE_motive):
    T_Yup2NED_inv = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
    T_Yup2NED = invert_SE(T_Yup2NED_inv)
    SE_transformed = SE_motive @ T_Yup2NED
    return SE_transformed

The next two tries gave the same result:

using this functions which should be equivalent:

def euler_from_quaternion(x, y, z, w):
    """
    Convert a quaternion into euler angles (roll, pitch, yaw)
    roll is rotation around x in radians (counterclockwise)
    pitch is rotation around y in radians (counterclockwise)
    yaw is rotation around z in radians (counterclockwise)
    """
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + y * y)
    roll_x = math.atan2(t0, t1)

    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch_y = math.asin(t2)

    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (y * y + z * z)
    yaw_z = math.atan2(t3, t4)

    return roll_x, pitch_y, yaw_z  # in radians


def quaternion_to_rotation_matrix(q):
    """Return a 3x3 rotation matrix representing the orientation specified by a quaternion in x,y,z,w format.
    The matrix is a Python list of lists.
    """

    x = q[0]
    y = q[1]
    z = q[2]
    w = q[3]

    return [[w * w + x * x - y * y - z * z, 2 * (x * y - w * z), 2 * (x * z + w * y)],
            [2 * (x * y + w * z), w * w - x * x + y * y - z * z, 2 * (y * z - w * x)],
            [2 * (x * z - w * y), 2 * (y * z + w * x), w * w - x * x - y * y + z * z]]

on the input new_quat = np.vstack([quato[0], -quato[2], quato[1], quato[3]]) where qauto is the returned quaternion from the motive system in its mentioned above coordinate system. As much as I understand rearrangement of the quaternion values in that way should give me them in an xyz coordinate system and then I should been able to use the above function or even as_euler with xyz argument and etc. but it didn't work.

What is the shortest, working and elegant way to achieve the transform with the sanity check working of course ?

can it be done in that fashion:

def SE_motive2transoform(SE_motive):
    T_Yup2NED_inv = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
    T_Yup2NED = invert_SE(T_Yup2NED_inv)
    SE_transformed = SE_motive @ T_Yup2NED
    return SE_transformed

Thank you in advance.