Proper Euler angle conversion - solution

[Spirit532]

Rather than posting it in every single duplicate issue that is currently in the list, I'm creating a new issue and posting the solution here.
It's a bit crude, but it does work:

#### This code converts the python hmdmatrix34_t equivalent that you get from pose.mDeviceToAbsoluteTracking into sort-of functioning Euler angles.
#### Euler angles are still Euler angles however, so be careful.

#From https://gist.github.com/awesomebytes/778d4a1720a0ec3af2086ff6a0b057c3
def from_matrix_to_pose_dict(matrix):
    pose = {}
    # From http://steamcommunity.com/app/358720/discussions/0/358417008714224220/#c359543542244499836
    position = {}
    position['x'] = matrix[0][3]
    position['y'] = matrix[1][3]
    position['z'] = matrix[2][3]
    q = {}
    q['w'] = math.sqrt(max(0, 1 + matrix[0][0] + matrix[1][1] + matrix[2][2])) / 2.0
    q['x'] = math.sqrt(max(0, 1 + matrix[0][0] - matrix[1][1] - matrix[2][2])) / 2.0
    q['y'] = math.sqrt(max(0, 1 - matrix[0][0] + matrix[1][1] - matrix[2][2])) / 2.0
    q['z'] = math.sqrt(max(0, 1 - matrix[0][0] - matrix[1][1] + matrix[2][2])) / 2.0
    q['x'] = math.copysign(q['x'], matrix[2][1] - matrix[1][2])
    q['y'] = math.copysign(q['y'], matrix[0][2] - matrix[2][0])
    q['z'] = math.copysign(q['z'], matrix[1][0] - matrix[0][1])
    pose['position'] = position
    pose['orientation'] = q
    return pose


#Ported from https://www.reddit.com/r/Vive/comments/6toiem/how_to_get_each_axis_rotation_from_vive/dlmczdn/
def NormalizeAngle(angle):
    while (angle > 360):
        angle -= 360
    while (angle < 0):
        angle += 360
    return angle

def NormalizeAngles(angles):
    angles['x'] = NormalizeAngle(angles['x'])
    angles['y'] = NormalizeAngle(angles['y'])
    angles['z'] = NormalizeAngle(angles['z'])
    return angles

def RadianToDegree(angles):
    angles['x'] = math.degrees(angles['x'])
    angles['y'] = math.degrees(angles['y'])
    angles['z'] = math.degrees(angles['z'])
    return angles

def get_proper_euler(matrix):
    in_quat = from_matrix_to_pose_dict(matrix)
    sqw = in_quat['orientation']['w'] * in_quat['orientation']['w']
    sqx = in_quat['orientation']['x'] * in_quat['orientation']['x']
    sqy = in_quat['orientation']['y'] * in_quat['orientation']['y']
    sqz = in_quat['orientation']['z'] * in_quat['orientation']['z']
    unit = sqx + sqy + sqz + sqw # if normalised is one, otherwise is correction factor
    test = in_quat['orientation']['x'] * in_quat['orientation']['w'] - in_quat['orientation']['y'] * in_quat['orientation']['z']

    v = { 'x': 0, 'y': 0, 'z':0 }

    if (test > 0.49995 * unit): #singularity at north pole
        v['y'] = 2.0 * math.atan2(in_quat['orientation']['y'], in_quat['orientation']['x'])
        v['x'] = math.pi / 2.0
        v['z'] = 0
        return NormalizeAngles(RadianToDegree(v))

    if (test > 0.49995 * unit): #singularity at south pole
        v['y'] = -2.0 * math.atan2(in_quat['orientation']['y'], in_quat['orientation']['x'])
        v['x'] = -math.pi / 2.0
        v['z'] = 0
        return NormalizeAngles(RadianToDegree(v))

    v['y'] = math.atan2(2 * in_quat['orientation']['x'] * in_quat['orientation']['w'] + 2.0 * in_quat['orientation']['y'] * in_quat['orientation']['z'], 1 - 2.0 * (in_quat['orientation']['z'] * in_quat['orientation']['z'] + in_quat['orientation']['w'] * in_quat['orientation']['w'])) # Yaw
    v['x'] = math.asin(2 * (in_quat['orientation']['x'] * in_quat['orientation']['z'] - in_quat['orientation']['w'] * in_quat['orientation']['y'])) # Pitch
    v['z'] = math.atan2(2 * in_quat['orientation']['x'] * in_quat['orientation']['y'] + 2.0 * in_quat['orientation']['z'] * in_quat['orientation']['w'], 1 - 2.0 * (in_quat['orientation']['y'] * in_quat['orientation']['y'] + in_quat['orientation']['z'] * in_quat['orientation']['z'])) # Roll
    return NormalizeAngles(RadianToDegree(v))

def convert_to_euler(pose_mat):
    x = pose_mat[0][3]
    y = pose_mat[1][3]
    z = pose_mat[2][3]
    rot = get_proper_euler(pose_mat)
    return [x,y,z,rot['x'],rot['y']-180,rot['z']] #Y is flipped!

[lauraflu]

There seem to be some issues with this code as well. All the angles seem to go from 0 to 360 degrees except for the rotation around the y axis, which is in [-180, 180] degrees. Also, although the rotation around the x and z axes should go continuously from 0 to 360, there seem to be some positions, around 180 and 0 degrees, respectively, when they suddenly jump from those values to 0 and 90 (also, respectively).

[Spirit532]

It's mathematically impossible to completely convert quat to euler without jumping, so there's always going to be issues. There has to be some logic on top to keep track of absolute rotation.

[lauraflu]

I agree, but the implementation currently in the repository seems to be more stable. I don't know if they fixed more things since your initial answer. I posted so that people know that there are issues with this implementation as well.

[Spirit532]

This was posted a while ago, so I think the repo version is better now.

[lgbeno]

Definitely make a pull request if you have better methods 😁

[lauraflu]

After playing around with it, I noticed that the repo version still has flaws. For instance, if the tracker is flat on the table (roll +/-180, pitch -90) and you change only the yaw, at yaw = -90 the roll and pitch become, respectively, 0 and 90.

Unfortunately, I'm not skilled enough to propose a solution of my own, since I'm new to the field. However, I started using the transforms3d package which has a function for quaternion to Euler conversion which seems to be quite stable (at least based on my measurements so far). If you prefer to rely on this external package, I can make a pull request.