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

一.基于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