PCL ICP算法实现点云精配准

算法原理

代码实现

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>  
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>   // pcl中计算程序运行时间有很多函数，其中利用控制台的时间计算
using namespace std;
int
	main (int argc, char** argv)
{
	pcl::console::TicToc time;
	// 加载第一次扫描点云数据作为目标云
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>("G.pcd", *target_cloud);
	

	// 加载从新视角得到的第二次扫描点云数据作为源点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>("G1.pcd", *input_cloud);
	
	cout << "从目标点云中读取 " << target_cloud->size() << " 个点" << endl;
	cout << "从源点云中读取 " << input_cloud->size () << " 个点" << endl;
	time.tic ();
	//初始化ICP对象
	int iterations = 35;
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; // 根据输入数据的尺度设置ICP相关参数
	icp.setInputCloud (input_cloud);  //源点云
	icp.setInputTarget (target_cloud);  //目标点云
	icp.setTransformationEpsilon (1e-10);   //为终止条件设置最小转换差异
	icp.setMaxCorrespondenceDistance(100); //设置对应点对之间的最大距离（此值对配准结果影响较大）。
	icp.setEuclideanFitnessEpsilon(0.1);  //设置收敛条件是均方误差和小于阈值， 停止迭代；
	icp.setMaximumIterations(iterations); //最大迭代次数，icp是一个迭代的方法，最多迭代这些次（若结合可视化并逐次显示，可将次数设置为1）；  
	
	// 计算需要的刚体变换以便将输入的源点云匹配到目标点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
	icp.align (*output_cloud);
	cout << "Applied " << 35 << " ICP iterations in " << time.toc() << " ms" << endl;
	cout << "\nICP has converged, score is " << icp.getFitnessScore() << endl;
	cout << "变换矩阵：\n" << icp.getFinalTransformation() << endl;
	// 使用创建的变换对为输入点云进行变换
	pcl::transformPointCloud (*input_cloud, *output_cloud, icp.getFinalTransformation ());

	// 保存转换后的源点云作为最终的变换输出
	//  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

	// 初始化点云可视化对象
	boost::shared_ptr<pcl::visualization::PCLVisualizer>
		viewer_final (new pcl::visualization::PCLVisualizer ("配准结果"));
	viewer_final->setBackgroundColor (0, 0, 0);  //设置背景颜色为黑色

	// 对目标点云着色可视化 (red).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		target_color (target_cloud, 255, 0, 0);
	viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
	viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "target cloud");
	// 对源点云着色可视化 (blue).
	/*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		input_color(input_cloud, 0, 0, 255);
	viewer_final->addPointCloud<pcl::PointXYZ>(input_cloud, input_color, "input cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "input cloud");
		*/
	// 对转换后的源点云着色 (green)可视化.
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		output_color (output_cloud, 0, 255, 0);
	viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
	viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "output cloud");
		
	while (!viewer_final->wasStopped ())
	{
		viewer_final->spinOnce (100);
		boost::this_thread::sleep (boost::posix_time::microseconds (100000));
	}

	return (0);
}

实验结果

官网链接pcl::IterativeClosestPoint