qDemo.py 文件源码

def quatFromRotMatx(R):
    """Get a quaternion from a given rotation matrix `R`."""
    q = np.zeros(4)

    q[0] = ( R[0,0] + R[1,1] + R[2,2] + 1) / 4.0
    q[1] = ( R[0,0] - R[1,1] - R[2,2] + 1) / 4.0
    q[2] = (-R[0,0] + R[1,1] - R[2,2] + 1) / 4.0
    q[3] = (-R[0,0] - R[1,1] + R[2,2] + 1) / 4.0

    q[q<0] = 0   # Avoid complex number by numerical error.
    q = np.sqrt(q)

    q[1] *= np.sign(R[2,1] - R[1,2])
    q[2] *= np.sign(R[0,2] - R[2,0])
    q[3] *= np.sign(R[1,0] - R[0,1])

    return q