点云配准算法-旋转矩阵估计-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 $H$ between $P$ and $Q$

$$H=P^{T}Q$$

Third Step: calculate the optimal rotation matrix

base calculating formula:

$$R=(H^{T}H{)}^{\frac{1}{2}}{H}^{-1}$$

但是如果出现一些特殊情况，例如：$H$ 不存在逆矩阵时，上述公式变得复杂，甚至不可解。所以一般都会使用SVD（奇异值分解，singular value decomposition）变换上述公式。

$$SVD:H=U\mathrm{\Sigma }{V}^T$$

并且校正旋转矩阵确保在一个右手坐标系，最终计算最佳旋转矩阵$R$ 。

$$d=sign(det(V{U}^T))$$

$$R=V\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& d\end{array}\right)U^T$$

最佳旋转矩阵还可以用四元数（quaternions来表示）

Generalizations

The algorithm was described for points in a three-dimensional space. The generalization to D dimensions is immediate.