Find scale, rotation, and translation that maps three points to three points in 3D

Question

Given three source points $\mathbf{P}_1 = [x_1\ y_1\ z_1]^T,$ $\mathbf{P}_2 = [x_2\ y_2\ z_2]^T,$ $\mathbf{P}_3 = [x_3\ y_3\ z_3]^T,$ and
three destination points $\mathbf{P}'_1 = [x'_1\ y'_1\ z'_1]^T,$ $\mathbf{P}'_2 = [x'_2\ y'_2\ z'_2]^T,$ $\mathbf{P'}_3 = [x'_3\ y'_3\ z'_3]^T,$ how do we find the scale $S,$ rotation $R,$ and translation $T$ that transforms the source points into the destination points as in the following equation? $$ \left[\begin{array}{ccc} s_x & & \\ & s_y & \\ & & s_z \\ \end{array}\right] \left[\begin{array}{cccc} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \end{array}\right] \left[\begin{array}{ccc} x_1 & x_2 & x_3 \\ y_1 & y_2 & y_3 \\ z_1 & z_2 & z_3 \\ 1 & 1 & 1 \end{array}\right] = \left[\begin{array}{ccc} x'_1 & x'_2 & x'_3 \\ y'_1 & y'_2 & y'_3 \\ z'_1 & z'_2 & z'_3 \end{array}\right] $$ Since the $3 \times 3$ rotation $R$ is orthonormal and can be parameterized by three rotation angles (e.g. yaw, pitch, roll) there are 9 equations and 9 unknowns. Ideally the scale would be uniform $(s_x = s_y = s_z)$ resulting in a similarity transformation, but then the system would be over constrained. As long as the source and destination points are not co-linear, this should have a unique solution.

One could solve for a general affine transformation $A$ and decompose this, but this would require 4 non-coplanar source and destination points and may include shears.

EDIT: A possible approach is to solve for the general affine case $X A = B$ $$ \left[\begin{array}{cccc} x_1 & y_1 & z_1 & 1\\ x_2 & y_2 & z_2 & 1\\ x_3 & y_3 & z_3 & 1 \end{array}\right] \left[\begin{array}{ccc} a_{11} & a_{21} & a_{31} \\ a_{12} & a_{22} & a_{32} \\ a_{13} & a_{23} & a_{33} \\ a_{14} & a_{24} & a_{34} \\ \end{array}\right] = \left[\begin{array}{ccc} x'_1 & y'_1 & z'_1 \\ x'_2 & y'_2 & z'_2 \\ x'_3 & y'_3 & z'_3 \end{array}\right] $$ which is under-constrained since there are 9 equations and 12 unknowns which means there are infinite solutions. Therefore, following the recipe here, we will find the solution $A'$ that has the minimum norm $\|A\|.$ The normal equation for an underdetermined system is $$ A^* = X^T (X X^T)^{-1} B $$ from which we can get the optimal value via SVD. The SVD for $X$ is $$ \begin{eqnarray} X &=& U \Sigma V^T \\ &=& \left[\begin{array}{ccc} \cdot & \cdot & \cdot \\ \cdot & \cdot & \cdot \\ \cdot & \cdot & \cdot \end{array}\right] \left[\begin{array}{cccc} \sigma_1 & & & \cdot \\ & \sigma_2 & & \cdot \\ & & \sigma_3 & \cdot \end{array}\right] \left[\begin{array}{cccc} \cdot & \cdot & \cdot & \cdot \\ \cdot & \cdot & \cdot & \cdot\\ \cdot & \cdot & \cdot & \cdot\\ \cdot & \cdot & \cdot & \cdot \end{array}\right]^T \end{eqnarray} $$ where $\sigma_1 \geq \sigma_2 \geq \sigma_3 \geq 0$ are the singular values (if $\sigma_3 = 0$ then the source points are co-linear, i.e., $X$'s rank is less than 3) and $U$ and $V$ are orthonormal matrices. Plugging this into our normal equation we have $$ \begin{eqnarray} A^* &=& V \Sigma^T (\Sigma \Sigma^T )^{-1} U^T B \\ &=& V \left[\begin{array}{ccc} \frac{1}{\sigma_1} & & \\ & \frac{1}{\sigma_2} & & \\ & & \frac{1}{\sigma_3} \\ \cdot & \cdot & \cdot \end{array}\right] U^T B \end{eqnarray} $$ where the matrix above containing the reciprocals of the singular values is the pseudo-inverse of $\Sigma.$ The hope is the solution $A^*$ which the minimal norm from the set of all solutions will be a similar transformation. Yet to be verified.

Answer 1

The assumption is that this can be accomplished with a similarity transformation. We use the following sequence of transformations to compose the desired transformation:

1. Translate $\mathbf{P}_0$ to the origin;
2. Orient $\triangle \mathbf{P}_0 \mathbf{P}_1 \mathbf{P}_2$ to align with $\triangle \mathbf{P}'_0 \mathbf{P}'_1 \mathbf{P}'_2$ via rotation $R;$
3. Uniform scale by $s = \frac{\|\mathbf{P}'_1 - \mathbf{P}'_0\|}{\|\mathbf{P}_1 - \mathbf{P}_0\|}$;
4. Translate to $\mathbf{P'}_0$

$$ \begin{eqnarray} S &=& \left[\begin{array}{cccc} 1 & 0 & 0 & x_0' \\ 0 & 1 & 0 & y_0' \\ 0 & 0 & 1 & z_0' \\ 0 & 0 & 0 & 1 \end{array}\right] \left[\begin{array}{cccc} s & 0 & 0 & 0 \\ 0 & s & 0 & 0 \\ 0 & 0 & s & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \left[\begin{array}{cccc} r_{11} & r_{12} & r_{13} & 0 \\ r_{21} & r_{32} & r_{23} & 0 \\ r_{31} & r_{32} & r_{33} & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \left[\begin{array}{cccc} 1 & 0 & 0 & -x_0 \\ 0 & 1 & 0 & -y_0 \\ 0 & 0 & 1 & -z_0 \\ 0 & 0 & 0 & 1 \end{array}\right] \\ &=& %\left[sI\ | \mathbf{P}'_0 \right] \left[R\ | -R \mathbf{P}_0 \right] \left[\begin{array}{cccc} s & 0 & 0 & x_0' \\ 0 & s & 0 & y_0' \\ 0 & 0 & s & z_0' \\ 0 & 0 & 0 & 1 \end{array}\right] \left[\begin{array}{cccc} r_{11} & r_{12} & r_{13} & -x''_0 \\ r_{21} & r_{32} & r_{23} & -y''_0 \\ r_{31} & r_{32} & r_{33} & -z''_0 \\ 0 & 0 & 0 & 1 \end{array}\right]\ \ \ \ \mbox{(let $\mathbf{P}''_0 = R \mathbf{P}_0)$} \\ &=& \left[\begin{array}{cccc} s r_{11} & s r_{12} & s r_{13} & x'_0 - s x''_0 \\ s r_{21} & s r_{32} & s r_{23} & y'_0 - s y''_0 \\ s r_{31} & s r_{32} & s r_{33} & z'_0 - s z''_0 \\ 0 & 0 & 0 & 1 \end{array}\right] \end{eqnarray} $$

To find the rotation $R$ we align the edge $\overline{\mathbf{P}_0 \mathbf{P}_1}$ with the edge $\overline{\mathbf{P}'_0 \mathbf{P}'_1}$ and align the plane of $\triangle \mathbf{P}_0 \mathbf{P}_1 \mathbf{P}_2$ with the plane of $\triangle \mathbf{P}'_0 \mathbf{P}'_1 \mathbf{P}'_2.$ We define the source orthonormal coordinate axes as $$ \begin{eqnarray} \mathbf{X} &=& \frac{\mathbf{P}_1 - \mathbf{P}_0}{\|\mathbf{P}_1 - \mathbf{P}_0 \|}\\ \mathbf{Z} &=& \frac{(\mathbf{P}_2 - \mathbf{P}_0) \times \mathbf{X}}{\| (\mathbf{P}_2 - \mathbf{P}_0) \times \mathbf{X}\|} \\ \mathbf{Y} &=& \mathbf{X} \times \mathbf{Z} \end{eqnarray} $$ and the target coordinate axes as $$ \begin{eqnarray} \mathbf{X}' &=& \frac{\mathbf{P}'_1 - \mathbf{P}'_0}{\|\mathbf{P}'_1 - \mathbf{P}'_0 \|}\\ \mathbf{Z}' &=& \frac{(\mathbf{P}'_2 - \mathbf{P}'_0) \times \mathbf{X}'}{\| (\mathbf{P}'_2 - \mathbf{P}'_0) \times \mathbf{X}'\|} \\ \mathbf{Y}' &=& \mathbf{X}' \times \mathbf{Z}' \end{eqnarray} $$ We are looking for the rotation $R$ that maps the source coordinate axes into the target axes: $$ \begin{eqnarray} R \left[ \mathbf{X}\ \mathbf{Y}\ \mathbf{Z} \right] &=& \left[ \mathbf{X}'\ \mathbf{Y}'\ \mathbf{Z}' \right] \\ R &=& \left[ \mathbf{X}'\ \mathbf{Y}'\ \mathbf{Z}' \right] \left[ \mathbf{X}\ \mathbf{Y}\ \mathbf{Z} \right]^T %\\ % &=& \left[ \mathbf{X}'\ \mathbf{Y}'\ \mathbf{Z}' \right] % \left[ \begin{array}{c} % \mathbf{X}^T \\ % \mathbf{Y}^T \\ % \mathbf{Z}^T % \end{array} % \right] \end{eqnarray} $$ So we have a uniform scale $s,$ rotation $R,$ and translation $\mathbf{P}'_0 - s R \mathbf{P}_0.$