Given 3 points of a rigid body in space, how do I find the corresponding orientation (aka rotation or attitude)? 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.
 A: 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.
A: You may solve it like this:
1. Define base-reference frame that doesnt move, say $F_0$. This could be simply your frame of reference for these 3 positions you have ($p_i$) 
2. Define reference frame, say $F_r$ attached to the rigid body using 3 points ($p_i$). For example with $p_0$ as origin, unit vector in $p_1$ from $p_0$ as x axis and in $p_2$ from $p_0$ as y-axis and choosing z-axis by right-hand-rule.
3. Assuming T as homogenous transformation matrix, you know orientation of points in both reference frames $F_0$ and $F_r$ and also displacement of $F_r$ wrt $F_0$,so you could solve for T as an inverse problem. You would still need matrix inversion function though.
