# How to solve an overdetermined system of point mappings via rotation and translation

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?

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)

-
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

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)$].

-
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

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.

-
Looks good to me. – joriki Oct 31 '12 at 17:06