Kabsch-algorithm

Kabsch-algorithm 是一种用于计算两个几何体(点集)之间的最佳对齐的算法。

目标

两个集合体 $P,Q\in {\mathbb{R}}^{N\times 3}$，找到一个$SE(3)$变换使得，$P$和$Q$的距离最小。

默认所有向量都为列向量

$$\underset{R,t}{\min }l(R,t)=\sum _{i=1}^N\Vert p_i-(R{q}_i+t){\Vert }^2=(p_i-(R{q}_i+t){)}^T(p_i-(R{q}_i+t)),t\in {\mathbb{R}}^3,R\in SO(3)$$ $$\begin{array}{rl}l& =\sum _{i=1}^N\Vert p_i{\Vert }^2+\Vert (R{q}_i+t){\Vert }^2-2p_i^T(R{q}_i+t)\\ & =\sum _{i=1}^N\Vert p_i{\Vert }^2+\Vert R{q}_i{\Vert }^2+\Vert t{\Vert }^2+2t^T(R{q}_i)-2p_i^T(R{q}_i+t)\\ & =\sum _{i=1}^N\Vert p_i{\Vert }^2+\Vert R{q}_i{\Vert }^2+\Vert t{\Vert }^2+2t^T(R{q}_i-2p_i)-2p_i^{T}R{q}_i\\ & =\sum _{i=1}^N\Vert p_i{\Vert }^2+\sum _{i=1}^N\Vert R{q}_i{\Vert }^2+N\Vert t{\Vert }^2+2t^T\sum _{i=1}^N(R{q}_i-2p_i)-2\sum _{i=1}^N{p}_i^{T}R{q}_i\\ \frac{\partial l}{\partial t}& =2Nt+2\sum _{i=1}^N(R{q}_i-2p_i)=0\\ & \Rightarrow t=\frac{1}{N}\sum _{i=1}^N(p_i-R{q}_i)\\ & =\frac{1}{N}\sum _{i=1}^N{p}_i-\frac{1}{N}\sum _{i=1}^{N}R{q}_i\\ & =\frac{1}{N}\sum _{i=1}^N{p}_i-R\frac{1}{N}\sum _{i=1}^N{q}_i\end{array}$$

因此原问题可以先将两个点集的质心对齐，然后计算旋转矩阵。

$A=P-\frac{1}{N}{\sum }_{i=1}^N{p}_i$

$B=Q-\frac{1}{N}{\sum }_{i=1}^N{q}_i$

最小化目标转化为

$$\underset{R}{\min }l(R)=\sum _{i=1}^N\Vert p_i^{\prime }-R{q}_i^{\prime }{\Vert }^2⟺\underset{R}{\min }\Vert A-(R{B}^T{)}^T{\Vert }_F^2$$

为了简化记号，

$$\begin{array}{rl}\Vert A-(R{B}^T{)}^T{\Vert }_F^2& =\Vert A-B{R}^T{\Vert }_F^2\\ & =Tr((A-B{R}^T{)}^T(A-B{R}^T))\\ & =Tr(A^{T}A-R{B}^{T}A-A^{T}B{R}^T+R{B}^{T}B{R}^T)\\ & =Tr(A^{T}A)+Tr(B^{T}B)-2Tr(A^{T}B{R}^T)\end{array}$$

因此原问题等价于最大化

$$\underset{R}{\max }Tr(A^{T}B{R}^T)=\underset{R}{\max }Tr(R{B}^{T}A)$$

$C:=B^{T}A=US{V}^T$ (SVD分解)

$$Tr(RC)=Tr(RUS{V}^T)=Tr(V^{T}RUS)$$

令 $M=V^{T}RU$，则有：

$$\text{Tr}(RC)=\text{Tr}(MS)$$

最大化迹的形式

• $M$ 仍然是正交矩阵，因为 $V^{T}RU$ 保持正交性。
• $S$ 是对角矩阵，奇异值为 $s_1,s_2,s_3$。

根据迹的性质：

$$\text{Tr}(MS)=\sum _{i=1}^3{M}_{ii}{s}_i$$

此时，我们需要最大化 $M_{ii}{s}_i$ 的和。

利用性质取最大值

由于 $M$ 是正交矩阵，满足：

$$-1\le M_{ii}\le 1$$

为了最大化 $\text{Tr}(MS)$，我们需要 $M_{ii}=1$。最优选择是使得：

$$M=I⟹V^{T}RU=I⟹R=V{U}^T$$

修正旋转矩阵的行列式

旋转矩阵必须满足 $\det (R)=1$。然而 $\det (V{U}^T)$ 的行列式可能为 -1。为了修正这一点，引入一个校正矩阵 $F$：

$$F=\mathrm{diag}(1,1,\det (V{U}^T))$$

这样可以确保：

$$\det (VF{U}^T)=\det (V)\cdot \det (F)\cdot \det (U^T)=1$$

最终最优解为：

$$R=VF{U}^T$$