rvec2rpy_ros.py

    '''
    /** this conversion uses conventions as described on page:
    *   https://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
    *   Coordinate System: right hand
    *   Positive angle: right hand
    *   Order of euler angles: heading first, then attitude, then bank
    *   matrix row column ordering:
    *   [m00 m01 m02]
    *   [m10 m11 m12]
    *   [m20 m21 m22]*/
    '''
    def rvec2rpy_ros(self, rvec):

        m, _ = cv2.Rodrigues(rvec)

        # // Assuming the angles are in radians.
        if (m[1, 0] > 0.998):  # // singularity at north pole
            yaw = math.atan2(m[0, 2], m[2, 2])
            roll = math.PI / 2
            pitch = 0
        elif m[1, 0] < -0.998:  # // singularity at south pole
            yaw = math.atan2(m[0, 2], m[2, 2])
            roll = -math.PI / 2
            pitch = 0

        else:
            yaw = -math.atan2(-m[2, 0], m[0, 0]) + math.pi
            pitch = math.atan2(m[2, 2], m[1, 2]) + math.pi/2 # math.atan2(-m[1, 2], m[1, 1])
            roll = -math.asin(m[1, 0])

        return roll, pitch, yaw