add quaternions log and distance operation

[jacknlliu]

add quaternions log qlog() and distance qdist() operation.

import transforms3d.quaternions as q_op

def q_log(q):
    """
    Args:
        q: unit quternions

    Return: 
        the log of the quarternions: (3,) numpy array
    """
    # transform to a unit quaternion
    u_q = np.array(q)/q_op.qnorm(q)
	unit_q = q_op.fillpositive(np.array(u_q[1:4]))
    log_result = np.zeros(3)
    if not np.allclose(unit_q[1:4], np.zeros(3)):
        log_result =np.arccos(unit_q[0])*unit_q[1:4]/np.linalg.norm(unit_q[1:4])

    return log_result


def q_distance(q1, q2):
    """
    Args:
        q1,q2: unit quternions

    Return: 
        the distance of the quarternions
    """
    u_q1 = np.array(q1)/q_op.qnorm(q1)
    u_q2 = np.array(q2)/q_op.qnorm(q2)
    q_1 = q_op.fillpositive(np.array(u_q1[1:4]))
    q_2 = q_op.fillpositive(np.array(u_q2[1:4]))
    delta_q = q_op.qmult(q_1, q_op.qconjugate(q_2))

    log_q = q_log(delta_q)

    if not np.allclose(delta_q, np.array([-1, 0, 0, 0])):
        d = 2 * np.linalg.norm(log_q)
    else:
        d = 2 * np.pi 

    return d

Reference

[1] A. Ude, “Filtering in a unit quaternion space for model-based object tracking,” Robotics and Autonomous Systems, vol. 28, no. 2, pp. 163–172, Aug. 1999.

I'll create a pull request when I have time.

[matthew-brett]

Thanks - please do ...