PCL ICP算法实现点云精配准（自生成点云数据）

(自学笔记，防遗忘）

  PCL1.8.1完成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>

int main(int argc, char** argv)
{

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

	// Fill in the CloudIn data
	cloud_in->width = 1000;
	cloud_in->height = 1;
	cloud_in->is_dense = false;
	cloud_in->points.resize(cloud_in->width * cloud_in->height);
	for (size_t i = 0; i < cloud_in->points.size(); ++i)
	{
		cloud_in->points[i].x =  5*rand() / (RAND_MAX + 1.0f);
		cloud_in->points[i].y =  5*rand() / (RAND_MAX + 1.0f);
		cloud_in->points[i].z =  5*rand() / (RAND_MAX + 1.0f);
	}

	*cloud_out = *cloud_in;
	//对源点云施加刚性变换，得到目标点云
	for (size_t i = 0; i < cloud_in->points.size(); ++i)
	{
		cloud_out->points[i].x = cloud_in->points[i].x + 15.0f;
		cloud_out->points[i].y = cloud_in->points[i].y + 8.0f;
		cloud_out->points[i].z = cloud_in->points[i].z + 5.0f;
	}
		

	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	// 设置源点云和目标点云
	icp.setInputCloud(cloud_in);
	icp.setInputTarget(cloud_out);
	icp.setMaxCorrespondenceDistance(30);//设置对应点之间的最大差距，对结果影响较大。大于这个距离的点将会被忽略掉
	icp.setMaximumIterations(30);//设置最大迭代次数
	icp.setTransformationEpsilon(1e-10);//设置终止条件的最小转换差异
	icp.setEuclideanFitnessEpsilon(0.01);//设置收敛的条件是均方误差和小于这个数值，则认为匹配完成
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr Final(new pcl::PointCloud<pcl::PointXYZ>);//定义一个点云，以便将源点云进行刚性变换，和目标点云进行匹配
	//调用ICP进行匹配
	icp.align(*Final);
	// 获取匹配的刚性变换矩阵
	Eigen::Matrix4f transformation = icp.getFinalTransformation();


	//如果两个点云正确匹配了，则认为收敛，hasConverged()==true 
	std::cout << "After ICP has converged: " << icp.hasConverged() << std::endl;

	//获得源点云到匹配的目标点云的欧氏距离之和
	std::cout << "score: " << icp.getFitnessScore() << std::endl;
	std::cout << "----------------------------------------------------------" << std::endl;

	//得到匹配结果，即旋转、平移矩阵
	std::cout << icp.getFinalTransformation() << std::endl;



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

	// 对目标点云着色可视化 (red).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>	cloud_outColor(cloud_out, 255, 0, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(cloud_out, cloud_outColor, "cloud_outColor");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1, "cloud_outColor");
	// 对源点云着色可视化 (blue).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>	cloud_inColor(cloud_in, 0, 0, 255);
	viewer_final->addPointCloud<pcl::PointXYZ>(cloud_in, cloud_inColor, "cloud_inColor");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_inColor");
	// 对转换后的源点云着色 (green)可视化.
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>	finalColor(Final, 0, 255, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(Final, finalColor, "finalColor");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "finalColor");

	while (!viewer_final->wasStopped())
	{
		viewer_final->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}


	system("pause");
	return (0);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91