quaternion

Ref & Credits: https://github.com/Krasjet/quaternion

Basics about complex number $\mathbb{C}$

$$\begin{array}{c}{i}^2=-1\\ z=a+bi=\left[\begin{array}{cc}a& -b\\ b& a\end{array}\right]\\ z_1{z}_2=z_2{z}_1\\ ||z||=\sqrt{a^2+b^2}=\sqrt{z\overline{z}}\end{array}$$

2D Rotation

2D rotation (counter-clockwise by $\theta$) can be represented by:

$$\begin{array}{c}{\mathbf{v}}^{\prime }=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\mathbf{v}\end{array}$$

A complex number can represents a 2D vector.

Multiply it with a complex number equals scaling and rotating in the 2D plane:

let $\theta =\arccos \frac{b}{\sqrt{a^2+b^2}},r=||z||=\sqrt{a^2+b^2}$:

$$\begin{array}{c}z=\left[\begin{array}{cc}a& -b\\ b& a\end{array}\right]=\sqrt{a^2+b^2}\left[\begin{array}{cc}\frac{a}{\sqrt{a^2+b^2}}& \frac{-b}{\sqrt{a^2+b^2}}\\ \frac{b}{\sqrt{a^2+b^2}}& \frac{a}{\sqrt{a^2+b^2}}\end{array}\right]=r\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\\ =r(\cos \theta +i\sin \theta )\\ =r{e}^{i\theta }\end{array}$$

Therefore, we also have $v^{\prime }=zv$ if the scaling factor $r=||z||=1$.

3D Rotation

3D rotation can be represented by three Euler angles $(\theta ,\varphi ,\gamma )$, but it relies on the axes system and can lead to Gimbal Lock.

