Average transformation matrix for a list of transformations

HugoRune picture HugoRune · Jan 20, 2014 · Viewed 7.5k times · Source

I have multiple estimates for a transformation matrix, from mapping two point clouds to each other via ICP (Iterative Closest Point).

How can I generate the average transformation matrix for all these matrices?

Each matrix consists of a rigid translation and a rotation only, no scale or skew.

Ideally I would also like to calculate a weighted average, but an unweighted one is fine for now.

Averaging the translation vectors is of course trivial, but the rotations are problematic. One approach I found is averaging the individual base vectors for the rotations, but I am not sure that will result in a new orthonormal base, and the approach seems a little ad-hoc.

Answer

Nico Schertler picture Nico Schertler · Jan 20, 2014

Splitting the transformation in translation and rotation is a good start. Averaging the translation is trivial.

Averaging the rotation is not that easy. Most approaches will use quaternions. So you need to transform the rotation matrix to a quaternion.

The easiest way to approximate the average is a linear blending, followed by renormalization of the quaternion:

q* = w1 * q1 + w2 * q2 + ... + w2 * qn
normalize q*

However, this is only an approximation. The reason for that is that the combination of two rotations is not performed by adding the quaternions, but by multiplying them. If we convert quaternions to a logarithmic space, we can use a simple linear blend (because multiplication will become additions). Then transform the quaternion back to the original space. This is the idea of the Spherical Average (Buss 2001). If you're lucky, you find a library that supports log and exp of quaternions:

start with q* as above
do until convergence
    for each input quaternion i (index)
        diff = q[i] * inverse(q*)
        u[i] = log(diff, base q*)
    //Now perform the linear blend
    adapt := zero quaternion
    weights := 0
    for each input quaternion i
        adapt += weight[i] * u[i]
        weights += weight[i]
    adapt *= 1/weights
    adaptInOriginalSpace = q* ^ adapt    (^ is the power operator)
    q* = adaptInOriginalSpace * q*

You can define a threshold for adaptInOriginalSpace. If it is a very very small rotation, you can break the loop. This algorithm is proven to preserve geodesic distances on a sphere.