基于ICP算法点云部分与整体的配准

pcl点云库提供了完整的icp算法，能完成点云的配准，但是能找到的例子都是书上的demo：一个完整点云，使用旋转矩阵旋转了一个角度，然后利用icp迭代了几次可以实现完美的配准，这是因为本来就是一个点云，选点集坐标都一样， 这样的例子没有代表性，也解决不了实际的例子
我遇到的问题是这样的：一个物体由扫描仪扫描后重采样，重采样的间距一定，造成前端和后端两个部位的点云密度与其他部位不同，点云缺失，点云后端只有一圈两圈这样的数据点，在应用分割算法的时候，这里会出现分割不出来的情况，需要将后端这部分补全；思路就是将物体的后端重新扫描提取一部分，然后将这部分点云配准回原点云上去，实现整体与部分点云的配准，然后再做重影消除，下采样等算法处理，然后完成分割算法。
这里分享一个在源代码上稍作修改的方法，当然不能算是改进icp了
首先熟悉一下icp的原理
一个是source点云一个是target点云，实际就是求两片点云的旋转矩阵、平移向量，构成一个目标函数，把source点云能经过旋转平移重合到target点云上去；

知道原理之后，先来看图一是target
下图为source点云

source点云是重新扫描得到的，和target在一个坐标系下如下图左，