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}-Rm_{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}-2m_{i}^{\top }Rm_{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 $ab^{\top }$. That is:

$a^{\top }b=\operatorname {trace} (ab^{\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:=UV^{\top }$

Then we have:

$V^{\top }R^{\top }U=V^{\top }(UV^{\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

See also

• Orthogonal Procrustes problem
• Wahba's problem
• Kabsch algorithm