Closed-form Solution of Absolute Orientation Using Unit Quaternions

Finding the relationship between two coordinate systems using pairs of measurements of the coordinates of a number of points in both systems is a classic photogrammetric task. It finds applications in stereophotogrammetry and in robotics. I present here a closed-form solution to the least-squares problem for three or more points. Currently various mpirical, graphical, and numerical iterative methods are in use. Derivation of the solution is simplified by use of unit quaternions to represent rotation. I emphasize a symmetry property that a solution to this problem ought to possess. The best translational offset is the difference between the centroid of the coordinates in one system and the rotated and scaled centroid of the coordinates in the other system. The best scale is equal to the ratio of the root-mean-square deviations of the coordinates in the two systems from their respective centroids. These exact results are to be preferred to approximate methods based on measurements of a few selected points. The unit quaternion representing the best rotation is the eigenvector associated with the most positive eigenvalue of a symmetric 4 × 4 matrix. The elements of this matrix are combinations of sums of products of corresponding coordinates of the points.

1. Rotation matrix from 3 points

Xl = Pl2 - Pl1
Xl = Xl / ||Xl||
Yl = (Pl3 - Pl1) - [(Pl3 - Pl1) • Xl]  Xl
Yl = Yl / ||Yl||
Zl = Xl × Yl
Ml = |Xl Yl Zl|
Mr = |Xr Yr Zr|
Ml^T  Pl
Pr = Mr  Ml^T  Pl
R = Mr Ml^T

Appendix