Estimate Rotation and Translation from two sets of points in different coordinate systems

Question

I got one set of 3d $(x,y,z)$ points (number of points $\geq3$) located in two different coordinate systems. Is it possible to estimate the rotation and translation between these systems?

Something like $$ \begin{pmatrix} x_i' \\ y_i' \\ z_i' \\ \end{pmatrix} = \begin{pmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{pmatrix} \begin{pmatrix} x_i \\ y_i \\ z_i \\ \end{pmatrix} + \begin{pmatrix} t_x \\ t_y \\ t_z \\ \end{pmatrix} $$

$$i = 0,1,...,n$$

Answer 1

You have 12 variables, $r_{ij}$s and $t_i$s. Each point gives 3 equations. So we need 4 points for unique solution to the matrices. Anything less, and you can have many solutions.

If you have more points than required you can try to use some estimation method like LSE.

Answer 2

Collect the "set of points" into vectors $$ \def\qiq{\quad\implies\quad} x_k=\pmatrix{x_k\\y_k\\z_k},\qquad x'_k=\pmatrix{x'_k\\y'_k\\z'_k} $$ Calculate the centroid of each group of vectors $$c=\frac1n\sum_{k=1}^nx_k,\qquad c'=\frac1n\sum_{k=1}^nx'_k$$ Assuming that the rotation is with respect to the centroid $c,\,$ we have $$c=Rc$$ which allows us to immediately solve for the translation vector $$c' = (Rc+t) = (c+t) \qiq t = (c'-c)$$ For the next part, concatenate the vectors into matrices $$\eqalign{ \def\m#1{\left[\begin{array}{r|r}#1\end{array}\right]} X\:&=\m{x_1&x_2&\cdots} \\ X'&=\m{x'_1&x'_2&\cdots} \\ T\:&=\m{\;t\;\,&\;t\;&\cdots} \\ }$$ and write the whole problem in matrix form $$\eqalign{ RX &= (X'-T) \;\doteq\; Y \\ }$$ This system is overdetermined, so we must solve it in a least-squares sense $$\eqalign{ \min_R\:&\left\|RX-Y\right\|^2_F \\ \operatorname{s.t.}\:&R^TR=I \\ }$$ This is the well-known Orthogonal Procrustes Problem whose solution utilizes the singular value decomposition of $YX^T$ $$YX^T = USV^T \qiq R = UV^T$$

Answer 3

Wikipedia calls it Kabsch algorithm, based on:

KABSCH, Wolfgang. A solution for the best rotation to relate two sets of vectors. Acta Crystallographica Section A: Crystal Physics, Diffraction, Theoretical and General Crystallography, 1976, 32.5: 922-923.

KABSCH, Wolfgang. A discussion of the solution for the best rotation to relate two sets of vectors. Acta Crystallographica Section A: Crystal Physics, Diffraction, Theoretical and General Crystallography, 1978, 34.5: 827-828.

A recent exaplanation in Least-Squares Rigid Motion Using SVD by Olga Sorkine-Hornung and Michael Rabinovich.

Another reference is: ARUN, K. Somani; HUANG, Thomas S.; BLOSTEIN, Steven D. Least-squares fitting of two 3-D point sets. IEEE Transactions on pattern analysis and machine intelligence, 1987, 5: 698-700.

If you do not know the correspondences between the points then you can apply a method called Iterative Closest Point (ICP):

BESL, P. J.; MCKAY, Neil D. A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14.2: 239-256.

CHEN, Yang; MEDIONI, Gérard. Object modelling by registration of multiple range images. Image and vision computing, 1992, 10.3: 145-155.

Answer 4

You're missing one important point in the problem statement. A coordinate translate preserves all point distances, and that implies that the "shape" of a point-cloud is preserved. The same applies to coordinate rotation. And, those two facts imply that shape is preserved under the combined T * R transform.

The implication is that there are stringent conditions on the dataset used as the input to the problem. For the input points, a minimum of 3 are needed, and they must form a non-degenerate triangle. But the greater constraint is on the "output" points...they must conform to the same rigid object shape (i.e. the same size and shape triangle). If the input and output point clouds do not agree exactly in shape, there is no solution transform of the form T * R.