6
$\begingroup$

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)

  • 0
    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...2012-10-31

2 Answers 2

2

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

  • 0
    I mixed up two different things: transformation error and error induced due to the "erroneous" transformation.2012-10-31
1

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.

  • 1
    Looks good to me.2012-10-31