Quaternions

To visualize quaternions in the fanciest way, visit Ben eater, Quaternion.

Euler angles suffer from a problem of gimbal lock. When rotating around a 3-perpendicular axis in euclidean space, if either two of these axes align i.e becomes parallel, it causes gimbal lock. Once the object is locked, the object will lose one degree of freedom for rotation. This video provides an intuitive explanation of the problem.

Pitfalls

• When converting the Euler angle to a quaternion, it will lose some information. For example, Rotating about the x-axis for \(720^\circ\) will be equivalent to performing \(0^\circ\) rotation about the x-axis.
• When the amount of rotation is necessary, quaternions can’t preserve this information. For most computer graphics tasks, we can avoid this.

Representation of Unit Quaternion

• A unit quaternion of form \(w + x\hat{i} + y\hat{j} + z\hat{k}\), can represent a 3D rotation of any object in the space. The alternative form is angle axis representation \[(\theta,(x, y, z)) = \cos(\theta/2) + \sin(\theta/2)(x\hat{i} + y\hat{j} + z\hat{k})\]
• Property: For a given angle and axis, rotating about the negative angle and negated axis is the same as rotating about the given angle and axis.

Operations and Conversions

Addition:

Simple and straight forward. Add angles with angles, and axis with axis. \[q_1 = w_1 + x_1\hat{i} + y_1\hat{j} + z_1\hat{k}\] \[q_2 = w_2 + x_2\hat{i} + y_2\hat{j} + z_2\hat{k} \] \[q_1 + q_2 = (w_1 + w_2) + (x_1 + x_2)\hat{i} + (y_1 + y_2)\hat{j} + (z_1 + z_2)\hat{k}\] Or, \[[\theta_1, (x_1, y_1, z_1)] + [\theta_2, (x_2, y_2, z_2)] = [(\theta_1 + \theta_2) + (x_1 + x_2, y_1 + y_2, z_1 + z_2)]\]

Multiplication:

Quaternion multiplication is associative but not commutative. Using angle-axis notation, we can represent multiplication in easy terms \[q_1 = (\theta_1, \boldsymbol{a_1}), \textnormal{where} \boldsymbol{a_1} = x_1\hat{i} + y_1\hat{j} + z_1\hat{k}\] \[q_2 = (\theta_2, \boldsymbol{a_2}), \textnormal{where} \boldsymbol{a_2} = x_2\hat{i} + y_2\hat{j} + z_2\hat{k}\] Now, \[q_1q_2 = (\theta_1\theta_2 - \boldsymbol{a_1\cdot a_2}, \theta_1\boldsymbol{a_2} + \theta_2\boldsymbol{a_1} + \boldsymbol{a_1 \times a_2})\]

Note: Multiplying two quaternions results in adding the rotation.

Inverse

To calculate the inverse of the quaternions.

• Find the magnitude as: \(|q| = \sqrt{w^2 + x^2 + y^2 + z^2}\)
• Then, \(q^{-1} = \left(\dfrac{1}{|q|}\right)^2(\theta, -\boldsymbol{a}) = \left(\dfrac{1}{|q|}\right)^2 q^*\), where \(q^*\) represents conjugate

From Quaternion to Euler angle

def quat2euler(quat):
    """Requires quaternion in form of [w, x, y, z]"""
    w, x, y, z = quat
    test = x * y + z * w
    if test > 0.499:  # singularity at north pole
        yaw = 2 * np.arctan2(x, w)
        pitch = np.pi / 2
        roll = 0
        return np.array([roll, yaw, pitch]) * 180 / np.pi

    if test < -0.499:  # singularity at south pole
        yaw = -2 * np.arctan2(x, w)
        pitch = -np.pi / 2
        roll = 0
        return np.array([roll, yaw, pitch]) * 180 / np.pi

    sqx = x * x
    sqy = y * y
    sqz = z * z
    yaw = np.arctan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz)
    pitch = np.arcsin(2 * test)
    roll = np.arctan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz)

    return np.array([roll, yaw, pitch]) * 180 / np.pi

The link to original Java implementation.

From Euler angles to Quaternions

def euler2quat(angle):
    """Requires angle in order of Z, Y, X
    """
    yaw = angle[:, 0]
    pitch = angle[:, 1]
    roll = angle[:, 2]
    cy = np.cos(yaw * 0.5);
    sy = np.sin(yaw * 0.5);
    cp = np.cos(pitch * 0.5);
    sp = np.sin(pitch * 0.5);
    cr = np.cos(roll * 0.5);
    sr = np.sin(roll * 0.5);

    w = cr * cp * cy + sr * sp * sy;
    x = sr * cp * cy - cr * sp * sy;
    y = cr * sp * cy + sr * cp * sy;
    z = cr * cp * sy - sr * sp * cy;

    return np.array([w, x, y, z]).T

• The link to original implementation.

Quaternions to Matrix transformations

def quat2mat(quat):
    qw = quat[:, 0]
    qx = quat[:, 1]
    qy = quat[:, 2]
    qz = quat[:, 3]

    sqrx = np.square(qx)
    sqry = np.square(qy)
    sqrz = np.square(qz)

    rot = np.zeros((quat.shape[0], 3, 3))
    rot[:, 0, 0] = 1.0 - 2.0 * (sqry + sqrz)
    rot[:, 0, 1] = 2.0 * (qx * qy - qz * qw)
    rot[:, 0, 2] = 2.0 * (qx * qz + qy * qw)

    rot[:, 1, 0] = 2.0 * (qx * qy + qz * qw)
    rot[:, 1, 1] = 1.0 - 2.0 * (sqrx + sqrz)
    rot[:, 1, 2] = 2.0 * (qy * qz - qx * qw)

    rot[:, 2, 0] = 2.0 * (qx * qz - qy * qw)
    rot[:, 2, 1] = 2.0 * (qy * qz + qx * qw)
    rot[:, 2, 2] = 1.0 - 2.0 * (sqrx + sqry)
    return rot

This conversion is found in every standard text book.

Angle-axis representation

It comes form the fact that: \[[ \cos(\theta/2), a_x\sin(\theta/2), a_y\sin(\theta/2), a_z\sin(\theta/2)]^T = [w, x, y, z]^T\]

def angleaxis(quat):
    qw = quat[:, 0]
    qx = quat[:, 1]
    qy = quat[:, 2]
    qz = quat[:, 3]
    mag = np.sqrt(qw ** 2 + qx ** 2 + qy ** 2 + qz ** 2)
    angle = 2 * np.arccos(qw / mag)
    sin_factor = np.sqrt(1.0 - (qw / mag) ** 2)
    sin_factor[sin_factor == 0] = 1e-4
    axis = np.array([qx, qy, qz]) / sin_factor
    return angle.T, axis.T