Spatial Algebra

Quaternion

A quaternion is a four-part hypercomplex number used to describe 3D rotations and orientations.

Quaternions are used everywhere. To use this data for localization, you can capture it using a quaternion object, perform mathematical operations on it, or convert it to other rotation formats, such as Euler Angles and Rotation Matrix.

Fundamentals:

• https://docs.ros.org/en/foxy/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html
• Watch this video when you need to by Steven M. Lavalle
• http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lectures/Lecture8.pdf recommended by Raghava
• Also check out this https://eater.net/quaternions, which is an amazing intuitive video

$q=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]\in {\mathbb{R}}^4$

$||q||=1$

IMPORTANT: Not sure if I completely missed this part in the VSLAM book, but $q=-q$

• important detail because I thought that the orientation was unique

Double Cover Property (Singularity)

Was trying to remind myself why quaternions are better.

Quaternions have singularities in the context of representing orientation, known as the double-cover property. This means that two quaternions can represent the same orientation. Specifically, a quaternion (q) and its negation (-q) represent the same spatial orientation.

So then why are quaternions better if they also have singularities? Because

Notes from Visual SLAM book

Notes start at page 45 (chapter 2.4). A quaternion $\mathbf{q}$ has a real part and three imaginary parts. We write the real part in the front (and there are also some books where the real part is in the last), like this:

$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$$

where $i,j,k$ are three imaginary parts of the quaternion. These three imaginary parts satisfy the following relationship:

\begin{equation} \begin{cases} \begin{array}{l} {i^2} = {j^2} = {k^2} = - 1\\ ij = k, ji = - k \\ jk = i,kj = - i\\ ki = j, ik = - j \end{array} \end{cases} \end{equation}

If we look at $i,j,k$ as three axes, they look the same as complex numbers when multiplying with themselves. And look the same as the outer product when multiplying with the others.

We can also use a scalar and a vector to express quaternions: $\mathbf{q}={\left[s,\mathbf{v}\right]}^T,s=q_0\in \mathbb{R},\mathbf{v}=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$

• If the imaginary part $\mathbf{v}$ is $0$, it is called real quaternion
• Conversely, if its real part $s$ is $0$, it is called imaginary quaternion.

Rotation with Quaternions

To rotate a point $\mathbf{p}$ using Rotation Matrix, we know that is ${\mathbf{p}}^{\prime }=\mathbf{Rp}$. How do we do that with a quaternion?

Let the rotation be specified by a unit quaternion $\mathbf{q}$. First, we extend the 3D point to an imaginary quaternion: $\mathbf{p}=[0,x,y,z{]}^T=[0,\mathbf{v}{]}^T$

We just put the three coordinates into the imaginary part and leave the real part to be zero. Then, the rotated point ${\mathbf{p}}^{\prime }$ can be expressed as such a product:

${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}$

The multiplication here is the quaternion multiplication, and the result is also a quaternion. Finally, we take the imaginary part of ${\mathbf{p}}^{\prime }$ and get the coordinates of the point after the rotation.

It can be easily verified (we leave as an exercise here) that the real part of the calculation is 0, so it is a pure imaginary quaternion.

Properties of Quaternions

Quaternions are very similar to complex numbers, and a series of quaternion operations can be performed.

Assume there are two quaternions ${\mathbf{q}}_a,{\mathbf{q}}_b$, whose vectors are represented as $[s_a,{\mathbf{v}}_a{]}^{\mathrm{T}},[s_b,{\mathbf{v}}_b{]}^{\mathrm{T}}$, or the original quaternion is expressed as: ${\mathbf{q}}_a=s_a+x_{a}i+y_{a}j+z_{a}k,{\mathbf{q}}_b=s_b+x_{b}i+y_{b}j+z_{b}k$ Then, their operations can be expressed as follows.

1. Addition and subtraction ${\mathbf{q}}_a\pm {\mathbf{q}}_b={\left[s_a\pm s_b,{\mathbf{v}}_a\pm {\mathbf{v}}_b\right]}^T$

2. Multiplication. Quaternion multiplication is the multiplication of each item of ${\mathbf{q}}_a$ with each item of ${\mathbf{q}}_b$

