Fitting object poses in 3D space

Question

Using stereoscopic cameras, I track a certain object through it's path in space. For every frame, I compute it's pose in 3D. I can represent it's pose by either a translation vector + rotation matrix or a 6 element parameter vector (X, Y, Z, roll, pitch, yaw).

The problem is that the output generated by the cameras is noisy, and I'd like to filter some of that noise off by smoothing the poses in 3D.

Any pointers on that? What fitting works better for multidimensional problems?

Some more information: in my case, the XYZ path can probably be fitted with a 2nd order curve, and roll, pitch and yaw also demonstrate smooth change over samples.

Answer 1

Of the two representations you're proposing, the translation vector plus rotation matrix is clearly the most elegant. The translation vector is easy to smooth: all of its degrees of freedom make sense to average. So the problem is in smoothing the rotation matrix.

To smooth the rotation matrix, first note that the rotation matrices are the special orthogonal 3x3 matrices $\mathbf{SO}(3)$, so we can write the rotation matrix as the exponential of an $\mathbf{so}(3)$ matrix such as:
$$\exp\begin{pmatrix}0&\theta_{12}&-\theta_{31}\\ -\theta_{12}&0&\theta_{23}\\ \theta_{31}&-\theta_{23}&0\end{pmatrix}$$
These antisymmetric matrices have just the degrees of freedom you need. This suggests that you should write the rotation matrices in $\mathbf{so}(3)$ form, and then average the $\theta_{jk}$. The (solvable) problem with doing this is that the mapping from $\mathbf{so}(3)$ to $\mathbf{SO}(3)$ by exponentiation is not 1-1.

We can split the information about a rotation into two pieces, "how much to rotate by", and "what direction to rotate around". The "how much to rotate" is given by the length of the $(\theta_{23},\theta_{31},\theta_{12})$ vector:
$$\theta = \sqrt{\theta_{23}^2 + \theta_{31}^2 + \theta_{12}^2}.$$

The axis of the rotation is given by the unit vector in the direction of the $(\theta_{23},\theta_{31},\theta_{12})$ vector:
$$(\theta_{23},\theta_{31},\theta_{12})/\theta.$$ The above is not defined when $\theta=0$.

Written this way, the problem with multiple solutions to the $(\theta_{23},\theta_{31},\theta_{12})$ values becomes clear. For any integer $n$, we can multiply $(\theta_{23},\theta_{31},\theta_{12})$ by $1+2n\pi/\theta$ without changing the rotation. Accordingly, in your smoothing algorithm, choose $n$ in such a way as to minimize changes to $(\theta_{23},\theta_{31},\theta_{12})$. This will keep your rotation vectors compatible for smoothing. Of course the method fails when $\theta$ is near zero, but that is the condition of no rotation so does not pose a problem (special case that condition).

You should get reasonable rotation smoothing.