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)式，可以推得
$$minNE=\sum _{i=1}^N||p_i^{,}-q_i|{|}_2^2=Tr((P^{\prime }-Q{)}^T(P^{\prime }-Q))\text{\hspace{1em}(2)}$$
其中$p_i^{\prime }=R{p}_i$，$P^{\prime }=RP$，$Tr()$是求迹运算
根据迹的性质，有
$$Tr((P^{\prime }-Q{)}^T(P^{\prime }-Q))=Tr(P^{\prime T}{P}^{\prime })+Tr(Q^{T}Q)-2Tr(Q^T{P}^{\prime })$$
又因为$P^{\prime T}{P}^{\prime }=P^T{R}^{T}RP=P^{T}P$（旋转矩阵为正交矩阵），有
$$NE=Tr(P^{T}P)+Tr(Q^{T}Q)-2Tr(Q^T{P}^{\prime })\text{\hspace{1em}(3)}$$
求(2)式的最小值相当于求(3)式内$Tr(Q^T{P}^{\prime })$的最大值
$$Tr(Q^T{P}^{\prime })=Tr(Q^{T}RP)=Tr(P{Q}^{T}R)$$
对$P{Q}^T$做SVD分解，有
$$Tr(Q^T{P}^{\prime })=Tr(V\Sigma W^{T}R)=Tr(\Sigma W^{T}RV)=\sum _{i=1}^3{\sigma }_i{T}_{ii}$$
其中$T=W^{T}RV$
因为$R$和$V$都是正交矩阵，因此$T$正交，即$|T_{ii}|\le 1$，因此
$$Tr(Q^T{P}^{\prime })\le \sum _{i=1}^3{\sigma }_i$$
即$W^{T}RV=1$为所求：
$$R=W{V}^T\text{\hspace{1em}(4)}$$
这里需要注意的是$R$必须是右手系下的旋转，因此(4)式修正为
$$R=W\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& d\end{array}\right]V^T\text{\hspace{1em}(5)}$$
其中$d=sign(det(P{Q}^T))$

---

算法步骤

根据上一节的讨论，可以得到Kabsch算法求解旋转矩阵R的步骤

1. 将点集$P$和$Q$均值归一化到0
2. 计算$C=P{Q}^T$
3. 对$C$做SVD分解，$[V,S,W]=SVD(C)$
4. 计算修正符号，$d=sign(det(C))$
5. 计算旋转，$R=W\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& d\end{array}\right]V^T$

---

算法实现

WiKi上有Matlab、C++、Python的实现链接

---

参考文献

【1】Kabsch W . A Solution of the Best Rotation to Relate Two Sets of Vectors[J]. Acta Crystallographica Section A Foundations of Crystallography, 1976, A32(5).
【2】Chai Z, Sun Y, Xiong Z. A Novel Method for LiDAR Camera Calibration by Plane Fitting[C]//2018 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM). IEEE, 2018: 286-291.

---