我的毕业设计用到了CPD算法，以及CPD代码工具包，当时刚开始接触CPD时在网上没找到几篇教程或是经验总结的东西。现在毕设做完了，就结合我的理解写一篇文章。可能写的会有点乱，因为我也是一时兴起想要做一下整理，所以并没有列出详细的大纲和框架就直接写了。请多包涵。

        对于算法我只写我的简单理解，具体的算法推导网上已经有人写了，我的数学能力不太能够理解具体的算法推导，所以这篇文章只是用来将算法中的公式与代码的实现对应起来。希望对CPD算法的学习者能够有所帮助。

        CPD工具包是用MATLAB写的，下载链接在这里：https://blog.csdn/kaspar1992/article/details/78629916。

一、算法的简单介绍

        简单说一下点云配准算法的问题描述：对于两个需要进行配准的点云，一个点云为参考点云（或模板点云）X，另一个点云为待配准点云Y。配准过程是将待配准点云Y逐渐向参考点云移动，直到两者配准。如下图所示，这是一个简单的示意，X点集不动，Y点集向X点集移动。

  

        现在我们假设X点集的规模为N=4个点，Y点集的规模为M=3个点。

       

        现在计算X点集中的n0点到Y中的每一个点的距离。分别记作dn0m0，dn0m1……。CPD算法中GMM概率密度函数表示为：

                                                                   

        其中

   

        上式中的x-ym其实就是点与点之间的距离d，此处||x-ym||²即表示两点距离的平方。

        似然函数可以写为，通过最大化该似然函数，即可得到需要的参数θ和σ²，这里的θ表示一个参数集合，θ包含了待配准点集的旋转矩阵R，平移矩阵t，缩放矩阵s，可以理解为θ={R,t,s}。而为了求解该似然函数，可以利用EM算法求解。

        关于后续的算法推导，我确实一知半解，可以看看别人的推导过程。

 

二、文件夹中的各个文件用途

        首先简单介绍一下工具包中的各个文件作用。工具包下载下来后，里面一共有3个文件夹，分别为core、data、example。

        1.core中即为主要的算法核心代码，其中包含如下几个文件夹。

        FGT文件夹中的代码是快速CPD算法的实现。

        mex文件夹中存放着几个.c文件，其中最核心的是cpd_P.c，这个代码是用于计算点与点之间的距离的，后面会讲到。

        Nonrigid文件夹中的是非刚性点集配准的代码，Rigid文件夹中的是刚性点集配准代码。本文只介绍刚性点集配准代码。

        utils文件夹中是一些简单的函数代码，用于生成随机点集用于测试或是用于绘制点集使配准可视化等等功能。

        除了上述几个文件夹，core文件夹中还有两个文件。cpd_make用于编译mex中的.c文件。由于计算两点间的距离需要用到多次的循环运算，为了代码效率，将计算距离部分以C语言实现，工具包使用了C语言和matlab语言混合编程。

        另一个cpd_register.m则是用于返回待配准点集的空间变换矩阵的函数代码，这一部分后面会讲到。

        2.data文件夹中为点云测试集，以.mat的格式保存。

        这个文件夹中mat文件有的有X和Y两个数据集，一个用于刚性配准一个用于非刚性配准。

        3.example文件夹中的代码为实例，用于演示数据集的配准。

 

三、代码运行

        接下来我通过运行一个示例的example文件，看看会用到哪几个函数。

        在运行代码前记得把工具包添加入路径。

        以cpd_rigid_example1.m这个文件为例，可以将其类比为main函数。初次运行时可能会报错，可以将cpd_make.m中开头的

versn删除，即可运行。

        example1中的代码如下

        开头加载了鱼的点集。由于这个.mat文件中有X和Y两个表格，X点集是刚性鱼点集，Y点集是非刚性鱼点集。本示例为刚性配准，因此将X赋给Y，并用一个随机生成的旋转矩阵对X点集进行变换，使两点集位置位置不同，进行测试。（注意，这里并不是对.mat文件中的数据集进行修改，而是用将.mat中的数据赋给变量X和Y）

        Transform=cpd_register(X,Y,opt)这一句代码，是传入两个点云数据，调用cpd_register函数，Transform是cpd_register函数返回的空间变换的参数结构体，包含旋转矩阵，平移矩阵，缩放比例等等。

        打开cpd_register函数代码，前面一大段用于初始化变量，例如是否使用刚性配准，最大迭代次数，定义误差阈值，定义离群点权重等等等等。这些参数会传入cpd_register函数中调用的cpd_rigid函数，rigid这个函数用于计算待配准点云向参考点云移动的旋转、平移等矩阵。

        cpd_rigid函数中的算法步骤即为论文中的算法步骤

        在cpd_rigid函数中，由于计算pmn需要每个点之间的距离，因此调用了cpd_P函数，这个函数即前文提到的用c语言写的函数。在cpd_rigd函数通过上面论文中的算法步骤计算出旋转矩阵R，平移矩阵t，缩放比例s，协方差σ²后，将参数返回给cpd_register函数，同时对Y矩阵进行相应的变换，逐次迭代。

 

四、算法中的公式与代码中的变量的对应

        在最初学习CPD算法以及使用这个工具包的代码时，特别痛苦的一件事就是我搞不明白算法的数学公式是怎么对应到代码的实现中的。下面我就写一写我的理解。

        首先从cpd_P.c中的代码开始。这部分的代码是整个算法计算的开始。因为CPD虽然是一个基于概率匹配的算法，但是计算概率时仍是以距离进行计算的。

        向cpd_P.c中传入点集X和点集Y的数组。假设是平面点集配准，则D=2（若三维点集配准则D=3），X点集的数组规模为N×2，Y点集的数组规模为M×2。若以下图为例，则X点集的规模为4×2，Y点集规模为3×2。

        cpd_P.c中的代码多看几遍就能理解其意思了。cpd_P.c计算了点与点之间的距离，并存入数组中。以及刚性点集配准算法流程中的P·1和P^T·1也是直接在cpd_P.c中进行了计算。cpd_P.c中的*P存放的是exp(d²/-2σ²)，*P为一个M×1的一位数组。

        这里我直接放上我当时做的一个笔记，如下图：

        通过上面的笔记，就可以很清楚的理解cpd_P.c中计算的P1和Pt1数组中的内容是什么了。但是cpd_P.c中的Px是如何写成代码中的那种实现方式，这一点我没有推导出来，如果有人看明白希望可以解答。

        可能整篇文章写的有点乱，总之先发出来，这也是我的一点粗浅的个人理解。