点云配准算法-旋转矩阵估计-Kabsch-Umeyama algorithm

Kabsch-Umeyama algorithm

参考文献：

1. https://www.wikiwand.com/en/Kabsch_algorithm

面向点云配准，最小化两点集均方根误差(RMSD, root mean squared deviation) 来计算最佳旋转矩阵。

注：该算法只能计算旋转矩阵，做点云配准还需要计算平移向量。当平移和旋转都正确计算出，该算法有时也叫partial Procrustes superimposition (see also orthogonal Procrustes problem)

步骤：

1. a translation
2. the computation of a covariance matrix
3. 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