点云配准算法-旋转矩阵估计-Kabsch-Umeyama algorithm
Kabsch-Umeyama algorithm
参考文献：
面向点云配准，最小化两点集均方根误差(RMSD, root mean squared deviation)来计算最佳旋转矩阵。
注：该算法只能计算旋转矩阵，做点云配准还需要计算平移向量。当平移和旋转都正确计算出，该算法有时也叫_partial Procrustes superimposition (see also orthogonal Procrustes problem)_
步骤：
- a translation
- the computation of a covariance matrix
- the computation of the optimal rotation matrix
First Step: Translation
将两组点集的质心对齐坐标系原点：
\[\hat{P} = || P - P_{centroid} ||_1
\]
Second Step: calculate the covariance matrix
calculate the corrs-covariance matrix \(H\) between \(P\) and \(Q\)
\[H = P^TQ
\]
Third Step: calculate the optimal rotation matrix
base calculating formula:
\[R=(H^TH)^{\frac{1}{2}}H^{-1}
\]
但是如果出现一些特殊情况，例如：\(H\) 不存在逆矩阵时，上述公式变得复杂，甚至不可解。所以一般都会使用SVD（奇异值分解，singular value decomposition）变换上述公式。
\[SVD: H=U\Sigma V^T
\]
并且校正旋转矩阵确保在一个右手坐标系，最终计算最佳旋转矩阵\(R\) 。
\[d = sign(det(VU^T))
\]
\[R = V \begin{pmatrix} 1&0&0\\0&1&0\\0&0&d\end{pmatrix}U^T
\]
最佳旋转矩阵还可以用四元数（quaternions来表示）
Generalizations
The algorithm was described for points in a three-dimensional space. The generalization to D dimensions is immediate.
