1. 4PCS算法定义4PCS4-Points Congruent Sets四点一致集是一种经典的三维点云粗配准算法由 Aiger、Mitra 和 Cohen-Or 于 2008 年提出。其核心思想是在源点云中选取四个近似共面的点作为基准集合然后利用刚体变换过程中点之间的几何关系保持不变这一特性在目标点云中搜索与该基准集合具有相似几何结构的四点集合并根据对应的四点集合计算两组点云之间的刚体变换。与ICP依赖初始位姿以及最近点对应关系不同4PCS主要利用点云的几何结构进行全局搜索因此不要求源点云和目标点云具有较好的初始对齐状态特别适合作为ICP等精配准算法的前置粗配准方法。原始4PCS方法通过宽基底和共面四点集合降低错误匹配的影响并能够处理一定程度的噪声、离群点和部分重叠点云2. 4PCS算法具体步骤步骤1输入源点云和目标点云首先输入待配准的源点云 P 和目标点云 Q。与ICP不同4PCS不要求两组点云在初始状态下已经具有较高的空间重合度因此可以用于初始位姿未知的点云粗配准。步骤2选择四点基准集合从源点云中选择四个近似共面的点B {b₁, b₂, b₃, b₄}这四个点构成4PCS算法的基准集合。通常希望四个点之间具有较大的空间跨度即形成较宽的基准结构这样能够提高刚体变换估计的稳定性。步骤3提取几何不变量由于刚体变换不会改变点之间的距离关系因此可以利用四点之间的几何关系作为匹配依据。例如选择两组点对d₁ ‖b₁ − b₂‖d₂ ‖b₃ − b₄‖在目标点云中搜索距离分别接近 d₁ 和 d₂ 的点对从而减少需要进行组合匹配的候选点数量。原始4PCS进一步利用共面四点的交点比例等仿射不变量寻找潜在的一致四点集合。步骤4搜索目标点云中的一致四点集合根据源点云基准集合的几何约束在目标点云中寻找满足相似几何关系的四点集合U {u₁, u₂, u₃, u₄}如果源点云中的四点集合和目标点云中的四点集合在允许误差范围内具有一致的几何结构则认为两组四点可能存在对应关系。步骤5计算候选刚体变换建立四点之间的对应关系后根据对应点集合计算旋转矩阵 R 和平移向量 t使uᵢ ≈ Rbᵢ t从而得到一个候选刚体变换T [R t]对于每一个候选四点集合都可以计算一个对应的候选变换。步骤6验证候选变换将候选变换应用于源点云观察变换后的源点云与目标点云之间的重叠程度并计算对应误差。4PCS原始算法采用最大公共点集LCPLargest Common Pointset思想评价候选变换即判断经过当前变换后有多少源点能够在目标点云中找到距离小于给定阈值的对应点。匹配数量越多说明当前刚体变换越可靠。步骤7确定最优变换对所有候选变换进行比较选择具有最大重叠程度或最优匹配评分的变换作为最终粗配准结果3. 4PCS算法的缺点虽然4PCS具有不依赖初始位姿、能够进行全局粗配准以及对一定噪声和离群点具有较好鲁棒性等优点但仍然存在一些不足。首先计算量较大。4PCS需要在目标点云中搜索与源点云基准集合具有相似几何关系的候选四点集合当点云规模较大时候选组合数量会明显增加从而影响配准速度。原始论文给出的核心搜索过程在一般情况下约为 O(n²k)其中 n 为候选点数量k 为输出的候选四点集合数量。其次算法对点云重叠区域具有一定要求。当两组点云重叠区域较小时选取的四点基准可能无法全部位于两组点云的共同区域从而降低正确匹配被搜索到的概率。此外四点基准的选择会影响配准稳定性。较宽的四点基准通常能够提高变换估计的稳定性但在部分重叠点云中如果基准点之间的跨度过大又可能导致部分基准点位于非重叠区域从而无法建立正确对应关系。原始4PCS论文也指出部分匹配情况下基准宽度受到重叠区域范围的限制。最后对于结构重复或几何特征相似的场景可能产生多个具有相似几何关系的候选四点集合需要进一步进行验证和筛选从而增加计算时间。因此在实际应用中4PCS通常用于获得较好的初始位姿再结合ICP等精配准算法进一步提高最终配准精度。4. VS PCL代码实现本文采用Visual Studio 2017作为程序开发环境使用C语言结合PCL 1.8.1点云库实现4PCS类粗配准方法#include iostream #include string #include pcl/io/pcd_io.h #include pcl/point_types.h #include pcl/point_cloud.h #include pcl/registration/ia_fpcs.h #include pcl/common/transforms.h #include pcl/visualization/pcl_visualizer.h #include Eigen/Dense using namespace std; int main() { // // 1. 定义点云类型 // typedef pcl::PointXYZ PointT; pcl::PointCloudPointT::Ptr sourceCloud( new pcl::PointCloudPointT); pcl::PointCloudPointT::Ptr targetCloud( new pcl::PointCloudPointT); pcl::PointCloudPointT::Ptr alignedCloud( new pcl::PointCloudPointT); // // 2. 设置点云路径 // string sourcePath source.pcd; string targetPath target.pcd; // // 3. 读取源点云 // if (pcl::io::loadPCDFilePointT( sourcePath, *sourceCloud) -1) { cerr 无法读取源点云 endl; cerr 文件路径 sourcePath endl; system(pause); return -1; } // // 4. 读取目标点云 // if (pcl::io::loadPCDFilePointT( targetPath, *targetCloud) -1) { cerr 无法读取目标点云 endl; cerr 文件路径 targetPath endl; system(pause); return -1; } // // 5. 输出点云信息 // cout endl; cout 4PCS点云粗配准程序 endl; cout endl; cout 源点云点数 sourceCloud-size() endl; cout 目标点云点数 targetCloud-size() endl; // // 6. 创建FPCS对象 // pcl::registration::FPCSInitialAlignment PointT, PointT fpcs; // // 7. 设置输入点云 // fpcs.setInputSource(sourceCloud); fpcs.setInputTarget(targetCloud); // // 8. 设置FPCS参数 // // 最大迭代次数 fpcs.setMaximumIterations(1000); // RANSAC迭代次数 fpcs.setNumberOfSamples(4); // // 9. 执行4PCS/FPCS粗配准 // cout endl; cout 开始执行4PCS粗配准... endl; fpcs.align(*alignedCloud); // // 10. 判断是否收敛 // if (!fpcs.hasConverged()) { cout endl; cout 4PCS配准失败 endl; system(pause); return -1; } // // 11. 获取最终变换矩阵 // Eigen::Matrix4f transformation fpcs.getFinalTransformation(); // // 12. 输出变换矩阵 // cout endl; cout endl; cout 4PCS配准结果 endl; cout endl; cout 最终变换矩阵 endl; cout transformation endl; // // 13. 获取旋转矩阵 // Eigen::Matrix3f rotation transformation.block3, 3(0, 0); cout endl; cout 旋转矩阵 R endl; cout rotation endl; // // 14. 获取平移向量 // Eigen::Vector3f translation transformation.block3, 1(0, 3); cout endl; cout 平移向量 t endl; cout translation endl; // // 15. 输出Fitness Score // cout endl; cout Fitness Score fpcs.getFitnessScore() endl; // // 16. 保存粗配准结果 // string outputPath 4pcs_aligned.pcd; pcl::io::savePCDFileBinary( outputPath, *alignedCloud); cout endl; cout 配准后的点云已经保存 endl; cout outputPath endl; // // 17. 创建可视化窗口 // pcl::visualization::PCLVisualizer viewer( 4PCS Point Cloud Registration); // // 18. 设置背景 // viewer.setBackgroundColor( 0.05, 0.05, 0.05); // // 19. 显示目标点云 // pcl::visualization::PointCloudColorHandlerCustomPointT targetColor( targetCloud, 0, 255, 0); viewer.addPointCloudPointT( targetCloud, targetColor, target); viewer.setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, target); // // 20. 显示4PCS配准后的源点云 // pcl::visualization::PointCloudColorHandlerCustomPointT sourceColor( alignedCloud, 255, 0, 0); viewer.addPointCloudPointT( alignedCloud, sourceColor, source); viewer.setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, source); // // 21. 添加坐标系 // viewer.addCoordinateSystem(1.0); viewer.initCameraParameters(); // // 22. 显示结果 // while (!viewer.wasStopped()) { viewer.spinOnce(100); } // // 23. 程序结束 // cout endl; cout 4PCS配准程序执行完成 endl; system(pause); return 0; }