pitch_roll_yaw from rotation_matrix

rotation_params_from_rotation_matrix.py

# Based on https://gist.github.com/crmccreary/1593090

import numpy as np
import math

def isclose(x, y, rtol=1.e-5, atol=1.e-8):
    return abs(x-y) <= atol + rtol * abs(y)

def euler_angles_from_rotation_matrix(R):
    yaw = 0.0
    if isclose(R[2,0],-1.0):
        pitch = math.pi/2.0
        roll = math.atan2(R[0,1],R[0,2])
    elif isclose(R[2,0],1.0):
        pitch = -math.pi/2.0
        roll = math.atan2(-R[0,1],-R[0,2])
    else:
        pitch = -math.asin(R[2,0])
        cos_theta = math.cos(pitch)
        roll = math.atan2(R[2,1]/cos_theta, R[2,2]/cos_theta)
        yaw = math.atan2(R[1,0]/cos_theta, R[0,0]/cos_theta)

    return np.rad2deg(pitch), np.rad2deg(roll), np.rad2deg(yaw)

if __name__ == '__main__':
    rotation_matrix = np.array([
        [0.9999830108, -0.0005844507985, -0.005799700295, ],
        [0.000448344363, 0.9997251109, -0.02344145153, ],
        [0.005811806396, 0.02343845301, 0.9997083884],
    ])

    pitch, roll, yaw = euler_angles_from_rotation_matrix(rotation_matrix)

    print('pitch in deg', pitch)
    print('roll in deg', roll)
    print('yaw in deg', yaw)

    # pitch in deg -0.3329938524499643
    # roll in deg 1.3430701114407941
    # yaw in deg 0.025688674477130958