Kabsch算法求解旋转矩阵

Kabsch算法【1】由W.Kabsch在1976年提出的，用于求解最优旋转，在分子生物学，特别是比较蛋白质的相似性方面有重要的应用。

文章【2】将其应用在传感器外参标定上，即对属于同一目标的两批三维点，通过Kabsh算法求得其旋转矩阵R。

---

方法推导

设$P$和$Q$是同一个目标在不同坐标系下的两组点，$p_i$和$q_i$是集合内的第$i$个点。令两个坐标系的旋转为$R$，平移为$T$，那么Kabsch求解下述优化问题：
$$minE=\frac{1}{N}\sum _{i=1}^N||R{p}_i-q_i+T|{|}_2^2\text{\hspace{1em}(1)}$$
由(1)式，可以推得
(2) m i n N E = ∑ i = 1 N ∣ ∣ p i , − q i ∣ ∣ 2 2 = T r ( ( P ′ − Q ) T ( P ′ − Q ) ) minNE =\sum_{i=1}^{N}{||p_i^,-q_i||_2^2}=Tr((P'-Q)^T(P'-Q))\tag{2} minNE=i=1∑N​∣∣pi,​−qi​∣∣22​=Tr((P