$$\begin{array}{c}{\mathbf{R}}_x(\theta )=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \theta & -\sin \theta & 0\\ 0& \sin \theta & \cos \theta & 0\\ 0& 0& 0& 1\end{array}\right]\\ {\mathbf{R}}_y(\varphi )=\left[\begin{array}{cccc}\cos \varphi & 0& -\sin \varphi & 0\\ 0& 1& 0& 0\\ \sin \varphi & 0& \cos \varphi & 0\\ 0& 0& 0& 1\end{array}\right]\\ {\mathbf{R}}_z(\gamma )=\left[\begin{array}{cccc}\cos \gamma & -\sin \gamma & 0& 0\\ \sin \gamma & \cos \gamma & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$$

Another representation is axis-angle: rotation $\theta$ degree along axis $\mathbf{\text{u}}=(x,y,z{)}^T$, where $||\mathbf{u}||=1$.

(There are still only 3 Degree of Freedom)

which leads to the Rodrigues' Rotation Formula:

$$\begin{array}{c}{\mathbf{v}}^{\prime }=\cos \theta \mathbf{v}+(1-\cos \theta )(\mathbf{u}\cdot \mathbf{v})\mathbf{u}+\sin \theta (\mathbf{u}\times \mathbf{v})\end{array}$$

Basics about Quaternion $\mathbb{H}$

$$\begin{array}{c}q=a+bi+cj+dk={\left[\begin{array}{c}a,b,c,d\end{array}\right]}^T(a,b,c,d\in \mathbb{R})\\ ||q||=\sqrt{a^2+b^2+c^2+d^2}\end{array}$$

where

$$\begin{array}{c}{i}^2=j^2=k^2=ijk=-1\end{array}$$

by left-multiplying $i$ or right-multiplying $k$ to $ijk$, we have:

$$\begin{array}{c}ij=k,jk=i\end{array}$$

further left-multiplying $i$ or right-multiplying $j$ to $ij$ and similar to $jk$, we have:

$$\begin{array}{c}kj=-i,ik=-j,ji=-k\end{array}$$

lastly right-multiplying $i$ to $ji$, we have:

$$\begin{array}{c}ki=j\end{array}$$

which leads to an important difference with Complex number:

$$\begin{array}{c}{q}_1{q}_2\ne q_2{q}_1\end{array}$$

The matrix formulation of multiplication:

$$\begin{array}{c}{q}_1=a+bi+cj+dk,q_2=e+fi+gj+hk\\ q_1{q}_2=\left[\begin{array}{cccc}a& -b& -c& -d\\ b& a& -d& c\\ c& d& a& -b\\ d& -c& b& a\end{array}\right]\left[\begin{array}{c}e\\ f\\ g\\ h\end{array}\right]\\ q_2{q}_1=\left[\begin{array}{cccc}a& -b& -c& -d\\ b& a& d& -c\\ c& -d& a& b\\ d& c& -b& a\end{array}\right]\left[\begin{array}{c}e\\ f\\ g\\ h\end{array}\right]\end{array}$$

A more concise form can be represented by hybrid scalar-vector form (Grafman Product):

$$\begin{array}{c}{q}_1=[a,\mathbf{v}],q_2=[e,\mathbf{u}]\\ \mathbf{v}=[b,c,d{]}^T,\mathbf{u}=[f,g,h{]}^T\\ q_1{q}_2=[ae-\mathbf{v}\cdot \mathbf{u},a\mathbf{u}+e\mathbf{v}+\mathbf{v}\times \mathbf{u}]\end{array}$$

Pure quaternion

the real part equals 0.

$$\begin{array}{c}v=[0,\mathbf{v}],u=[0,\mathbf{u}]\\ vu=[-\mathbf{v}\cdot \mathbf{u},\mathbf{v}\times \mathbf{u}]\end{array}$$

Inverse

$$\begin{array}{c}q{q}^{-1}=q^{-1}q=1\end{array}$$

Conjugate

$$\begin{array}{c}q=[s,\mathbf{u}]\to q^{\ast }=[s,-\mathbf{u}]\\ (q^{\ast }{)}^{\ast }=q\\ q{q}^{\ast }=q^{\ast }q=||q|{|}^2=[s^2+\mathbf{u}\cdot \mathbf{u},0]\\ ||q||=||q^{\ast }||\\ q_1^{\ast }{q}_2^{\ast }=(q_2{q}_1{)}^{\ast }\end{array}$$

And we get a method to calculate the inverse:

$$\begin{array}{c}{q}^{-1}=\frac{q^{\ast }}{||q|{|}^2}=[\frac{s}{s^2+\mathbf{u}\cdot \mathbf{u}},\frac{-\mathbf{u}}{s^2+\mathbf{u}\cdot \mathbf{u}}]\end{array}$$

which also indicates:

$$\begin{array}{c}||q^{-1}||=\frac{1}{||q||}\\ (q^{-1}{)}^{-1}=q\end{array}$$

Quaternion for 3D Rotation

A pure quaternion can represent a 3D vector: $v=[0,\mathbf{v}]$

A unit quaternion can represent a 3D rotation: $||q||=1$

And we can rewrite the Rodrigues' Rotation Formula in a new form!

To rotate $\mathbf{v}$ for $\theta$ degree along axis $\mathbf{\text{u}}=(x,y,z{)}^T$, where $||\mathbf{u}||=1$,

define

$$\begin{array}{c}v=[0,\mathbf{v}],q=[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\mathbf{u}]\end{array}$$

note that \($||q||=1$\), we have:

$$\begin{array}{c}{v}^{\prime }=qv{q}^{\ast }=qv{q}^{-1}\end{array}$$

To understand it, we still need to decompose it: $$ v'=q(v_{||}+v_{\perp})q^* = qv_{||}q^+qv_{\perp}q^=qq^*v_{||}+qqv_\perp = v_{||}+qqv_\perp $$ where $$ qq = [\cos\theta, \sin\theta \mathbf u] $$ (rotate $\frac{\theta }{2}$ twice equals rotate $\theta$)

Inversely, given a unit quaternion $q=[a,\mathbf{b}]$, we can get the rotation angle and axis by:

$$\begin{array}{c}\theta =2\arccos a\\ \mathbf{u}=\frac{\mathbf{b}}{\sin \theta }\end{array}$$

Matrix form

Very complicated form...

Composition of rotation

Just do it sequentially,

$$\begin{array}{c}{v}^{\prime }=q_2(q_{1}v{q}_1^{\ast })q_2^{\ast }=(q_2{q}_1)v(q_2{q}_1{)}^{\ast }\end{array}$$

First apply $q_1$ then apply $q_2$ leads to equal rotation of $q_2{q}_1$.

Note that the order matters! $q_1{q}_2\ne q_2{q}_1$.

Double cover

One 3D rotation can be represented with TWO quaternions: $q$ and $-q$.

$$\begin{array}{c}(-q)v(-q{)}^{\ast }=qv{q}^{\ast }\\ -q=[-\cos \frac{\theta }{2},-\sin \frac{\theta }{2}\mathbf{u}]=[\cos (\pi -\frac{\theta }{2}),\sin (\pi -\frac{\theta }{2})(-\mathbf{u})]\end{array}$$

Notice that the matrix form is exactly the same for $-q$ and $q$, since all of the elements are multiplication of two coefficients!

Euler power form

$$\begin{array}{c}{e}^{\mathbf{u}\theta }=\cos \theta +\mathbf{u}\sin \theta \\ v^{\prime }=e^{\mathbf{u}\frac{\theta }{2}}v{e}^{-\mathbf{u}\frac{\theta }{2}}\end{array}$$

Implementation

import torch

def norm(q):
    # q: (batch_size, 4)

    return torch.sqrt(torch.sum(q**2, dim=1, keepdim=True))

def normalize(q):
    # q: (batch_size, 4)

    return q / (norm(q) + 1e-20)

def conjugate(q):
    # q: (batch_size, 4)

    return torch.cat([q[:, 0:1], -q[:, 1:4]], dim=1)

def inverse(q):
    # q: (batch_size, 4)

    return conjugate(q) / (torch.sum(q**2, dim=1, keepdim=True) + 1e-20)


def from_vectors(a, b):
    # get the quaternion from two 3D vectors, such that b = qa.
    # a: (batch_size, 3)
    # b: (batch_size, 3)
    # note: a and b don't need to be unit vectors.

    q = torch.empty(a.shape[0], 4, device=a.device)
    q[:, 0] = torch.sqrt(torch.sum(a**2, dim=1)) * torch.sqrt(torch.sum(b**2, dim=1)) + torch.sum(a * b, dim=1)
    q[:, 1:] = torch.cross(a, b)
    q = normalize(q)

    return q

def from_axis_angle(axis, angle):
    # get the quaternion from axis-angle representation
    # axis: (batch_size, 3)
    # angle: (batch_size, 1), in radians

    q = torch.empty(axis.shape[0], 4, device=axis.device)
    q[:, 0] = torch.cos(angle / 2)
    q[:, 1:] = normalize(axis) * torch.sin(angle / 2)

    return q

def as_axis_angle(q):
    # get the axis-angle representation from quaternion
    # q: (batch_size, 4)

    q = normalize(q)
    angle = 2 * torch.acos(q[:, 0:1])
    axis = q[:, 1:] / torch.sin(angle / 2)

    return axis, angle

def from_matrix(R):
    # get the quaternion from rotation matrix
    # R: (batch_size, 3, 3)

    q = torch.empty(R.shape[0], 4, device=R.device)
    q[:, 0] = 0.5 * torch.sqrt(1 + R[:, 0, 0] + R[:, 1, 1] + R[:, 2, 2])
    q[:, 1] = (R[:, 2, 1] - R[:, 1, 2]) / (4 * q[:, 0])
    q[:, 2] = (R[:, 0, 2] - R[:, 2, 0]) / (4 * q[:, 0])
    q[:, 3] = (R[:, 1, 0] - R[:, 0, 1]) / (4 * q[:, 0])

    return q

def as_matrix(q):
    # get the rotation matrix from quaternion
    # q: (batch_size, 4)

    R = torch.empty(q.shape[0], 3, 3, device=q.device)
    R[:, 0, 0] = 1 - 2 * (q[:, 2]**2 + q[:, 3]**2)
    R[:, 0, 1] = 2 * (q[:, 1] * q[:, 2] - q[:, 0] * q[:, 3])
    R[:, 0, 2] = 2 * (q[:, 1] * q[:, 3] + q[:, 0] * q[:, 2])
    R[:, 1, 0] = 2 * (q[:, 1] * q[:, 2] + q[:, 0] * q[:, 3])
    R[:, 1, 1] = 1 - 2 * (q[:, 1]**2 + q[:, 3]**2)
    R[:, 1, 2] = 2 * (q[:, 2] * q[:, 3] - q[:, 0] * q[:, 1])
    R[:, 2, 0] = 2 * (q[:, 1] * q[:, 3] - q[:, 0] * q[:, 2])
    R[:, 2, 1] = 2 * (q[:, 2] * q[:, 3] + q[:, 0] * q[:, 1])
    R[:, 2, 2] = 1 - 2 * (q[:, 1]**2 + q[:, 2]**2)

    return R

def mul(q1, q2):
    # q1: (batch_size, 4)
    # q2: (batch_size, 4)
    # return: q1 * q2: (batch_size, 4)

    q = torch.empty_like(q1)
    q[:, 0] = q1[:, 0] * q2[:, 0] - q1[:, 1] * q2[:, 1] - q1[:, 2] * q2[:, 2] - q1[:, 3] * q2[:, 3]
    q[:, 1] = q1[:, 0] * q2[:, 1] + q1[:, 1] * q2[:, 0] + q1[:, 2] * q2[:, 3] - q1[:, 3] * q2[:, 2]
    q[:, 2] = q1[:, 0] * q2[:, 2] - q1[:, 1] * q2[:, 3] + q1[:, 2] * q2[:, 0] + q1[:, 3] * q2[:, 1]
    q[:, 3] = q1[:, 0] * q2[:, 3] + q1[:, 1] * q2[:, 2] - q1[:, 2] * q2[:, 1] + q1[:, 3] * q2[:, 0]

    return q

def apply(q, a):
    # q: (batch_size, 4)
    # a: (batch_size, 3)
    # return: q * a * q^{-1}: (batch_size, 3)

    q = normalize(q)
    q_inv = conjugate(q)

    return mul(mul(q, torch.cat([torch.zeros(q.shape[0], 1, device=q.device), a], dim=1)), q_inv)[:, 1:]