Take the 2-minute tour ×
Mathematics Stack Exchange is a question and answer site for people studying math at any level and professionals in related fields. It's 100% free, no registration required.

I have a set of points in one coordinate system $P_1, \ldots, P_n$ and their corresponding points in another coordinate system $Q_1, \ldots , Q_n$. All points are in $\mathbb{R}^3$.

I'm looking for a "best fit" transformation consisting of a rotation and a translation. I.e.

$$ \min_{A,b} \sum (A p_i + b - q_i)^2 , \quad A \in \operatorname{SO}(3), b \in \mathbb{R}^3$$

Can anyone give me some hint in which direction I should search?

I already looked at:

http://en.wikipedia.org/wiki/Least_squares (don't know how to include the restriction to orthogonal matrices)

http://en.wikipedia.org/wiki/Singular_value_decomposition (I thought that I'd start with a matrix from $\operatorname{GL}(3)$ and use the "best fit orthogonal matrix" afterwards like stated in http://en.wikipedia.org/wiki/Orthogonal_Procrustes_problem but that seamed too complicated)

share|improve this question
Note that the stationarity condition with respect to $b$ can be solved explicitly: Differentiating with respect to $b$ yields $\sum(Ap_i+b-q_i)=0$ and thus $b=q-Ap$ with $p$ and $q$ the centroids of the $p_i$ and $q_i$, respectively, so you can subtract out the centroids and then you only need to minimize $\sum(A(p_i-p)-(q_i-q))^2$ with respect to the rotation. –  joriki Oct 30 '12 at 16:06
Thanks for correcting the Mathjax. I didn't see the result in my edit since our virus scanner blocked the mathJax script ;-). I hope it'll improve in future posts... –  Onur Oct 31 '12 at 16:29
add comment

2 Answers

up vote 2 down vote accepted

As described in my comment, first subtract out the centroids to reduce the problem to

$$ \min_A\sum_i\|Ap_i-q_i\|^2\;,\quad A\in SO(3)\;. $$

That problem is solved here (page 9), with the vectors $p_i$ and $q_i$ assembled into the matrices $B$ and $A$ (with the present $A$ corresponding to $Q$), but I'll write it out with the vectors in your notation.

The problem appears to be quadratic in $A$, but it isn't, since

$$ \begin{align} \|Ap_i-q_i\|^2&=(Ap_i-q_i)^\top(Ap_i-q_i) \\ &=p_iA^\top Ap_i+q_i^\top q_i-2q_i^\top Ap_i \\ &= p_ip_i+q_i^\top q_i-2q_i^\top Ap_i\;, \end{align} $$

since $A$ is orthogonal. Thus all we need to do is maximize

$$ \def\tr{\operatorname{tr}} \sum_iq_i^\top Ap_i=\sum_i\tr q_i^\top Ap_i=\sum_i\tr p_iq_i^\top A=\tr\left(\left(\sum_ip_iq_i^\top\right)A\right)=:\tr B^\top A $$

subject to the constraint $A\in SO(3)$. That problem is solved in the document linked to above, using the singular-value decomposition $B=U\Sigma V^\top$. For $A\in O(3)$ the solution is $A=UV^\top$, whereas if $A$ is restricted to $SO(3)$, the solution is $A=UZV^\top$, where $Z$ is the identity matrix, but with the last diagonal element replaced by $-1$ if this is required to make $\det A$ positive.

See also this question [which in the meantime has received another answer that derives the above solution to $\tr B^\top A\to\min$ for the case $A\in O(n)$].

share|improve this answer
Since you use the trace I assume I have to use the Frobenius norm to follow your steps. Since I'd like to measure the transformation error for a given set of points according to the 2-norm, will this work? –  Onur Oct 31 '12 at 12:14
@Onur: I don't understand the question. How is the fact that the solution uses the trace related to the measure of the transformation error? I started from your definition of your error and derived how to minimize it; where do you see a potential for problems? –  joriki Oct 31 '12 at 16:34
I mixed up two different things: transformation error and error induced due to the "erroneous" transformation. –  Onur Oct 31 '12 at 16:47
add comment

I try to summarize what I have to do in this answer:

  1. Convert the $P_i$s and $Q_i$s:

    $\overline{p}$ and $\overline{q}$ are the center of gravity for the set of points to be mapped: $$ \overline{p} := \frac1i\sum_iP_i, \quad \overline{q} := \frac1i\sum_iQ_i$$ Make the points relative to their respective center of gravity: $$ p_i := P_i - \overline{p}, \quad q_i := Q_i - \overline{q}$$ This removes the need to consider the translation $b$ in the optimization. $b$ can be calculated as $\overline{q} - A \overline{p}$ once we have determined the optimal $A$.

  2. Calulate $B$ as $$B = \left(\sum_i p q^\top \right)^\top = \sum_i q p ^\top$$

  3. Perform SVD decomposition of $B$: $$ B = U \Sigma V^\top$$

  4. Optimal $A$ if $A \in \operatorname{O}(3)$ (not $A \in \operatorname{SO}(3)$ as stated in the question): $$ A := U V^\top $$ Optimal $b$: $$ b := \overline{q} - A \overline{p}$$

  5. Optimal $A$ if $A \in \operatorname{SO}(3)$ would go here but I think $A \in \operatorname{O}(3)$ is what I'm really looking for.

share|improve this answer
Looks good to me. –  joriki Oct 31 '12 at 17:06
add comment

Your Answer


By posting your answer, you agree to the privacy policy and terms of service.

Not the answer you're looking for? Browse other questions tagged or ask your own question.