User:BenFrantzDale/Rotation estimation

This is the Kabsch algorithm: Find R for a matched set of point clouds to minimize the sum of squared error of rotating one point set onto the other:
 * $$\operatorname{argmin}_{R\in SO(3)} \sum_i W_i \| m_i - R m_i'\|^2$$

assuming the centroids of the point clouds are aligned.

This sum of squared error expands to
 * $$\operatorname{argmin}_{R\in SO(3)} \sum_i W_i (\|m_i\|^2 - 2 m_i^\top R m_i') + \|Rm_i'\|^2)$$

Note that an inner product, $$a^\top b$$ is the sum $$a_0 b_0 + a_1 b_1 + \cdots$$ which is equal to the sum of diagonal elements of the outer product $$a b^\top$$. That is:
 * $$a^\top b = \operatorname{trace}(a b^\top)$$

And by linearity, the sum of inner products is equal to the trace of a the sum of outer products. And so we have
 * $$\operatorname{argmin}_{R\in SO(3)} \sum_i W_i \|m_i\|^2 - 2\, \operatorname{trace} \left( R^\top \sum_i W_i m_i m_i'^\top\right) + \sum_i W_i \| m_i'\|^2$$.

Define the correlation matrix, K between the points:
 * $$ K:= \sum_i W_i m_i m_i'^\top$$

Note that this is the only part of the cost function affected by the rotation. We want to reduce the sum above so we want to maximize the term involving K, so we want to solve
 * $$\operatorname{argmax}_{R\in SO(3)} \operatorname{trace}(R^\top K)$$.

This is solved by singular value decomposition of K. Let:
 * $$U\Sigma V^\top = K$$

where $$\Sigma$$ is the diagonal matrix of ordered singular values and U and V are orthogonal. (We deal with the case that U or V is an inversion below; for now assume they are positive definite.) So now we have
 * $$\operatorname{argmax}_{R\in SO(3)} \operatorname{trace}(R^\top U\Sigma V^\top)$$

but trace is unchanged by rotation, so we can multiply by $$V^\top$$ on the left and $$V$$ on the right to get
 * $$\operatorname{argmax}_{R\in SO(3)} \operatorname{trace}(V^\top R^\top U\Sigma)$$

Now write $$Z = V^\top R^\top U$$ and note that $$Z$$ is the product of orthogonal matrices and so orthogonal. Thus we must optimize:
 * $$\operatorname{argmax}_{R\in SO(3)}(\operatorname{trace}(V^\top R^\top U \Sigma) = \operatorname{trace}(Z\Sigma) = \sum_i Z_{i,i} \Sigma_{i,i})$$.

But $$\Sigma$$ is diagonal and non-negative and $$Z$$ is always orthogonal, thus this is maximized by choosing $$R$$ such that $$Z=I$$, so we can set
 * $$R:=U V^\top$$

Then we have:
 * $$V^\top R^\top U = V^\top (U V^\top)^\top U = V^\top VU^\top U = I$$.

In reality, we are not guaranteed that $$UV^\top$$ is not an inversion. We can correct this by instead setting
 * $$ R := U \begin{bmatrix} 1&0& 0\\0&1&0 \\0 & 0&\det(UV^\top) \end{bmatrix} V^\top$$

Adapted from Geometric Computation for Machine Vision by Kanatani