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?
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)
 A: 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)$].
A: I try to summarize what I have to do in this answer:

*

*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$.


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


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


*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}$$


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