# Quaternions as 3D rotation representations
This notebook visualizes the use of unit quaternions as a 3D rotation representation.

**Subjects covered**  
1. **Definition and properties of quaternions.**
2. **Unit quaternion as a 3D rotation representation.**
3. **Visualizing quaternion rotation.**
4. **Solving a PnP problem using Levenberg–Marquardt and quaternions to encode camera poses** 
5. **Conclusion**
6. **Sources** 
   
Sections 1 and 2 serve as a theoretical underpinning and accessible explanation of methods used.   
They can be skipped by readers only interested in the algorithm's implementation. 

**Why are we interested in these subjects?**      
Quaternions, while less intuitive than rotation matrices, serve as a convenient representation of 3D rotation and pose.   
The quaternion avoids several shortcomings of rotation matrices, such as [gimbal lock](https://www.youtube.com/watch?v=zc8b2Jo7mno&ab_channel=GuerrillaCG) and the lack of a straightforward interpolation method.    
This has led to the popularity of quaternions in computer graphics, computer vision, robotics, and many other fields.  
Similar to 2-dimensional complex numbers, quaternions form a 4-dimensional number system, which makes them mathematically interesting.   
It also makes them quite difficult to comprehend in my opinion, but this notebook should alleviate that!

**Further reading**      
For a general overview of quaternions see [the Wikipedia page](https://en.wikipedia.org/wiki/Quaternion).  
For an accessible derivation of Quaternions, see [this blog](http://www.songho.ca/math/quaternion/quaternion.html) by Song Ho and [this Stanford tutorial](https://graphics.stanford.edu/courses/cs348a-17-winter/Papers/quaternion.pdf) by Yan-Bin Jia.     
For those especially interested in the use of quaternions for rotation, see [this Wikipedia page](https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation) and this [interactive visualization](https://eater.net/quaternions) by Ben Eater and Grant Sanderson.

# Quaternions

#### Definition
A quaternion is an element of the quaternion number system, which extends the complex numbers. It has one real and three imaginary components $i$, $j$, and $k$ and is generally represented in the form   

$$q =  a+b\ \mathbf {i} +c\ \mathbf {j} +d\ \mathbf {k}$$

Where $a$ is the "scalar part" and $ {b\ \mathbf {i} +c\,\mathbf {j} +d\,\mathbf {k}}$ is the "vector part" of the quaternion.

#### Mutiplication

Quaternions behave according to the following definitions:   

$$
 \begin{align}
  & \mathbf {i} ^{2} =\mathbf {j} ^{2}=\mathbf {k} ^{2}=\mathbf {i\,j\,k} =-1 \\     
  & \mathbf {i} \,1 =1\,\mathbf {i} =\mathbf {i} ,\qquad \mathbf {j} \,1=1\,\mathbf {j} =\mathbf {j} ,\qquad \mathbf {k} \,1=1\,\mathbf {k} =\mathbf {k} \,. \\   
  & \mathbf {i\,j} =\mathbf {k} \\
  & \mathbf {j\,i} =-\mathbf {k} \\   
  & \mathbf{jk} = \mathbf{i}  
  \end{align}
$$

Which can be summarized into a matrix. (screenshot from [Wikipedia article](https://en.wikipedia.org/wiki/Quaternion), as Jupyter does not allow table styling)
<center>

</center>

There is a geometrical interpretation of quaternion multiplication. Like that of complex numbers, it relates to rotation. The imaginary numbers $\mathbf {i\,j\,k}$ are interpreted as 3D basis vectors or "Quadrantal vectors" of a coordinate frame. Multiplying two of these vectors, e.g. $\mathbf{jk}$, is interpreted as counter-clockwise rotating $\mathbf{j}$ into $\mathbf{k}$ and taking the basis vector that is perpendicular to the rotation plane as the result. 

<center> 

</center> 


Mutiplication of two quaternions is known as the Hamilton product   
 
$$q_1q_2 = (a_1 + b_1\mathbf{i} + c_1\mathbf{j} + d_1\mathbf{k})\hspace{0.5em} (a_2 + b_2\mathbf{i} + c_2\mathbf{j} + d_2\mathbf{k})$$ 

and is determined by the distributive law the defined products of the basis elements. Thus, we distribute as one would do for a polynomial.

$${\displaystyle {\begin{alignedat}{4}&a_{1}a_{2}&&-b_{1}b_{2}&&-c_{1}c_{2}&&-d_{1}d_{2}\\{}+{}(&a_{1}b_{2}&&+b_{1}a_{2}&&+c_{1}d_{2}&&-d_{1}c_{2})\mathbf {i} \\{}+{}(&a_{1}c_{2}&&-b_{1}d_{2}&&+c_{1}a_{2}&&+d_{1}b_{2})\mathbf {j} \\{}+{}(&a_{1}d_{2}&&+b_{1}c_{2}&&-c_{1}b_{2}&&+d_{1}a_{2})\mathbf {k} \end{alignedat}}}$$

Note that quaternion multiplication is not commutative, it is associative, and distributive across addition.
  
$$
\begin{align}
 pq & \ne qp  \\
 (op)q &= o(pq)\\
 o(p + q) & = op + oq \\
\end{align}
$$
  
#### Addition
Quaternion addition, e.g. $q_1 + q_2$, entails adding the real parts of $q_1$ to $q_1$ and adding the imaginary parts of $q_1$ to the corresponding imaginary parts of $q_2$. 


$$  
\begin{align}
q_1 + q_2 & = (a_1 + b_1\mathbf{i} + c_1\mathbf{j} + d_1\mathbf{k}) + (a_2 + b_2\mathbf{i} + c_2\mathbf{j} + d_2\mathbf{k}) \\
 & = (a_1 + a_2) + \mathbf{i} (b_1 + b_2) + \mathbf{j} (c_1 + c_2) + \mathbf{k} (d_1 + d_2)
\end{align}
$$ 

#### Subtraction 
Similar to addition 
$$
q_1 - q_2  = (a_1 - a_2) + \mathbf{i} (b_1 - b_2) + \mathbf{j} (c_1 - c_2) + \mathbf{k} (d_1 - d_2)
$$

#### Scalar multiplication
Quaternion scalar multiplication, e.g. $r q$, simply entails multiplying the real part of $q$ by $r$ and each imaginary part by $r$. 

$$ 
\begin{align}
r q & = (r a + r (b \mathbf{i} + c \mathbf{j} +  \mathbf{k}))  \\
    & = (r a + r b \mathbf{i} + r c \mathbf{j} + r \mathbf{k}) 
\end{align}
$$



#### Conjugate
The conjugation of quaternions is analogous to the conjugation of complex numbers. That is, for a complex number $z = a + b\textbf{i}$ the complex conjugate is $z^{*} = a - b\textbf{i} $. 
For a quaternion $q$, the complex conjugate is defined by negating the vector part
$$
\begin{align}
q & =  a+b\ \mathbf {i} +c\ \mathbf {j} +d\ \mathbf {k} \\
q^{*} & =a - b\,\mathbf {i} -c\,\mathbf {j} -d\,\mathbf {k}
\end{align}
$$   
   
Some complex conjugate properties:
$$
\begin{align}
(q^*)^* & = q \\
(p + q)^* & = p^* + q^* \\
(pq)^* &= q^*p^* \\
qq^* &= q^*q
\end{align}
$$

#### Norm 
The square root of the product of a quaternion with its conjugate is called its norm and is denoted $\lVert q \rVert$
$$ 
\lVert q \rVert = \sqrt {qq^{*}}= \sqrt {q^{*}q} = \sqrt {a^{2}+b^{2}+c^{2}+d^{2}}
$$

A real number, $\alpha \in \mathbb{R} $, scales a quaternion's norm by the absolute value of the number. 

$$  \lVert \alpha q \rVert =  \lvert \alpha \rvert \hspace{0.5em} \lVert q \rVert$$

This is entailed by the fact that the norm of two quaternions is multiplicative.

$$ \lVert pq \rVert = \lVert p \rVert \hspace{0.5em} \lVert q \rVert $$

The norm of a quaternion is equal to that of its conjugate 
$$ \lVert q \rVert = \lVert q^* \rVert$$   

This definition of a norm is analogous to the Euclidian norm in a vector space $\mathbb{R}^{n}$ which allows for an analogous definition of the distance between quaternions.  

$$ d(p, q) = \lVert p - q \rVert $$

A unit quaternion has a norm of one. We will see later how it is used as a representation of 3D rotation.

$$ \mathbf {U} q=\frac {q}{\lVert q \rVert } $$

#### Inverse 
The reciprocal of a quaternion can be defined using the norm and complex conjugate.
$$ q^{-1}={\frac {q^{*}}{\lVert q\rVert ^{2}}}$$  

$$ q^{-1}q = q q^{-1} = 1 $$

The inverse of a unit quaterion is its conjugate

$$ \mathbf {U} q^{-1} = \mathbf {U} q^* $$ 

## Matrix & vector forms of quaternions
Real valued matrices $\mathbb{R}^{m \times n}$ can be used to represent quaternion addition and multiplication. That is, we can convert the four values of quaternions into a matrix representation. Matrix multiplication and addition will correspond to quaternion multiplication and addition. The conjugate of the quaternion will correspond to the transpose of the matrix.

What I find **brilliant** about this correspondence, is that more complex operations on these matrices will have equivalent quaternion semantics.  

For example,  we can find a partial derivative of a rotation represented by a unit quaternion with respect to its parameters $\{a,b,c,d\}$ by finding the derivative of the matrix representation. While this is computationally convenient, it is considered peculiar abuse of notation by mathematicians.  
   
For the quaternion 

$$q =  a+b\ \mathbf {i} +c\ \mathbf {j} +d\ \mathbf {k}$$

the matrix representation is 

$$ 
\begin{bmatrix}
a & -b & -c & -d  \\
b &  a & -d &  c  \\
c &  d &  a & -b  \\
d & -c & b & a 
\end{bmatrix}
$$

Keep in mind that this matrix representation is not unique (There are 48 distinct matrix representations). That is, there exist other matrix representations of quaternions that conserve quaternion multiplication and addition. For details, see this [Wikipedia section](https://en.wikipedia.org/wiki/Quaternion#Matrix_representations).


## Unit quaternion as 3D rotation representations
Unit quaternions provide a convenient mathematical notation for representing pose and rotations in three dimensions. Unlike Euler angles, they avoid gimbal lock, offer a straightforward method to smoothly interpolate between poses, and are efficient to compute. Additionally, they encode rotation as an axis-angle rotation about an arbitrary axis, which is an intuitive mental model of rotation.

A rotation of $\theta$ radians about a unit axis $\mathbf{x} = (x, y, z)$ is represented by the unit quaternion $(c, sx, sy, sz)$ where $C= \cos(\theta /2)$ and $s=\sin(\theta /2)$. In other words: 


$$ 
\begin{align}
& q = \text{Rotatation representation $\theta$ rads. about $\mathbf{x}$} \\ 
& q = \cos(\theta /2) + \sin(\theta/2)(xi + yj + zk)
\end{align}
$$

#### Performing the rotation with conjugation
The conjugation operation is required to rotate a 3D point by a unit quaternion. Conjugating a 3D point $p$ by unit quaternion $q$ refers to the operation $p \rightarrow qpq^{−1}$. Note that the 3D point $p$ is encoded as a quaternion with a real part equal to zero. 

$$
\begin{align}
\text{p rotated } & = \text{Conjugate point p by unit quaternion q} \\ 
p' & = qpq^{−1}
\end{align}
$$

Proofs are omitted due to their size. For a proof that unit quaternion conjugation is equivalent to Rodrigues' rotation formula, a different axis-angle rotation scheme, see [this post](https://erkaman.github.io/posts/quaternion_rotation.html#mjx-eqn-eqrodri) by Eric Arnebäck. For a more succinct, but weaker proof that unit quaternion conjugation preserves the norm of the rotated point, see [this section](https://www.songho.ca/math/quaternion/quaternion.html#rotation) of Song Ho's blog.

#### Why conjugation?
An intuitive mental model of what the conjugation operation is doing can be found in [this interactive visualization](https://eater.net/quaternions/video/intro). I.e., $\hat{p} = qp$ performs half of the rotation of $p$ while scaling simultaneously scaling $p$'s vector part's norm<sup>*</sup>. Then, $\hat{p}q^{−1}$ completes the second half of the rotation and reverts $p$'s vector part's norm to its original value. 

*- Quaternion norms are multiplicative (see definitions above), but this does not mean the norm of solely the vector parts of $q$ and $p$ is  multiplicative; thus, in general, $qp$ scales $p$'s norm.

#### Matrix form of quaternion rotation
As with the fundamental quaternion operations, conjugation by a unit quaternion has a corresponding matrix form. 

$$
\begin{align}
qpq^{-1} & = 
\begin{bmatrix}
a \\ 
b \\
c \\
d 
\end{bmatrix}
\begin{bmatrix}
0 \\ 
x \\
y \\
z 
\end{bmatrix}
\begin{bmatrix}
a \\ 
-b \\
-c \\
-d 
\end{bmatrix} 
\quad \text{(Quaternion multiplication)}
\\
\\
& = 
\begin{bmatrix}
1 - 2 c^2 - 2 d^2  & 2bc - 2ad         & 2 bd + 2 ac     \\
2 bc + 2 ad        & 1 - 2 b^2 - 2d^2  & 2 cd - 2 ab     \\
2 bd - 2 ac        & 2 cd + 2 ab       & 1 - 2b^2 - 2c^2 
\end{bmatrix} 
\begin{bmatrix}
x \\
y \\
z 
\end{bmatrix} \quad \text{(Matrix multiplication)}
\end{align}
$$



For a proof of this equivalence, see [this article](https://www.songho.ca/opengl/gl_quaternion.html).

#### Converting a rotation matrix to a rotation quaternion
The 'rotation matrix $\rightarrow$ quaternion' mapping is simply the inversion of the 'quaternion$\rightarrow$rotation matrix' formula shown above. This inversion is too lengthy to describe here. 
For a formal proof of the inversion, see [this article](https://intra.ece.ucr.edu/~farrell/AidedNavigation/D_App_Quaternions/Rot2Quat.pdf) by Jay A. Farrell. 
The code for the inversion was taken from [this article](https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf) by Mike Day.


## Unit-test matrix representation of quaternions


```python
import ipyvolume as ipv 
import numpy as np 
import time
np.random.seed(0)

def quaternion_to_matrix(quaternion):
    """ Converts a quaternion to its matrix representation. """
    a, b, c, d = quaternion
    return np.array([[a, -b, -c, -d],
                     [b,  a, -d,  c], 
                     [c,  d,  a, -b], 
                     [d, -c,  b,  a]]) 

def matrix_to_quaternion(matrix):
    """ Converts a matrix to its quaternion representation. """
    return matrix[:, 0]


def multiply(q1, q2):
    """ Multiplies two quaternions using the  Hamilton product. """
    a1, b1, c1, d1 = q1
    a2, b2, c2, d2 = q2
    a = a1*a2 - b1*b2 - c1*c2 - d1*d2
    b = a1*b2 + b1*a2 + c1*d2 - d1*c2
    c = a1*c2 - b1*d2 + c1*a2 + d1*b2
    d = a1*d2 + b1*c2 - c1*b2 + d1*a2
    return np.array([a, b, c, d])


def conjugate(quaternion):
    """ Returns the quaternion conjugate. """
    a, b, c, d = quaternion
    return np.array([a, -b, -c, -d])


def norm(quaternion): 
    """ Returns the quaternion norm. """
    return np.linalg.norm(quaternion)


def inverse(quaternion): 
    """ Returns the quaternion inverse. """
    return conjugate(quaternion) / (norm(quaternion)**2)


def conjugation(quaternion, point):
    """ Conjugates the point by the unit quaternion. 
        'point' is a the 3D point represented by a quaternion
        with real part 0 when using conjugation as a rotation.
    """
    q_inv = inverse(quaternion)
    return multiply(multiply(quaternion, point), q_inv)
    
def axis_angle_to_rot_quaternion(rot_axis, theta):
    """ Returns the quaternion representing rotation for a rotation axis and angle. 
        Theta is in radians. 
    """ 
    rot_axis = rot_axis / np.linalg.norm(rot_axis)
    b, c, d = rot_axis # Interpret axis as quaternion with real part = 0.
    cos = np.cos(theta / 2)
    sin = np.sin(theta / 2)
    return np.array([cos, sin*b, sin*c, sin*d]).squeeze()
    
def quaternion_to_rot_matrix(quaternion):
    """ Converts a unit quaternion to the matrix representing its conjugation. """
    a, b, c, d = quaternion / np.linalg.norm(quaternion)
    rot_matrix = np.array([[1 - 2 * c**2 - 2 * d**2,  2 * b * c - 2 * a * d,    2 * b *d + 2 * a * c   ], 
                           [2 * b * c + 2 * a * d,    1 - 2 * b**2 - 2 * d**2,  2 * c * d - 2 * a * b  ], 
                           [2 * b * d - 2 * a * c,    2 * c * d + 2 * a * b,    1 - 2 * b**2 - 2 * c**2]])
    return rot_matrix 

def rot_matrix_to_quaternion(rot):
    """ Converts a rotation matrix to a quaterion that represents the same rotation.
        Taken from  https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf
    """
    r = rot.T
    if r[2,2] < 0: 
        if r[0, 0] > r[1,1]: 
            t = 1 + r[0, 0] - r[1, 1] - r[2, 2]
            q = [t, r[0, 1] + r[1, 0], r[2, 0] + r[0, 2], r[1, 2] - r[2, 1]]
        else: 
            t = 1 - r[0, 0] + r[1, 1] - r[2, 2]
            q = [r[0, 1] + r[1, 0], t, r[1, 2] + r[2, 1], r[2, 0] - r[0, 2]]
    else:
        if r[0, 0] < -r[1, 1]:
            t = 1 - r[0, 0] - r[1, 1] + r[2, 2]
            q = [r[2, 0] + r[0, 2], r[1, 2] + r[2, 1], t, r[0, 1] - r[1, 0]]
        else:
            t = 1 + r[0, 0] + r[1, 1] + r[2, 2] 
            q = [r[1, 2] - r[2, 1], r[2, 0] - r[0, 2], r[0, 1] - r[1, 0], t]
    q = np.array(q)
    q *= 0.5 / np.sqrt(t) 
    q = -np.roll(q, 1)
    return q                               
```


```python
# Check multiplication 
q1 = np.random.rand(4)
q2 = np.random.rand(4)
q1_m = quaternion_to_matrix(q1)
q2_m = quaternion_to_matrix(q2)
q = multiply(q1, q2)
q_m = matrix_to_quaternion(q1_m @ q2_m)
equal = np.allclose(q, q_m)
print(f'Quaternion product equals matrix multiply: {equal}')

# Check if conjugate equals transpose
q = np.random.rand(4)
matrix = quaternion_to_matrix(q)
q_t = matrix_to_quaternion(matrix.T)
equal = np.allclose(conjugate(q), q_t)
print(f'Quaternion conjugate equals matrix transpose: {equal}')

# Check if inverse equals matrix inverse
q = np.random.rand(4)
q_m = quaternion_to_matrix(q)
q_inv = inverse(q)
q_m_inv = matrix_to_quaternion(np.linalg.inv(q_m))
equal = np.allclose(q_inv, q_m_inv)
print(f'Quaternion inverse equals matrix inverse: {equal}')

# Check if matrix rotation is same as conjugation. Rotate 30 degrees around y axis
point = [1, 0, 0] 
point_q = [0, 1, 0, 0]
rad = np.radians(30)
rot_matrix =  np.array([[np.cos(rad),  0, np.sin(rad)], 
                        [0,            1,           0], 
                        [-np.sin(rad), 0, np.cos(rad)]])
rot_q = axis_angle_to_rot_quaternion([0, 1, 0], rad)
rot_point = rot_matrix @ point
rot_point_q = conjugation(rot_q, point_q)
equal = np.allclose(rot_point, rot_point_q[1:])
print(f'Rotation by quaternion conjugation equals rotation by matrix: {equal}')

q = rot_matrix_to_quaternion(rot_matrix)
rot_matrix_2 = quaternion_to_rot_matrix(q)
equal = np.allclose(rot_matrix, rot_matrix_2) 
print(f'Rotation matrix and quaternion conversions work: {equal}')
```

## Visualize rotation with quaternions
In this cell, we will visualize the rotation of points using unit quaternions.  
Note how unit quaternions are a straightforward way to encode axis-angle rotations.
Here, the blue line is the rotation axis. 
The red line is a random unit vector that will be rotated around the axis.    
The random vector is rotated around the axis by 500 degrees.   
Then, a different rotation axis is chosen.    
This process is repeated 5 times.


```python
def random_line():
    """ Returns a random unit length 3D line segment placed at the origin.
    
        Note that the line consists of 50 sampled points. 
        This is done in order to better visualize the line, 
        as ipyvolume's default lines have almost no diameter.
    """
    direction = np.random.rand(3)
    norm_direction = direction / np.linalg.norm(direction)
    line = np.array([norm_direction * length for length in np.linspace(0.0, 1.0, 50)])
    return line 

def unit_sphere(): 
    """ Returns the point cloud of a unit square placed at the origin. """
    sphere = list()
    for x in np.linspace(-1, 1, 10):
        for y in np.linspace(-1, 1, 10): 
            for z in np.linspace(-1, 1, 10):
                sphere_point = np.array([x, y, z])
                sphere_point = sphere_point / np.linalg.norm(sphere_point)
                sphere.append(sphere_point)
    sphere = np.array(sphere)
    return sphere
```


```python
# Plot the unit sphere, rotation axis, and line to be rotated.
sphere = unit_sphere()
rot_axis = random_line()
line = random_line()

line_xs, line_ys, line_zs = line.T
sphere_xs, sphere_ys, sphere_zs = sphere.T
axis_xs, axis_ys, axis_zs = rot_axis.T 

fig = ipv.pylab.figure(figsize=(15, 15), width=800)
line_plot = ipv.scatter(line_xs, line_ys, line_zs, marker='sphere')
sphere_plot = ipv.scatter(sphere_xs, sphere_ys, sphere_zs, size=0.5, marker='sphere')
rot_axis_plot = ipv.scatter(axis_xs, axis_ys, axis_zs, color='blue', marker='sphere')
ipv.show()
time.sleep(3)
# Visualize rotation in steps of 1 degree
theta = (1 / 360) * (2 * np.pi)

for _ in range(5):
    # Represent an axis-angle rotation with a quaternion
    # Convert to a rot-matrix for efficient broadcasting to all points.
    q_rot = axis_angle_to_rot_quaternion(rot_axis[-1], theta)
    q_rot_matrix = quaternion_to_rot_matrix(q_rot)
    
    for _ in range(500):
        line = (q_rot_matrix @ line.T).T
        xs, ys, zs = line.T
        line_plot.x = xs
        line_plot.y = ys
        line_plot.z = zs
        time.sleep(0.01)

    # After each rotation, choose a new rotation axis
    rot_axis = random_line()
    axis_xs, axis_ys, axis_zs = rot_axis.T
    rot_axis_plot.x = axis_xs
    rot_axis_plot.y = axis_ys
    rot_axis_plot.z = axis_zs
    time.sleep(1)
```

## Solving a PnP problem using a quaternion for camera pose
Next, we will use quaternions to solve a [Perspective-n-Point](https://en.wikipedia.org/wiki/Perspective-n-Point) (PnP) problem. PnP refers to the process of estimating the pose of a calibrated camera given a set of $n$ 3D points in the world and their corresponding 2D projections in an image.   

An intuitive way to think of PnP is as a "camera placement" problem. You have a camera that is back-projecting rays from 2D pixel points. You want to find a camera pose such that these rays intersect with their respective world points. The error between the current and optimal pose is quantified by the reprojection error. The reprojection error measures the in-image distance between the pixel point in the provided image, and the pixel point from the projected 3D world point. The projected 3D world point is calculated using the estimated camera pose and known 3D world point coordinate.

<center>
    
<div align="center">
Image from <a href="https://support.pix4d.com/hc/en-us/articles/202559369-Reprojection-error">Pix4D article</a>
</div>
</center>   

In this section, we will assume to have perfect knowledge of 
* the 3D world point coordinates
* which pixel points correspond to the 3D world points


```python
# Functions defined and used in 1_camera_basics.ipynb notebook.
from notebook_functions import (init_3d_plot, 
                                plot_chessboard, 
                                plot_camera_wireframe, 
                                plot_picture,
                                calibrate_cameras, 
                                extrinsics_from_calibration, 
                                project_points_to_picture,
                                object_points,
                                images,
                                intrinsics,
                                camera_centers, 
                                extrinsics, 
                                random_rotation)
import ipyvolume as ipv
import numpy as np 
import time
```

## Show a calibrated camera and distort its pose.
To perform PnP, we will need an initial estimate of a camera pose.     
Here, we will use a distorted camera extrinsics as an initial estimate.   
The distortion is implemented as a random rotation and translation.     
This also provides a ground truth pose: the undistorted pose can be used to measure the accuracy of the solution.  


```python
def extrinsics_from_quaternion(quaternion, translation):
    """ Return an camera extrinsic matrix for a orientation 
        quaternion and displacement translation vector.
    """
    rotation = quaternion_to_rot_matrix(quaternion)
    extrinsic = np.hstack([rotation.squeeze(), translation[None,].T])
    extrinsic = np.vstack([extrinsic, [[0,0,0,1]]])
    return extrinsic

def random_angle(max_angle): 
    """ Returns a random angle in radians """
    rad = np.radians(max_angle)
    rand_factor = np.random.uniform(low=-1, high=1)
    return rad * rand_factor
                                  
def random_rotation(max_angle=100): 
    """ Returns a matrix for random rotation around x, y, and z axis. """
    t_x = random_angle(max_angle)
    t_y = random_angle(max_angle) 
    t_z = random_angle(max_angle)
                                  
    r_x = np.array([[1,           0,            0], 
                    [0, np.cos(t_x), -np.sin(t_x)], 
                    [0, np.sin(t_x),  np.cos(t_x)]])
                                  
    r_y = np.array([[np.cos(t_y),  0, np.sin(t_y)], 
                    [0,            1,         0], 
                    [-np.sin(t_y), 0, np.cos(t_y)]])
                                  
    r_z = np.array([[np.cos(t_z), -np.sin(t_z), 0], 
                    [np.sin(t_z),  np.cos(t_z), 0], 
                    [0,            0,           1]])
    
    return r_x @ r_y @ r_z 

def random_translation(max_offset=10): 
    """ Returns a random translation vector. """
    return np.random.uniform(low=-max_offset, high=max_offset, size=3)

def distort_extrinsics(extrinsic): 
    """ Randomly distorts an extrinsic matrix such that 
        the pose it represents is rotated and moved.  
    """
    extrinsic = extrinsic.copy()
    rot = extrinsic[:3, :3]
    trans = extrinsic[3, :3]
    rand_rot = random_rotation()
    rand_trans = random_translation()
    extrinsic[:3, :3] = rand_rot @ extrinsic[:3, :3]
    extrinsic[:3, 3] = extrinsic[:3, 3] + rand_trans 
    return extrinsic

def distort_quaternion(q):
    """ Distorts a pose quaternion by randomly rotating it, """
    rot_q = random_rotation_quaternion()
    q = conjugation(rot_q, q)
    return q
```


```python
vis_scale = 5 
init_3d_plot()
plot_chessboard(object_points)
ipv.show()

inv_intrinsics = np.linalg.inv(intrinsics)
first_cam = extrinsics[0]
# first_cam_est = distort_extrinsics(first_cam) # Uncomment for random distortion. Next line is distortion show in videos.
first_cam_est = np.array([[-.2438,.3825,.8911,6.6443],[.9257,.3655,.0963,-3.8001],
                          [-0.2889,  0.8485, -0.4432, 35.4257],[0,0,0,1]])
quaternion_true = rot_matrix_to_quaternion(first_cam[:3, :3])
quaternion_est = rot_matrix_to_quaternion(first_cam_est[:3, :3])
translation_true = first_cam[:3, 3]
translation_est = first_cam_est[:3, 3]

for idx, extrinsic in enumerate([first_cam, first_cam_est]): 
    color = 'red' if idx else 'blue'
    inv_extrinsic = np.linalg.pinv(extrinsic)
    proj_matrix = intrinsics @ extrinsic
    cam_center = -extrinsic[:3, :3].T @ extrinsic[:3, 3]
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic, color)    
    # back-project pixel points into rays
    if idx == 0: 
        image = project_points_to_picture(images[0], object_points, intrinsics, extrinsic)
        chess_corner_proj = proj_matrix @ object_points
        chess_corner_proj /= chess_corner_proj[2]
        chess_corner_proj[3] = 1 
    plot_picture(image, inv_extrinsic, vis_scale)
    dir_vecs_in_cam_ref = inv_intrinsics @ chess_corner_proj
    dir_vecs_in_cam_ref[:3] *= 35
    dir_vecs_in_world = inv_extrinsic @ dir_vecs_in_cam_ref
    for vec in dir_vecs_in_world.T:
            ipv.plot([cam_center[0], vec[0]], [cam_center[1], vec[1]], [cam_center[2], vec[2]], color)
```

## Levenberg-Marquardt algorithm to solve PnP and realign camera
The PnP problem is solved by minimizing the reprojection error. The [Levenberg-Marquardt](https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm) optimization algorithm is well suited for this. It iteratively minimizes errors by combining a [gradient descent](https://en.wikipedia.org/wiki/Gradient_descent) solution and a [least-squares](https://en.wikipedia.org/wiki/Least_squares) solution. In other words, it finds the derivative of the squared reprojection error w.r.t the camera pose, and combines two solutions: 
* Gradient descent - Update the camera in the parameter direction that **reduces** the error.    
* Least-squares - Update the camera to the parameter location that **minimizes** the error.      

These two solutions model the reprojection error in different ways.    

* Gradient descent - Assumes the squared error w.r.t parameters lie on a manifold. A manifold is a mathematical space that is not necessarily Euclidean on a global scale, but can be seen as Euclidean on a local scale. That is, gradients are valid indicators of directions that decrease errors at a local scale.
* Least-squares - Assumes the squared error w.r.t parameters is modeled by a convex quadratic polynomial. Like in high school maths, this function is minimized by setting the derivative to zero.

This makes gradient descent slow but stable and least-squares faster but more erratic. Least-squares makes more assumptions that can be false. The dampening factor $\lambda$ is the parameter that controls the weight of each solution. When $\lambda=0$, only the least-squares solution is used, and it is essentially [Gauss-Newton](https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm) optimization. For greater values of $\lambda=0$, the gradient descent determines a larger portion of the update. The code implements the equations in the [Wikipedia article](https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm).  


```python
def project_points(extrinsics, object_points):
    """ Projects 3D points to an image using a projection matrix. """
    proj_matrix = intrinsics @ extrinsics
    image_points = proj_matrix @ object_points
    image_points = image_points / image_points[2]
    return image_points

def reprojection_error(projection, target):
    """ Calculates the mean squared reprojection error. 
    
    Args: 
        projection (np.ndarray): Where the known 3D points are 
            currently projected in the image.
        target (np.ndarray): Where the known 3D points should
            be projected to.
    returns: 
        error (float): The mean squared error.
    """ 
    pixel_dist = projection[:2] - target[:2]
    error = np.sqrt(np.sum(pixel_dist ** 2, axis=0))
    return error    
    
def jacobian(quaternion, translation, object_points, target_proj): 
    """ Returns the jacobian w.r.t the extrinsic parameters encoded by 
        a unit quaternion and translation vector. The derivatives are numerical. 

    Args:
        quaternion (np.ndarray): A camera orientation unit quaternion. 
        translation (np.ndarray): A camera displacement translation vector. 
        object_points (np.ndarray): Known 3D object points being projected the camera. 
    Returns: 
        jacobian_array: The derivatives of the squared reprojection error 
            w.r.t the pose quaternion and the translation vector.
    """
    eps = 0.0000001
    orig_ext = extrinsics_from_quaternion(quaternion, translation)
    orig_error = reprojection_error(project_points(orig_ext, object_points)[:2], target_proj[:2])
    
    # Get changed extrinsic matrices
    da_ext = extrinsics_from_quaternion(quaternion + [eps, 0, 0, 0], translation)
    db_ext = extrinsics_from_quaternion(quaternion + [0, eps, 0, 0], translation)
    dc_ext = extrinsics_from_quaternion(quaternion + [0, 0, eps, 0], translation)
    dd_ext = extrinsics_from_quaternion(quaternion + [0, 0, 0, eps], translation)
    dx_ext = extrinsics_from_quaternion(quaternion, translation + [eps, 0, 0])
    dy_ext = extrinsics_from_quaternion(quaternion, translation + [0, eps, 0])
    dz_ext = extrinsics_from_quaternion(quaternion, translation + [0, 0, eps])
    
    # Get numerical derivatives
    da = (reprojection_error(project_points(da_ext, object_points)[:2], target_proj[:2]) - orig_error) / eps
    db = (reprojection_error(project_points(db_ext, object_points)[:2], target_proj[:2]) - orig_error) / eps
    dc = (reprojection_error(project_points(dc_ext, object_points)[:2], target_proj[:2]) - orig_error) / eps
    dd = (reprojection_error(project_points(dd_ext, object_points)[:2], target_proj[:2]) - orig_error) / eps
    dx = (reprojection_error(project_points(dx_ext, object_points)[:2], target_proj[:2]) - orig_error) / eps
    dy = (reprojection_error(project_points(dy_ext, object_points)[:2], target_proj[:2]) - orig_error) / eps
    dz = (reprojection_error(project_points(dz_ext, object_points)[:2], target_proj[:2]) - orig_error) / eps
    
    jacobian_array = np.array([da, db, dc, dd, dx, dy, dz]).T    
    return jacobian_array

def levenberg_marquardt_step(quaternion, translation, object_points, target_proj, dampening_factor):
    """ Takes a step in the gradient descent and least-squares direction. 
    
    Args: 
        quaternion (np.ndarray): A camera orientation unit quaternion. 
        translation (np.ndarray): A camera displacement translation vector.
        object_points (np.ndarray): Known 3D object points being projected the camera. 
        target_proj (np.ndarray): Where the known 3D points should be projected to.
        dampening_factor (float): Paramater to balance gradient descent and least squares solution.
    Return: 
        quaternion (np.ndarray): The updated camera orientation unit quaternion.
        translation (np.ndarray): The updated camera displacement translation vector.
        corrections_norm (float): The length of the correction correction vector. 
            Can be used to check for convergence.
    """
    extrinsic = extrinsics_from_quaternion(quaternion, translation)
    j = jacobian(quaternion, translation, object_points, target_proj)
    r = reprojection_error(project_points(extrinsic, object_points)[:2], target_proj[:2])
    jtj = j.T @ j
    ident = np.identity(jtj.shape[0])
    inv = np.linalg.pinv(jtj + ident * dampening_factor)
    inv = inv @ j.T
    learning_rate = 0.1
    corrections = -inv @ r * learning_rate
    corrections_norm = np.linalg.norm(corrections)
    quaternion  = quaternion + corrections[:4]
    quaternion = quaternion / np.linalg.norm(quaternion)
    translation = translation + corrections[4:]
    return quaternion, translation, corrections_norm
```


```python
ipv.show()
time.sleep(3)
iterations = 50

for method in ['levenberg', 'gauss']: 
    quaternion = quaternion_est.copy()
    translation = translation_est.copy() 
    dampening_factor = 0 if method == 'gauss' else 8
    color = 'orange' if method == 'gauss' else 'purple'

    for i in range(iterations): 
        quaternion, translation, corrections_norm = levenberg_marquardt_step(quaternion, 
                                                                            translation, 
                                                                            object_points, 
                                                                            chess_corner_proj,
                                                                            dampening_factor)
        extrinsic = extrinsics_from_quaternion(quaternion, translation)
        cam_center = -extrinsic[:3, :3].T @ extrinsic[:3, 3]
        inv_extrinsics = np.linalg.pinv(extrinsic)
        plot_camera_wireframe(cam_center, vis_scale, inv_extrinsics, color)
        time.sleep(0.5)
```

## Conclusion
Pat yourself on the back! In this notebook we just:
* went through the maths behind quaternions
* implemented quaternions
* used quaternions to solve a non-trivial pose optimization problem
* implemented the popular Levenberg Marquardt algorithm  

Quaternions will often be abstracted away in software libraries.    
Nevertheless, it is good to have an intuition of how they work.     
In notebook 4, we will use the g2o library to solve an optimization problem called bundle adjustment.    
If one digs into [g2o's source code](https://github.com/RainerKuemmerle/g2o), you will find that the poses and rotations are all represented by quaternions!    
Next up: a notebook on epipolar geometry and its many uses.

## Sources: 
1. [Wikipedia article](https://en.wikipedia.org/wiki/Quaternion)
2. [Visualization by Ben Eater and Grant Sanderson](https://eater.net/quaternions)
3. [Blog by Song Ho](http://www.songho.ca/math/quaternion/quaternion.html)
4. [Stanford tutorial by Yan-Bin Jia](https://graphics.stanford.edu/courses/cs348a-17-winter/Papers/quaternion.pdf)
5. [Proof by Eric Arnebäck](https://erkaman.github.io/posts/quaternion_rotation.html#mjx-eqn-eqrodri)
6. [Proof by Jay A. Farrell](https://intra.ece.ucr.edu/~farrell/AidedNavigation/D_App_Quaternions/Rot2Quat.pdf) 
7. [Code by Mike Day](https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf) 

1b88e94d587dd8513c744eb5a97b96836bb9dbe9
ipynb
Jupyter Notebook
2_quarternions.ipynb
maxcrous/multiview_notebooks
bea2f87b8c78c5819337a496a0d330c255b492d1
MIT
2021-12-05T16:12:01.000Z
2022-03-28T12:18:23.000Z
2_quarternions.ipynb
maxcrous/multiview_notebooks
bea2f87b8c78c5819337a496a0d330c255b492d1
MIT
2_quarternions.ipynb
maxcrous/multiview_notebooks
bea2f87b8c78c5819337a496a0d330c255b492d1
MIT
2021-12-05T18:48:06.000Z
2022-03-26T02:19:43.000Z
Qwen/Qwen-72B
1. YES
2. YES