Math utilites for quaternions. A wrapper to Christoph Gohlke's quaternion library (tf.transformations.py in ros) as well as some quaternion distribution and difference functions.

Quaternion difference between two quaternions

q_diff = quatDiff(q1, q2)

Angular difference between two orientation quaternions

theta = quatAngularDiff(q1, q2)

Converts orientation quaternion to axis angle form

axis, angle = quat2AxisAngle(q)

The pdf of two uniformly sampled orientation quaternions having an angular difference of theta

pdf = angularPDF(theta)

To use rejection sampling to resample two uniformly sampled orientation quaternions so the difference between them is uniformly distributed, up theta_ref. Angle differences below theta_ref are not uniformly sampled but are boosted.

ref_pdf = angularPDF(theta_ref)
reject = invAngularPDF(theta, ref_pdf) > np.random.rand())

Samples a orientation quaternion from a uniform random distribution within theta of the q

q_near, q_delta = randomQuatNear(q, theta)