User:BenFrantzDale/Rotation estimation

Find R for

$\operatorname {argmin} _{R\in SO(3)}\sum _{i}W_{i}\|m_{i}-Rm_{i}'\|^{2}$

This becomes

$\sum _{i}W_{i}(\|m_{i}\|^{2}-2(m_{i},Rm_{i}')+\|Rm_{i}'\|^{2})$

$=\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}$

So we let the correlation matrix, K between the points be

$K:=\sum _{i}W_{i}m_{i}m_{i}'^{\top }$

since that's the part we can affect by 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 SVD of K to give

$K=V\Lambda U^{\top }$

where $\Lambda$ is the diagonal matrix of ordered singular values and U and V are orthogonal. Then we get

$R=V{\begin{bmatrix}1&0&0\\0&1&0\\0&0&\det(VU^{\top })\end{bmatrix}}U^{\top }$

from Geometric Computation for Machine Vision by Kanatani