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

-
What do you mean by orientation? –  M.B. Oct 27 '12 at 15:24
What is also known as attitude. en.wikipedia.org/wiki/Orientation_(geometry) The rotation required from the initial coordinate system to end up where it is oriented. Usually represented by a rotation matrix R ( ∈ SO3) or or quaternion q (∈ H) –  user1323995 Oct 27 '12 at 19:35
A rotation is only defined between two coordinate frames. To give just a single set of points and ask for its orientation doesn't make sense. You have to have one set of points in the initial orientation, and one set of points in the final orientation, and ask for the rotation matrix that takes one to the other. –  Rahul Oct 27 '12 at 20:23
Ok, I think you are right. I can only treat one set of three points as my start orientation and then compute the orientation towards a second set of three points. –  user1323995 Oct 27 '12 at 20:40

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.

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.

-
Thank you. I will have a look at this. Seems much more complicated than I had hoped the solution would be. –  user1323995 Oct 27 '12 at 20:39
This is perhaps more general than you need. It handles the least squares fit for more than three points. However, if you know which source points correspond to which destination points, then you can simplify this greatly. I will add this to my answer. –  robjohn Oct 27 '12 at 22:31

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.

-