Some quaternion code for numpy/Theano.

quaternions.py

def q_mult(q1, q2):
    w1, x1, y1, z1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3]
    w2, x2, y2, z2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3]
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    
    if isinstance(q1, np.ndarray):
        w = w[:, np.newaxis]
        x = x[:, np.newaxis]
        y = y[:, np.newaxis]
        z = z[:, np.newaxis]
        return np.concatenate([w, x, z, y], axis=1)
    else:
        w = w.dimshuffle(0, 'x')
        x = x.dimshuffle(0, 'x')
        y = y.dimshuffle(0, 'x')
        z = z.dimshuffle(0, 'x')
        
        return T.concatenate([w, x, y, z], axis=1)


def from_quat_to_quat(q1, q2):
    q1_conj = q1 * np.array([1, -1, -1, -1])[np.newaxis]
    q1_norm = np.sqrt((q1 ** 2).sum(axis=1))
    q1_inv = q1_conj / q1_norm[:, np.newaxis]
    from_q1_to_q2 = q_mult(q1_inv, q2)
    from_q1_to_q2 /= np.sqrt((from_q1_to_q2 ** 2).sum(axis=1))[:, np.newaxis]
    return from_q1_to_q2
    
    
def angle_btw_quats(q1, q2):
    from_q1_to_q2 = from_quat_to_quat(q1, q2)
    from_q1_to_q2_angleaxis = angleaxis_from_quat(from_q1_to_q2)
    return from_q1_to_q2_angleaxis[:, 0:1]


def angleaxis_from_quat(quat):
    angle = 2 * np.arccos(quat[:, 0:1])
    div = np.sqrt(1 - quat[:, 0:1] ** 2)
    axis = quat[:, 1:] / div
    #print div[np.where(np.isnan(div).any(axis=1))]
    return np.concatenate([angle, axis], axis=1)


def quat_from_angleaxis(angleaxis):
    angle, axis = angleaxis[:, :1], angleaxis[:, 1:]
    fac = np.sin(angle / 2)
    res = np.concatenate([np.cos(angle / 2), axis * fac], axis=1)
    return res / np.sqrt((res ** 2).sum(axis=1))

Hi,

I noticed two bugs in the code

return np.concatenate([w, x, z, y], axis=1)

This probably should be w,x,y,z

from_q1_to_q2 = q_mult(q1_inv, q2)

This I believe should be multiplied in the opposite order.

Cheers,
Kwang