Find the optimized rotation

Question

I have an application where I must find a rotation from a set of 15 orderer&indexed 3D points (X1, X2, ..., X15) to another set of 15 points with the same index (1 initial point corresponding to 1 final point).

I've read manythings about finding the rotation with Euler angles (evil for some persons), quaternions or with projecting the vector on the basis axis. But i've an additionnal constraint : a few points of my final set can be wrong (i.e. have wrong coordinates) so I want to discriminate the points that ask a rotation very far from the median rotation.

My issue is : for every set of 3 points (not aligned ones) and their images I can compute quaternions (according to the fact that the transformation matrix won't be a pure rotation I have some additionnal calculations but it can be done). So I get a set of quaternions (455 ones max) and I want to remove the wrong ones.

Is there a way to find what points give rotations far from the mean rotation ? Does the "mean" and "the standard deviation" mean something for quaternions or must I compute Euler angles ? And once I've the set of "good" quaternions, how can I compute the "mean" quaternion/rotation ?

Cheers,

Ricola3D

Answer 1

There are 2 problems here:

• how do you compute a "best fit" for an arbitrary number of points?
• how do decide which points to accept, and which points to reject?

---

The general answer to the first is, "do a least squares fit". Quaternions would probably be better than Euler angles for this; try the following:

foreach point pair (a -> b), ideal rotation by unit quaternion q is:
   b = q a q*   ->   q a - b q = 0

So, look for a least-squares fit for q:

minimize sum[over i] of |q a_i - b_i q|^2
under the constraint:  |q|^2 = 1

As presented above, the least-squares problem is linear except for the constraint, which should make it easier to solve than an Euler angle formulation.

---

For the second problem, I can see two approaches:

• if your points aren't too far off, you could try running the least-squares solver with all points, then go back, throw out the "outliers" (those point pairs whose squared-error is greatest), and try again.
• if wildly inconsistent points are throwing off the above procedure, you could try selecting random, small subsets of 3 or 4 pairs, and find a least-squares fit for each. If a large group of these results have similar rotations with low total error, you can use this to identify "good" pairs (and thereby eliminate bad pairs); then go back and find a least-squares fit for all good pairs.

Answer 2

In computer vision, there's a technique called RANSAC for doing something like what you propose. Instead of finding all of the possible quaternions, you would use a minimal set of point correspondences to find a single quaternion/transformation matrix. You'd then evaluate all of the points for quality of fit, discarding those that don't fit well enough. If you don't have enough good matches, perhaps you got a bad match in your original set. So you'll throw away that attempt and try again. If you do get enough good matches, you'll do a least-squares regression fit of all the inlier points to get a new transformation matrix and then iterate until you're happy with the results.

Alternatively, you could take all of your normalized quaternions and find the dot-product between them all. The dot-product should always be positive; if it's not for any given calculation, you should negate all of the components of one of the two quaternions and re-compute. You then have a distance measure between the quaternions and you can cluster or look for gaps.