How to apply smoothing on a sequence of 3D rotations

Question

I wanna apply smoothing on a sequence of noisy 3D rotations. Is there a straightforward way to achieve this? The rotation is represented by quaternions at the moment, but I can convert them into other forms e.g. rotation vector or rotation matrix. Thanks in advance!

Answer 1

Yes, you can "smooth" quaternions in a way very similar to filtering a discrete signal.

Let $\mathbf{q}_i$ represent the orientation quaternion $i$, and $w_i$ be the relative weight (a scalar, similar to a Finite Impulse Response filter, or any windowed filtering function), with $i = 1 \dots N$. Then, $$\begin{aligned} \mathbf{q}_\text{sum} & = \sum_{i=1}^{N} w_i \mathbf{q}_i \\ \mathbf{q}_\text{filtered} & = \frac{\mathbf{q}_\text{sum}}{\left\lVert \mathbf{q}_\text{sum} \right\rVert} \\ \end{aligned}$$ Multiplying a quaternion $\mathbf{q} = (q_r, q_i, q_j, q_k)$ by a scalar $w$ is done component-wise, yielding quaternion $\mathbf{q}^\prime = (q^\prime_r, q^\prime_i, q^\prime_j, q^\prime_k)$: $$\mathbf{q} w = \mathbf{q}^\prime \quad \iff \quad \begin{cases} q^\prime_r = w q_r \\ q^\prime_i = w q_i \\ q^\prime_j = w q_j \\ q^\prime_k = w q_k \\ \end{cases}$$ Similarly for division. The norm $\lVert\mathbf{q}\rVert$ of quaternion $\mathbf{q} = (q_r, q_i, q_j, q_k)$ is $$\left\lVert \mathbf{q} \right\rVert = \sqrt{ q_r^2 + q_i^2 + q_j^2 + q_k^2 }$$

The reason this kind of weighted mixing of unit quaternions (also known as versors) works, as long as one normalizes the result afterwards to unit norm, is that unit quaternions describing rotations or orientations do not have any bias towards any direction; and because the components of the quaternion can be described via a 3D (unit) axis vector $(a_x, a_y, a_z)$ and the angle of rotation $\theta$ around that axis: $$\left\lbrace ~ \begin{aligned} q_r &= \cos\left(\frac{\theta}{2}\right) \\ q_i &= a_x \sin\left(\frac{\theta}{2}\right) \\ q_j &= a_y \sin\left(\frac{\theta}{2}\right) \\ q_k &= a_z \sin\left(\frac{\theta}{2}\right) \\ \end{aligned} \right.$$ Obviously, when the axis is an unit vector, the quaternion is an unit quaternion too.

If you consider what happens to such a representation when you sum unit quaternions with positive weights $w_i$ such that $\sum w_i = 1$, you'll see that the result is an intuitive mix of the summed unit quaternions.

In the case of just two unit quaternions, i.e. $$\mathbf{q} = (1 - \alpha) \mathbf{q}_0 + \alpha \mathbf{q}_1, \quad 0 \le \alpha \le 1$$ causes a 3D vector $\vec{v}$ rotated by the resulting quaternion ($\vec{v}^\prime = \mathbf{q} \vec{v} \mathbf{q}^{-1}$) to trace a great circle around the origin from $\alpha = 0$, $\vec{v}^\prime = \mathbf{q}_0 \vec{v} \mathbf{q}_0^{-1}$ to $alpha = 1$, $\vec{v}^\prime = \mathbf{q}_1 \vec{v} \mathbf{q}_1^{-1}$. If the two axis vectors are in the same half-space (their dot product nonnegative), then the arc traced is the shorter arc; otherwise the longer arc.

Note that if $$\left\lbrace ~ \begin{aligned} q^\prime_r &= -q_r \\ q^\prime_i &= -q_i \\ q^\prime_j &= -q_j \\ q^\prime_k &= -q_k \\ \end{aligned} \right.$$ the two quaternions $\mathbf{q}$ and $\mathbf{q}^\prime$ describe the same rotation or orientation. For a single rotation, or a change in orientation, if the $r$ component is negative, the rotation is through the "long way around", i.e. vectors trace the longer arc of the great circle through the rotation; negating the entire unit quaternion as above (so that $r$ component is positive) keeps the end orientation the same, but through the shorter arc of a great circle. This is important when mixing/filtering quaternions! You'll need to ensure their $r$ components all have the same sign, or you'll get unintuitive/odd results!

This is extensively used in 3D animation to rotate things from one orientation to another (using just two quaternions) in an intuitive manner; especially the camera itself. For a sequence of camera positions and orientations, piecewise continuous sequences of curves (often cubic Bézier curves) are used.

Since the mixing above is linear, it can feel "jerky" at the beginning and ending -- hopefully you can compare this to "noisy" orientations! -- so instead of a linear function of time $t$ ($\alpha = (t - t_0) / (t_1 - t_0)$), a curve is used; for example, given $\tau = (t - t_0) / (t_1 - t_0)$, i.e. $\tau = 0 \dots 1$, $$\alpha = tau^2 ( 3 - 2 \tau )$$ or $$\alpha = tau^3 ( 10 - 15 \tau + 6 \tau^2 )$$ Both start and stop at standstill (accelerating and decelerating between start and stop orientations), but the latter also starts and stops with zero acceleration; much like elevators where you barely sense the acceleration/deceleration.