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

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.

## 1 Answer

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.$$

• Why did you assume a similarity? The question allows for a non-uniform scale. Commented Mar 28 at 21:52