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

Question

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)

Answer 1

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

Answer 2

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.