ICP配准算法研究

       最近，在学习PCL当中的ICP配准算法，对点云数据进行配准。整体来讲，迭代最近点算法是已经很成熟的配准算法，但是，仍有相关研究者对其进行进一步的改进。在我最近阶段看的文章中，对于改进的该算法的方面重在于：①直接缩短ICP算法运行的时间；②.改进相应的分割算法，为后续ICP配准缩短时间；③.改进初始配准算法，使得源点云与目标点云之间的距离更进，ICP在配准过程中，迭代次数大大减少；④.改进拓扑关系（K-d树、Octree）等等。但是，对ICP本身算法的改进少。个人认为，ICP算法已相当成熟，若能够开发出基于PCL的ICP算法的动态调试参数的方法，就能自动算出最优解。这是很理想的，但是难度大。若有哪位博友想做动态参数计算设计算法的，我们可以合作。

      以下，是我最近做的一部分ICP算法的最基本的代码程序，希望各位博友能指点。其中，源点云与目标点云我已经预处理过了，所以相应的预处理代码就没有上传到这篇博文中。

      在程序中，显示那块，出现了问题，不知是我的vtk有问题，还是哪里。但是程序运行编译是没有任何问题的，VS13+PCL1.8。

      源点云与目标点云我不能打包成文件上传，各位博友研究者可以联系我，再发给各位吧。

      在程序的最后，我将配准完以后的点云，存储在“C://Users//YCJ//Desktop//pcd文件夹//aaaaa.pcd”这里，但是最后用Cloudcompare显示的时候，确只是显示了一个点云，按理说应该是两幅配准完以后的完整点云，我就很不明白，这是为什么？难道不能这么写么？还是说icp.align(*cloud_source_registration);这一步当中，最后配准完以后的点云没有存储在cloud_source_registration当中？我也希望各位博友能提出自己的解决方案，共同完成并优化程序。

#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<pcl\visualization\cloud_viewer.h>


int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source_registration(new pcl::PointCloud<pcl::PointXYZRGB>);


	// 检查参数
	/*if(argc != 3) {
	std::cout << "ERROR: the number of arguments is illegal!" << std::endl;
	return -1;
	}*/

	// 载入点云文件
	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("C://Users//YCJ//Desktop//pcd文件夹//rabbit_t11.pcd", *cloud_source) == -1) { 
		std::cout << "load source failed!" << std::endl;                //载入源点云
		return -1;
	}
	std::cout << "source loaded!" << std::endl;

	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("C://Users//YCJ//Desktop//pcd文件夹//rabbit12.pcd", *cloud_target) == -1) { 
		std::cout << "load target failed!" << std::endl;                //载入目标点云
		return -1;
	}
	std::cout << "target loaded!" << std::endl;

	// ICP配准
	pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;       //设置迭代最近点的对象icp
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZRGB>);
	tree1->setInputCloud(cloud_source);                                  //设置源点云的kd-tree
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZRGB>);
	tree2->setInputCloud(cloud_target);                                  //设置目标点云的kd-tree
	icp.setSearchMethodSource(tree1);        //设置源点云的搜索方法
	icp.setSearchMethodTarget(tree2);        //设置目标点云的搜索方法
	icp.setInputSource(cloud_source);        //输入源点云
	icp.setInputTarget(cloud_target);        //输入目标点云
	icp.setMaxCorrespondenceDistance(1500);   //设置最大的对应点距离
	icp.setTransformationEpsilon(1e-10);      //设置精度
	icp.setEuclideanFitnessEpsilon(0.01);
	icp.setMaximumIterations(300);            //设置最大的迭代次数
	icp.align(*cloud_source_registration);    //开始配准
	std::cout << "获取最终的刚体变换矩阵：" << std::endl;
	Eigen::Matrix4f transformation = icp.getFinalTransformation();
	std::cout << transformation << std::endl;

	pcl::PCDWriter writer;
	writer.write<pcl::PointXYZRGB>("C://Users//YCJ//Desktop//pcd文件夹//aaaaa.pcd", *cloud_source_registration, false);

	//pcl::visualization::CloudViewer viewer("cloud_source_registration");
	//viewer.showCloud(cloud_source_registration);

	// 显示
	//pcl::visualization::PCLVisualizer p;
	//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_r_h(cloud_source_registration, 255, 0, 0);
	//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(cloud_target, 0, 255, 0);
	//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(cloud_source, 0, 0, 255);
	//p.addPointCloud(cloud_source_registration, src_r_h, "source_r");
	//p.addPointCloud(cloud_target, tgt_h, "target");
	//p.addPointCloud(cloud_source, src_h, "source");
	//p.spin();
	return 0;
}

      运行结果如下所示：

      编译成功

      运行成功

 

     运行结果