Conversion between axis angle and quaternion

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.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
def axis_angles_to_quaternions(axisAngles, wxyz=False):
nf = axisAngles.shape[1]
# convert to array for point-wise division
axisAngles = axisAngles.getA()
# get rotation axis and angles
angles = LA.norm(axisAngles, axis=0).reshape((1,nf))
axis = axisAngles/np.tile(angles, (3,1))
# compute quaternions
quats = np.zeros((4, nf))
if wxyz:
quats[0,:] = np.cos(angles/2.0).reshape(-1)
quats[1:,:] = axis * np.tile(np.sin(angles/2.0), (3,1))
else:
quats[:3,:] = axis * np.tile(np.sin(angles/2.0), (3,1))
quats[3,:] = np.cos(angles/2.0).reshape(-1)
return np.matrix(quats)

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
def quaternions_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

Comments