点云粗配准4PCS及相应变种+在R稠密三维重建场景中的应用效果

一、4PCS简介：

4PCS（4 point congruent sets）算法是一种点云粗配准的算法。适用于重叠区域较小的两帧点云图（深度图）进行粗配准。传统的ICP算法需要两帧点云有较大的重叠区域。

4PCS算法的基本思想，来源于RANSAC算法，从宏观上讲，其基本框架也和RANSAC算法的基本也基本一致。
RANSAC算法是随机选取三个点，按照预先设定**的数学公式或者其他规则，**进行建模，然后判断其他点和该模型的关系，在该模型上的点成为局内点，不在该模型上的点成为局外点，当局内点数量高于一定条件时，我们就可以得到该具体的模型，和在模型上的点。

但是对于实际的点云配准的任务中，是将两个有一定重叠的两幅点云进行检测重叠，现实场景中重复部分的点云不可能都满足数学公式。所以4PCS在源点云（点云配准时，可以将第一帧成为源点云，第二帧成为目标点云）**利用四个点创造一种特点的规则，然后在目标点云中寻找满足这种规则的点。**如果找对对应点，进行粗配准，计算两帧点云变换的变换矩阵T，计算粗配准后的两幅点云的重合程度，当重合程度足够好的时候，我们认为完成了粗配准。，

所以利用RANSAC算法思路来完成点云配准工作的最关键的部分是如何利用源点云中的点创造一种有效的规则。 4PCS就是自行创造这种有效的规则。

二、4PCS算法：

4PCS算法中，利用的有效规则是，共面的四个点，两两相连后，会形成一个交点，根据这个交点，我们可以计算出上式的两个比例，当点云发生刚性变换时，这个两个比例关系肯定不会发生变化。当两帧点云配准成功时，在重叠区域中一定会存在类似的四个点。我们按照这个比例相等，同时连线距离相等，这两个规律当成匹配的有效规则。

具体做法：

4PCS算法中，首先在源点云中随机选择四个共面不共线的四个点。分别连接a，b和成c，d（能形成交点的对角线上的两个点），相交于点e，利用公式
计算两个比率。
在目标点云中，将所有点两两相连，根据这两个比率，分别在每条连接线上计算可能存在的交点的e1和e2，

若在目标点云中，存在e1和e2在空间中位置是相等的，如图（b）所示中的，q1,q4,q3,q5,同时，q1和q4之间的距离和源点云上的a和c之间的距离相等，q3和q5之间的距离和源点云中的c，d之间的距离相等，我们认为在目标点云中成功找到源点云｛a,b,c,d｝对应点｛q1,q2,q3,q4｝

进而将两个点云按照，对应点｛a,b,c,d｝和｛q1,q2,q3,q4｝之间的关系进行刚性变换，计算源点云和经过变换矩阵T变换后的目标点云中，最近点距离小于某个阈值的点的数目，该数目用来表征变换矩阵T的质量，然后迭代上面的四个步骤，至到T的质量最佳。此时的T就是两幅点云之间的粗配准的结果。

三、关键变种Sper4PCS的优化方法：

（1）在源点云中选取四个共面但不共线的四个点时，遵循最大距离原则。
根据预估的两幅点云之间的重叠率计算一个最大距离的阈值。在选择点时，使得点与点之间的距离较大但又不超过某个阈值。减少不必要的点与点之间的运算。
此时已经选取了四个共面不共线的点｛a,b,c,d｝
（2）在目标点云中寻找点时，按照球形方式进行点的预匹配。
在目标点云中，首先选择一个点q1，分别以a，b之间的距离r1， c，d之间的距离r2为半径画三维的球，选择在该球上的点作为可能的q2或者q3的点，再分别连接q
1和其他点，在该连接线上分别按照比率来计算e1和e2。
（3）在剔除错误点时，引入连接交线之间的角度，进而提高剔除错误点的速度。

四、其他变种：

