Say, I measure the 3D positions, $\mathbf{p_1(t), p_2(t), p_3(t)} \in \mathbb{R}^3$ of three points in space which are all connected by a rigid body at time $t = t_0$. Then, I make a second measurement, at $t = t_1$, after the body has rotated and translated. How can I determine the corresponding orientation of that movement?
Either a rotation matrix R ($\in SO3$) or quaternion q ($\in H$) is fine. I would like to implement this in software and I'm looking for a quick solution, ideally without the use of high level library functions (eg. Matlab qr() or oth()).
I guess we want to satisfy the following equations:
$$\mathbf{p_1}(t_1) = \mathbf{R}\ \mathbf{p_1}(t_0) + \mathbf{t} $$
$$\mathbf{p_2}(t_1) = \mathbf{R}\ \mathbf{p_2}(t_0) + \mathbf{t} $$
$$\mathbf{p_3}(t_1) = \mathbf{R}\ \mathbf{p_3}(t_0) + \mathbf{t}$$
Where $\mathbf{R}$ is the rotation I am looking for and $\mathbf{t}$ is the translation.
Best Answer
I posted this answer to a similar question on sci.math. I will transcribe the question and the summary of the solution below. For this problem, we don't need to compute $r$, just set it to $1$.
Least-Squares Conformal Multilinear Regression
Given $\{ P_j : 1 \le j \le m \}$ and $\{ Q_j : 1 \le j \le m \}$, two sets of points, we want to find a conformal map, defined by a linear map, $M$, and a vector, $R$, which maps one set of points to the other via $$ Q = P M + R\tag{1} $$ where we require that $M M^T = r^2 I$ and that the square residue $$ \sum_{j=1}^m\left|P_jM+R-Q_j\right|^2\tag{2} $$ is minimal. Note that $(1)$ requires that $P$ and $Q$ are row vectors.
Summary of the Method
To find the least squares solution to $P M + R = Q$ for a given set of $\{ P_j \}$ and $\{ Q_j \}$, under the restriction that the map be conformal, we first compute the centroids $$ \overline{P}=\frac1m\sum_{j=1}^mP_j\qquad\text{and}\qquad \overline{Q}=\frac1m\sum_{j=1}^mQ_j $$ Next, compute the matrix $$ \begin{align} S &=\sum_{j=1}^m\left(Q_j-\overline{Q}\right)^T\left(P_j-\overline{P}\right)\\ &=\sum_{j=1}^mQ_j^TP_j-m\overline{Q}^T\overline{P} \end{align} $$ Let the Singular Value Decomposition of $S$ be $$ S=UDV^T $$ Next compute $\{ c_k \}$ with $$ \begin{align} c_k &=\sum_{j=1}^m\left[\left(P_j-\overline{P}\right)V\right]_k\left[\left(Q_j-\overline{Q}\right)U\right]_k\\ &=\sum_{j=1}^m\left[P_jV\right]_k\left[Q_jU\right]_k-m\left[\overline{P}V\right]_k\left[\overline{Q}U\right]_k \end{align} $$ and define $$ a_k = \mathrm{sgn}( c_k ) $$ Let $I_k$ be the matrix with the $(k,k)$ element set to $1$ and all the other elements set to $0$. Then calculate $$ E=\sum_{k=1}^na_kI_k $$ Compute the orthogonal matrix $$ W=VEU^T $$ If $\det(W) < 0$ but $\det(W) > 0$ is required, change the sign of the $a_k$ associated with the $c_k$ with the smallest absolute value.
If required, compute $r$ by $$ r\sum_{j=1}^m\left|P_j-\overline{P}\right|^2=\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_j-\overline{Q}\right\rangle $$ or equivalently $$ r\left(\sum_{j=1}^m\left|P_j\right|^2-m\left|\overline{P}\right|^2\right) =\sum_{j=1}^m\left\langle P_jW,Q_j\right\rangle-m\left\langle\overline{P}W,\overline{Q}\right\rangle $$ Finally, we have the desired conformal map $Q = P M + R$ where $$ M = r W $$ and $$ R = \overline{Q} - \overline{P} M $$ More information, easier computation
Suppose you want to map $\{P_i\}_{i=1}^3$ to $\{Q_i\}_{i=1}^3$, and the distances between the $P_i$'s and $Q_i$'s are the same. Compute a fourth point by $$ P_4=P_1+(P_2-P_1)\times(P_3-P_1) $$ and $$ Q_4=Q_1+(Q_2-Q_1)\times(Q_3-Q_1) $$ Then create the matrix $P$ whose columns are $P_2-P_1$, $P_3-P_1$, and $P_4-P_1$.
Also create the matrix $Q$ whose columns are $Q_2-Q_1$, $Q_3-Q_1$, and $Q_4-Q_1$.
Then $x\mapsto QP^{-1}x+(Q_1-QP^{-1}P_1)$ maps the source points to the destination points.