点云配准算法--kabsch

点云配准算法–kabsch

转载来自： https://zhuanlan.zhihu.com/p/535105203

两个点云P和P’内各个顶点一一对应，那么如何旋转和平移点云P，使其尽量贴合点云P’？换句话讲，我们有一系列点P，将其整体进行一个旋转以后得到新的一系列点P’，那么我们在已知P和P’的条件下，怎么得到这个旋转矩阵？

流程

假设有两堆点云，各自坐标分别为 { p i , p i ′ } \{p_i, p'_i\} {pi​,pi′​}。点集一一对应，并且存在如下转换关系

$p_i^{\prime }=R{p}_i+T+N_i$

• R：旋转矩阵
• T: 平移矩阵
• $N_i$：噪声向量

我们如何求R和T？

1. 分别求两个点云的质心
   $p=\frac{1}{n}{\sum }_{i=1}^n{p}_i$
   $p^{\prime }=\frac{1}{n}{\sum }_{i=1}^n{p}_i^{\prime }$
   得到中心点

2. 求各点相对于质心的位移向量
   $q_i=p_i-p$
   $q_i^{\prime }=p_i^{\prime }=p^{\prime }$

3. 利用质心位移向量，计算H矩阵（质点的协方差矩阵）
   $H={\sum }_{i=1}^n{q}_i{q}^{\prime }{{}_i}^T$

4. 对H矩阵进行SVD分解
   $H=U\Lambda V^T$

5. 基于矩阵U和V，计算旋转矩阵R
   $R=U{V}^T$
   验证结果，若$det(R)=1$则R有效，若$det(R)=-1$，则算法失效（旋转矩阵的det®=1）

6. 计算点云间的位移
   $T=p^{\prime }-Rp$

推导

对于一个旋转矩阵，我们希望旋转后的点$p_i$与目标点$p_i^{\prime }$的差异最小，这里我们使用MSE(Minimum Squared Error)，即取它们之间差异，求二范数，严格来说，是求二范数的平方，即模长的平方。再把每个点差异的平方求和。我们期望它最小化

在上式中，求和符号中的部分，是两个向量的点乘。由于

而又由于这个式子的结果实际上是一个标量，因此式子整体求转置是一样的，标量的转置是标量本身。

继续推导

我们知道，前两项是常数，我们要从$q_i$和$q_i^{\prime }$之间的关系下手。我们的目标是最小化${\sum }^2$，因此我们是要最大化最后一项。即最大化函数F

注意，这个函数F是将RH求迹，即该矩阵对角线之和，换句话说就是我们的数据点决定了H矩阵长什么样子，而我们要再找到一个线性变换矩阵R，使其的迹最大。

其中
$H={\sum }_{i=1}^n{q}_i{q}^{\prime }{{}_i}^T$

补充矩阵的迹相关内容:
迹的性质：

1. $tr(A)=tr(A^T)$
2. tr(ABCD)=tr(BCDA)=tr(CDAB)=tr(DABC)
   即把第一个矩阵放最后一个，或者反着来，其迹不变。
3. 矩阵的迹等于特征根之和
4. 假设x，y都是n维列向量，我们有
   $tr(x{y}^T)=tr(y^{T}x)$

注意，这里的$q{q}^{\prime T}$可不是两个向量点乘了，而是数据$q,q^{\prime }$的协方差矩阵。对于3维空间的数据来说，$q:3\times 1,q^{\prime T}:1\times 3$。则有$q{q}^{\prime T}:3\times 3$。也就是说，这$H$矩阵实际上是坐标点前后做协方差矩阵，并求和的结果。

将H矩阵进行SVD分解，得到
$H=U\Lambda V^T$

• U和V：3x3正交阵
• $\Lambda$：奇异值矩阵，是个非负对角阵
  令
  $X=V{U}^T$
  那么
  $XH=V{U}^{T}U\Lambda V^T=V\Lambda V^T$
  是一个正定矩阵，见正定矩阵的定义。

注意SVD的的U和V是正交旋转阵，其转置等于逆 。XH矩阵的形式就是$V\Lambda V^T$，我们都知道$V{V}^T$是对称的，那么这个
作为对角阵，是对该矩阵的一个缩放，必然也是对称的。

定理：对于任意正定矩阵$A{A}^T$和任意正交矩阵B，有

证明：令$a_i$是矩阵$A$的第$i$列，那么

根据Cauchy—Schwarz不等式

这里的点代表的是点乘。
我们得到了一个结论

注意，B是正交阵，所以$B{B}^T=I$，开方符号下的内容相当于$a_i$自己对自己点乘的平方

得

我们根据该定理，得出：对于正定对称阵XH，和任意一个3x3的正交矩阵B，有

这就是在说，矩阵$XH$，在对其进行一次旋转$B$之后，该矩阵$BXH$的迹无论如何都小于原来的矩阵$XH$的迹。
回顾一下，
$F=Trace(RH)$

我们的目标是找到矩阵$R$，来最大化函数$F$，即$RH$的迹。而我们知道这个特殊的$X$能最大化这个迹，因为$XH$之后无论再怎么对其进行线性变换，它的迹都不可能超过$XH$的迹。那么实际上$X$就是我们要找的$R$了。

代码

def kabsch(P, Q):
    """
    kabsch算法（点云配准算法）：有两个点集P和Q，这两个点集的点一一对应，但是P和Q并不重合
    假设P受到一根骨骼的影响，那么如何旋转和平移这根骨骼，使得最终变换后的P'与Q的差异最小？
    Computes the optimal translation and rotation matrices that minimize the 
    RMS deviation between two sets of points P and Q using Kabsch's algorithm.
    More here: https://en.wikipedia.org/wiki/Kabsch_algorithm
    Inspiration: https://github.com/charnley/rmsd
    
    inputs: P  N x 3 numpy matrix representing the coordinates of the points in P
            Q  N x 3 numpy matrix representing the coordinates of the points in Q
            
    return: A 4 x 3 matrix where the first 3 rows are the rotation and the last is translation
    """
    if (P.size == 0 or Q.size == 0):
        raise ValueError("Empty matrices sent to kabsch")
    centroid_P = np.mean(P, axis=0)
    centroid_Q = np.mean(Q, axis=0)
    # 均值归一到0
    P_centered = P - centroid_P                       # Center both matrices on centroid
    Q_centered = Q - centroid_Q
    H = P_centered.T.dot(Q_centered)                  # covariance matrix
    U, S, VT = np.linalg.svd(H)                        # SVD
    R = U.dot(VT).T                                    # calculate optimal rotation
    # 这里变换为右手系
    if np.linalg.det(R) < 0:                          # correct rotation matrix for             
        VT[2,:] *= -1                                  #  right-hand coordinate system
        R = U.dot(VT).T                          
    t = centroid_Q - R.dot(centroid_P)                # translation vector
    # 反正最终返回的就是一个transform，注意这是一个右乘矩阵，即使一个4行3列的矩阵
    return np.vstack((R, t))