The following Python function converts a 3 x nf matrix, axisAngles, whose columns are axis-angles, to a 4 x nf matrix of quaternions under the (x,y,z,w) convention. Note that the quaternions should always have norm 1.
The following function converts a 4 x nf matrix, quats, whose columns are quaternions under the (x,y,z,w) convention, to a (3 x nf) matrix of axis-angle rotations. The function normalizes the quaternions to norm 1 before conversion.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
defquaternions_to_axis_angles(quats, wxyz=False): # convert to array for point-wise division quats = quats.getA() # normalzie the columns of quats norms = LA.norm(quats, axis=0) quats = quats/np.tile(norms, (4,1)) if wxyz: # get thetas thetas = 2.*np.arccos(quats[0,:]) # get rotation axis u u = quats[1:,:]/np.tile(np.sin(thetas/2.), (3,1)) else: # get thetas thetas = 2.*np.arccos(quats[3,:]) # get rotation axis u u = quats[:3,:]/np.tile(np.sin(thetas/2.), (3,1)) # get axis angle vectors axisAngles = np.matrix(u*thetas) return axisAngles