4PCS算法能够应对复杂场景的点云配准任务，一系列改进算法应运而生，其中Super4PCS[2]算法采用智能索引策略，将4PCS算法计算复杂度由o（n2）降到o（n），其中n为点的数量，算法可用于大场景点云配准工作；k-4PCS[3]算法针对原始算法中点云大幅度降采样不合理的问题，提出点云关键点检测策略，使用稀疏关键点代替原有算法随机采样点，进而完成点云高精度配准；semantic-keypoint 4PCS[4]算法针对城市建筑场景配准问题，首先分层提取建筑物语义关键点，然后使用语义点替代原始随机采样点进行点云配准；Generalized 4PCS[5]算法针对原始算法共面四点对构建过程进行改进，将共面四点基的构建一般化，不再严格限制四点共处一个平面，该方法极大的提高的点云配准的效率；Super Generalized 4PCS[6]算法是Super 4PCS算法和Generalized 4PCS算法的组合，在智能索引策略的基础上加入非共面优化，减少了一致四点基集合的匹配数量，从而进一步加速算法的运行速度；V4PCS[7]算法在构建共面四点基时加入体积一致理念，将共面四点匹配拓展到非共面四点，减少了算法计算复杂度，进而提高了算法的运算效率；MSSF-4PCS[8]算法在共面四点对匹配时加入法向量约束，减少了四点对匹配数量，从而加速算法运算速度，同时对4PCS算法得到的全等四点对优化，通过计算与匹配全等四点对R半径邻域内的点特征，进一步优化点对匹配结果，提高点云配准精度。2PNS[9]算法对Super4PCS算法进行了改进，仅使用空间两点间的拓扑关系及法向量来构建匹配规则，在无点噪声的场景中算法提速5.2倍，算法同时能够应对更小的重叠度场景配准（最小仅5%重叠度）。

Aiger, D.; Mitra, N.J.; Cohen-Or, D. 4-points congruent sets for robust pairwise surface registration. ACM Transactions on Graphics 2008, 27, 1-10.
Mellado N , Aiger D , Mitra N J . Super 4PCS Fast Global Pointcloud Registration via Smart Indexing[J]. Computer Graphics Forum, 2014, 33(5):205-215.
Theiler P W , Wegner J D , Schindler K . Markerless point cloud registration with keypoint-based 4-points congruent sets[C]// ISPRS Workshop Laser Scanning 2013. ISPRS Annals of Photogrammetry, Remote Sensing and Spatial Information Sciences, 2013.
Ge, Xuming. Automatic markerless registration of point clouds with semantic-keypoint-based 4-points congruent sets[J]. ISPRS Journal of Photogrammetry and Remote Sensing, 2017, 130:344-357.
Mohamad M , Rappaport D , Greenspan M . Generalized 4-Points Congruent Sets for 3D Registration[C]// 2014 2nd International Conference on 3D Vision. IEEE, 2015.
Mohamad M , Ahmed M T , Rappaport D , et al. Super Generalized 4PCS for 3D Registration[C]// 2015 International Conference on 3D Vision (3DV). IEEE Computer Society, 2015.
Huang J , Kwok T H , Zhou C . V4PCS: Volumetric 4PCS Algorithm for Global Registration[C]// ASME 2017 International Design Engineering Technical Conferences and Computers and Information in Engineering Conference. 2017.
Xu Z , Xu E , Zhang Z , et al. Multiscale Sparse Features Embedded 4-Points Congruent Sets for Global Registration of TLS Point Clouds[J]. IEEE Geoscience and Remote Sensing Letters, 2018, 16(2):286-290.
C. Raposo and J. P. Barreto, “Using 2 point+normal sets for fast registration of point clouds with small overlap,” 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 2017.

五、Super4PCS在稠密重建场景中的应用尝试

维护Super4PCS代码的地址：
https://github.com/STORM-IRIT/OpenGR
Super4PCS使用时根据输入点云形状密度等不同，需要进行相应参数的调整，调参可以参考：
https://storm-irit.github.io/OpenGR/a00012.html

当输入两帧的RGB-D数据空间位姿差距比较大时，通过设置合适的参数，确实能将这两帧数据进行一个较好的粗对齐，但是对于两帧位姿相邻比较近的点云，很难进行进一步的将两帧点云很好的对齐，还是需要进一步使用ICP等方法进行进一步精准对齐。

相关内容部分参考大佬：
https://blog.csdn.net/peach_blossom/article/details/80955343?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163141914116780265432180%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=163141914116780265432180&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_blogbaidu_landing_v2~default-3-80955343.pc_v2_rank_blog_default&utm_term=4PCS&spm=1018.2226.3001.4450