Kabsch算法【1】由W.Kabsch在1976年提出的,用于求解最优旋转,在分子生物学,特别是比较蛋白质的相似性方面有重要的应用。
文章【2】将其应用在传感器外参标定上,即对属于同一目标的两批三维点,通过Kabsh算法求得其旋转矩阵R。
方法推导
设 P P P和 Q Q Q是同一个目标在不同坐标系下的两组点, p i p_i pi和 q i q_i qi是集合内的第 i i i个点。令两个坐标系的旋转为 R R R,平移为 T T T,那么Kabsch求解下述优化问题:
(1) m i n E = 1 N ∑ i = 1 N ∣ ∣ R p i − q i + T ∣ ∣ 2 2 minE = \frac{1}{N}\sum_{i=1}^{N}{||Rp_i-q_i+T||_2^2}\tag{1} minE=N1i=1∑N∣∣Rpi−qi+T∣∣22(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