8
$\begingroup$

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.

$\endgroup$
4
  • $\begingroup$ What do you mean by orientation? $\endgroup$
    – M.B.
    Commented Oct 27, 2012 at 15:24
  • $\begingroup$ 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) $\endgroup$ Commented Oct 27, 2012 at 19:35
  • $\begingroup$ 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. $\endgroup$
    – user856
    Commented Oct 27, 2012 at 20:23
  • $\begingroup$ 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. $\endgroup$ Commented Oct 27, 2012 at 20:40

2 Answers 2

8
$\begingroup$

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.

$\endgroup$
7
  • $\begingroup$ Thank you. I will have a look at this. Seems much more complicated than I had hoped the solution would be. $\endgroup$ Commented Oct 27, 2012 at 20:39
  • $\begingroup$ 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. $\endgroup$
    – robjohn
    Commented Oct 27, 2012 at 22:31
  • $\begingroup$ For anyone else who might be confused by the notation the vectors are row vectors! For example, $Q^T_jP_j$ is the outer product, not the inner product. This might be obvious but, I thought I would make it explicit. $\endgroup$
    – McAngus
    Commented Nov 9, 2021 at 10:51
  • $\begingroup$ @McAngus: Sorry; I had thought the fact that the multiplication in $(1)$ was $Q=PM+R$ made that clear enough. I have included this in the answer. $\endgroup$
    – robjohn
    Commented Nov 9, 2021 at 17:09
  • $\begingroup$ I think it is clear! It just took me a few minutes to sort it out as I'm used to dealing with column vectors. $\endgroup$
    – McAngus
    Commented Nov 9, 2021 at 17:18
0
$\begingroup$

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.

$\endgroup$

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .