基于PCL的点云ICP近点对齐多线程提速的改进方法

本文介绍了在PCL 1.9.0版本上,如何通过多线程提升点云ICP(Iterative Closest Point)计算速度,特别是对于带法线的点云。作者提供了应用代码示例,并展示了多线程加速后的效果,速度提升了约3倍。此外,还提及了GPU CUDA加速的可能性。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

一.基于PCL的点云ICP近点对齐多线程提速

本例子是在PCL 1.9.0 版本上测试,无需修改源码,只需继承即可,改成多线程处理计算。在实际项目中大大提升了ICP的计算时间。这里我们只对带法线的点云做加速pcl::IterativeClosestPointWithNormals,因为带法线的点云ICP比不带法线的点云收敛更快,对齐精度更高。
下面是我的应用代码,跟平常使用没有什么区别:

            #include "CorrespondenceRejectorSurfaceNormalWithOMP.h"   //添加多线程去除不匹配法线角度的点头文件
            #include "CorrespondenceEstimationNormalShootingWithOMP.h"//添加多线程快速找点头文件
            pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> icp;
			float Normal_Angle_Threshold =60;//60度的余弦值,做阈值。法线夹角大于该阈值的不做对齐
			pcl::registration::TransformationEstimationPointToPlane<pcl::PointNormal, pcl::PointNormal>::Ptr  point_to_plane(new pcl::registration::TransformationEstimationPointToPlane<pcl::PointNormal, pcl::PointNormal>);
			pcl::registration::CorrespondenceRejectorSurfaceNormalWithOMP::Ptr rejector_normal(new pcl::registration::CorrespondenceRejectorSurfaceNormalWithOMP());
			rejector_normal->setThreshold(std::cos(M_PI * Normal_Angle_Threshold/ 180));//60度的余弦值,做阈值。法线夹角大于该阈值的不做对齐
			pcl::registration::CorrespondenceRejectorOneToOne::Ptr cor_rej_o2o(new pcl::registration::CorrespondenceRejectorOneToOne);
			pcl::registration::CorrespondenceEstimationNormalShootingWithOMP<pcl::PointNormal, pcl::PointNormal,pcl::PointNormal>::Ptr     correst(new pcl::registration::CorrespondenceEstimationNormalShootingWithOMP<pcl::PointNormal, pcl::PointNormal, pcl::PointNormal>);
 			//icp.setTransformationEstimation(point_to_plane);
 			icp.setCorrespondenceEstimation(correst);
			icp.addCorrespondenceRejector(rejector_normal);
			icp.addCorrespondenceRejector(cor_rej_o2o);   //只保留1对1匹配,并且距离最小的匹配点

二.对比

点云数量 迭代次数 计算时间
单线程 20万 12次 11秒
多线程 20万 12次 4秒
* GPU 20万 12次 0.2秒

这里看到当用多线程后,ICP有一定提速,提速为3倍的时间。
*备注:GPU为我用CUDA显卡加速ICP计算,这里仅作参考。

在这里插入图片描述
对齐效果图

三.增加的多线程代码

1.添加CorrespondenceRejectorSurfaceNormalWithOMP.h头文件

#pragma once
#include <pcl/registration/correspondence_rejection.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/correspondence_rejection_surface_normal.h>
namespace pcl
{
   
	namespace registration
	{
   
		class  CorrespondenceRejectorSurfaceNormalWithOMP : public CorrespondenceRejectorSurfaceNormal
		{
   
		public:
			CorrespondenceRejectorSurfaceNormalWithOMP()
			{
   
				//rejection_name_ = "CorrespondenceRejectorSurfaceNormalWithOMP";
			}
 
			virtual void getRemainingCorrespondences(
				const pcl::Correspondences& original_correspondences,
				pcl::Correspondences& remaining_correspondences)
			{
   
				if (!data_container_) {
   
					return;
				}
			 
				unsigned int number_valid_correspondences = 0;
				remaining_correspondences.resize(original_correspondences.size());

				pcl::Correspondences  TempCorrespondences;
				TempCorrespondences.resize(original_correspondences.size());
				//多线程去除大于threshold_阈值的匹配点
#pragma omp parallel 
### PCL点云数据结构对齐方法 在 Point Cloud Library (PCL) 中,点云数据结构的对齐主要通过一系列几何变换来实现。这些变换可以是平移、旋转或者两者的组合。为了完成这一过程,通常会采用迭代最算法(Iterative Closest Point, ICP)[^1]。 ICP 是一种广泛使用的用于配准两个三维点集的方法,在每次迭代过程中寻找最接对应关系并计算最优刚体转换矩阵以最小化误差距离。具体来说: - **初始化**:给定源点云(Source cloud) 和目标点云(Target cloud),初始猜测两者之间的相对位置姿态。 - **匹配阶段**:对于每一个来自源点云的数据点,在目标点云里找到最点(Nearest Neighbor Search)作为对应的假设匹配点对。 - **加权平均求解最佳变换参数**:利用所有已知匹配点对估计当前步下的全局位姿调整量(即线性方程组),从而更新源点云的位置状态向量。 - **重复上述操作直至收敛条件满足为止**:当连续两次迭代间的差异小于设定阈值时停止循环;否则继续执行下一轮优化直到达到最大允许次数或精度要求。 除了经典的 ICP 外,还有许多改进版本如 Generalized-ICP(G-ICP), Normal-Distributions Transform(NDT) 等可供选择,它们各自针对不同应用场景做了特定优化处理[^2]。 此外,为了提高效率和准确性,还可以考虑以下几点建议: - 使用降采样技术减少输入点的数量; - 设置合理的搜索半径范围限制查找空间大小; - 应用颜色信息辅助构建更鲁棒的目标函数形式; - 结合多尺度策略逐步细化最终结果质量。 ```cpp // C++ code example using PCL's implementation of the IterativeClosestPoint algorithm. #include <pcl/point_cloud.h> #include <pcl/io/pcd_io.h> #include <pcl/features/normal_3d.h> #include <pcl/keypoints/sift_keypoint.h> #include <pcl/filters/passthrough.h> #include <pcl/registration/icp.h> int main(int argc, char** argv){ // Load source and target point clouds from .pcd files or other sources... pcl::PointCloud<pcl::PointXYZ>::Ptr src(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr tgt(new pcl::PointCloud<pcl::PointXYZ>); // Perform preprocessing steps such as downsampling... // Create an instance of the IterativeClosestPoint class template with appropriate parameters set up. pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp; icp.setInputSource(src); icp.setInputTarget(tgt); // Execute alignment process between two sets of points by calling align() method on 'icp' object. pcl::PointCloud<pcl::PointXYZ> final_result; icp.align(final_result); std::cout << "Has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; return 0; } ```
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值