PCL点云配准（ICP）

#include   // for pcl::make_shared
#include 
#include 
#include 
#include 

#include 
#include 

#include 

#include 
#include 
#include 

#include 

#include 
#include 
#include 

using namespace pcl;
using namespace std;

int main()
{
	pcl::PointCloud::Ptr init_trans_cloud(new pcl::PointCloud());
	pcl::PointCloud::Ptr cloud_model(new pcl::PointCloud());
	pcl::io::loadPCDFile("station_cloud1.pcd", *init_trans_cloud);
	pcl::io::loadPCDFile("station_cloud_model.pcd", *cloud_model);

	clock_t startTime, endTime;
	startTime = clock();

	//IterativeClosestPoint icp;
	//icp.setInputSource(init_trans_cloud);    //设置CAD点云 //cloud_model
	//icp.setInputTarget(cloud_model); //
	0.01 (scale 0.001) 10 (scale 1) 
	The target is 100mm in size and the correspondence needs to be set to about 5-10mm (init trans between centroid needed)
	//icp.setMaxCorrespondenceDistance(5); // Set the max correspondence distance 设置对应点对之间的最大距离（此值对配准结果影响较大）
	//icp.setMaximumIterations(5);      //Set the maximum number of iterations (criterion 1) 最大迭代次数，icp是一个迭代的方法，最多迭代这些次（若结合可视化并逐次显示，可将次数设置为1）
	//icp.setTransformationEpsilon(1e-10);//Set the transformation epsilon (criterion 2) 设置两次变化矩阵之间的差值（一般设置为1e-10即可）
	//icp.setEuclideanFitnessEpsilon(1e-5);//Set the euclidean distance difference epsilon (criterion 3) 设置收敛条件是均方误差和小于阈值， 停止迭代

	//pcl::PointCloud::Ptr cloudFinal(new pcl::PointCloud);
	//icp.align(*cloudFinal);
	//string convFlag = icp.hasConverged() ? " " : " NOT ";
	//cout << "ICP has" << convFlag << "converged. Score: " << icp.getFitnessScore() << endl;

	//endTime = clock();

	//double curTimeLen = (double)(endTime - startTime) / CLOCKS_PER_SEC;
	//cout << "TimeLen: " << curTimeLen << endl;

	pcl::io::savePCDFile("station_cabin_icp_new.pcd", *cloudFinal);          //     0.137119

	//if (0)
	//{
	//	boost::shared_ptr view(new visualization::PCLVisualizer);
	//	//view->addPointCloud(cloudCAD);
	//	view->setBackgroundColor(255, 255, 255);
	//	view->addCoordinateSystem(0.2);

	//	visualization::PointCloudColorHandlerCustom handlerSource(cloudFinal, 90, 190, 250);
	//	view->addPointCloud(cloudFinal, handlerSource, "a");
	//	view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "a");

	//	visualization::PointCloudColorHandlerCustom handlerTarget(cloud_model, 250, 190, 90);
	//	view->addPointCloud(cloud_model, handlerTarget, "b");
	//	view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "b");

	//	while (!view->wasStopped())
	//		view->spinOnce(100);
	//}

	//法向量计算
	pcl::NormalEstimation n;
	pcl::PointCloud::Ptr normals(new pcl::PointCloud);
	//建立kdtree来进行近邻点集搜索
	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
	//为kdtree添加点云数据
	tree->setInputCloud(init_trans_cloud);

	n.setInputCloud(init_trans_cloud);
	n.setSearchMethod(tree);
	//点云法向计算时，需要搜索的近邻点大小
	n.setKSearch(20);
	//开始进行法向计算
	n.compute(*normals);
	//* normals should not contain the point normals + surface curvatures
	//将点云数据与法向信息拼接
	pcl::PointCloud::Ptr init_trans_cloud_normals(new pcl::PointCloud);
	pcl::concatenateFields(*init_trans_cloud, *normals, *init_trans_cloud_normals);


	pcl::PointCloud::Ptr normals1(new pcl::PointCloud);
	tree->setInputCloud(cloud_model);
	n.setInputCloud(cloud_model);
	n.setSearchMethod(tree);
	n.compute(*normals1);
	pcl::PointCloud::Ptr cloud_model_normals(new pcl::PointCloud);
	pcl::concatenateFields(*cloud_model, *normals1, *cloud_model_normals);

	//点到面的距离函数
	pcl::IterativeClosestPoint icp;
	typedef pcl::registration::TransformationEstimationPointToPlaneLLS PointToPlane;
	const std::shared_ptr point_to_plane(new PointToPlane);       //ICP has converged. Score: 0.134252
	//typedef pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS SymmPointToPlane;
	//const std::shared_ptr point_to_plane(new SymmPointToPlane);   //ICP has converged.Score: 0.134183
	icp.setTransformationEstimation(point_to_plane);

	icp.setInputSource(init_trans_cloud_normals);
	icp.setInputTarget(cloud_model_normals);

	//0.01 (scale 0.001) 10 (scale 1) 
	//The target is 100mm in size and the correspondence needs to be set to about 5-10mm (init trans between centroid needed)
	icp.setMaxCorrespondenceDistance(5); // Set the max correspondence distance 设置对应点对之间的最大距离（此值对配准结果影响较大）
	icp.setMaximumIterations(2);      //Set the maximum number of iterations (criterion 1) 最大迭代次数，icp是一个迭代的方法，最多迭代这些次（若结合可视化并逐次显示，可将次数设置为1）
	icp.setTransformationEpsilon(1e-10);//Set the transformation epsilon (criterion 2) 设置两次变化矩阵之间的差值（一般设置为1e-10即可）
	icp.setEuclideanFitnessEpsilon(1e-5);//Set the euclidean distance difference epsilon (criterion 3) 设置收敛条件是均方误差和小于阈值， 停止迭代

	pcl::PointCloud::Ptr cloudFinal(new pcl::PointCloud);
	icp.align(*cloudFinal);
	string convFlag = icp.hasConverged() ? " " : " NOT ";
	cout << "ICP has" << convFlag << "converged. Score: " << icp.getFitnessScore() << endl;

	endTime = clock();
	double curTimeLen = (double)(endTime - startTime) / CLOCKS_PER_SEC;
	cout << "TimeLen: " << curTimeLen << endl;

	//pcl::io::savePCDFile("station_cabin_icp_new.pcd",*cloudFinal);

	if (1)
	{
		boost::shared_ptr view(new visualization::PCLVisualizer);
		//view->addPointCloud(cloudCAD);
		view->setBackgroundColor(255, 255, 255);
		view->addCoordinateSystem(0.2);

		visualization::PointCloudColorHandlerCustom handlerSource(cloudFinal, 90, 190, 250);
		view->addPointCloud(cloudFinal, handlerSource, "a");
		view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "a");

		visualization::PointCloudColorHandlerCustom handlerTarget(cloud_model_normals, 250, 190, 90);
		view->addPointCloud(cloud_model_normals, handlerTarget, "b");
		view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "b");

		while (!view->wasStopped())
			view->spinOnce(100);
	}


	return 0;
}