The scalar form is a little complicated, but the vector form is more concise:

Note that the product of two real quaternions is still real, which is also consistent with the real number multiplication. NOTE: quaternion multiplication is usually NOT commutative unless ${\mathbf{v}}_a$ and ${\mathbf{v}}_b$ at ${\mathbb{R}}^3$ are parallel, which means the outer product term is zero.

3. Length. The length of a quaternion is defined as:

$$\Vert {\mathbf{q}}_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}.$$

It can be verified that the length of the product is the product of the lengths. This makes the unit quaternion keep unit-length when multiplied by another unit quaternion:

$$\Vert {\mathbf{q}}_a{\mathbf{q}}_b\Vert =\Vert {\mathbf{q}}_a\Vert \Vert {\mathbf{q}}_b\Vert$$

4. Conjugate. The conjugate of a quaternion is to take the imaginary part as the opposite:

$${\mathbf{q}}_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-{\mathbf{v}}_a{]}^T.$$

We get a real quaternion if the quaternion is multiplied by its conjugate. The real part is the square of its length:

\end{equation}$$ 5. **Inverse**. The inverse of a quaternion is: $$\begin{equation} \mathbf{q} ^ { - 1 } = \mathbf{q} ^ * / \| \mathbf{q} \| ^ 2 \end{equation}$$ According to this definition, the product of the quaternion and its inverse is the real quaternion $\mathbf{1}$ : $$\mathbf{q} \mathbf{q}^{-1} = \mathbf{q}^{-1} \mathbf{q} = \mathbf{1}$$ If $\mathbf{q}$ is a unit quaternion, its inverse and conjugate are the same. So the inverse of the product has properties similar to matrices: $$\begin{equation} \left( \mathbf{q}_a \mathbf{q}_b \right)^{-1} = \mathbf{q}_b^{-1} \mathbf{q}_a^{-1}. \end{equation}

6. Scalar Multiplication. Similar to vectors, quaternions can be multiplied by numbers: $k\mathbf{q}={\left[ks,k\mathbf{v}\right]}^T$

Quaternion as rotations

If you want to combine the effect of two quaternion rotations, you multiply them, not add them.

$q1\ast q2$

Conversion to other Rotation Representations

The math behind this starts getting quite hefty.

Quaternion to Rotation Matrix

Derivation at page 65 of Visual SLAM book.

Let $\mathbf{q}={\left[s,\mathbf{v}\right]}^T$. Then $\mathbf{R}=\mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+{({\mathbf{v}}^{\wedge })}^2$

In Eigen

q.toRotationMatrix();

Quaternion to Axis-Angle

Derivation at page 66 of Visual SLAM book.

Let $q=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]\in {\mathbb{R}}^4$. Then the conversion formula from quaternion to rotation vector can be written as

$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^T={\left[q_1,q_2,q_3\right]}^T/\sin \frac{\theta }{2}\end{array}$

In Eigen

Eigen::Vector3d axis = quaternion.axis(); 
double angle = quaternion.angle();
 
AngleAxisd rotation_vector(angle, axis);

Quaternion to Euler Angles

They don’t show it in the book. In Eigen, first convert to rotation matrix

q.toRotationMatrix().eulerAngles(0,1,2);

• q.eulerAngles(0,1,2) is not a valid function :(

No big deal though, Euler angles are rarely used apart to make it understandable for humans.

Quaternions in Eigen

AngleAxisd rotation_vector(M_PI/4, Vector3d(0,0,1));
Matrix3d rotation_matrix = Matrix3d::Identity();

Initialize a quaternion

// Quaternion (3 different ways)
Quaterniond q(rotation_matrix);  // using a 3x3 rotation matrix
Quaterniond q(rotation_vector); // using euler angles
Quaterniond q(w,x,y,z);  // raw values, where w,x,y,z are doubles

Rotate a vector

Vector3d v(1, 0, 0);
v_rotated =  q * v; // under the hood, it's qvq^{-1}
 
// equal to
v_rotated = q * Quaterniond(0, 1, 0, 0) ∗ q.inverse()).coeffs().transpose()

Related

• Euler Angle
• Axis-Angle Representation
• Rotation Matrix