点云配准方法：ICP与GICP

ICP已经成为点云配准的主流算法，因此，这两天测试了PCL库中的两个ICP函数：

• pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >

• pcl::IterativeClosestPoint< PointSource, PointTarget > 

GICP的用法和ICP类似，照葫芦画瓢即可。为了加快配准速度，程序中对点云做了半径和体素滤波处理。

经过多次实验，可以得出如下结论：

1. 无论是在配准速度还是精度上，ICP的配准效果好于GICP；
2. ICP 的配准速度受最大迭代次数影响，次数越多，耗时越长，GICP则不受最大迭代次数的影响；
3. 在点云配准之前，是可以先进行滤波处理的，尤其是经过半径滤波和体素滤波，配准速度提升了6倍左右，精度则未受影响；
4. 迭代的次数并不是越大越好，超过一定次数之后，配准精度提升微小；
5. 即使设置了约束条件（ setMaximumIterations，setMaxCorrespondenceDistance，setEuclideanFitnessEpsilon ），出来的结果并不是最好的。

程序如下：

#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/filters/filter.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/registration/gicp.h>
#include <pcl/common/time.h>
#include <pcl/filters/voxel_grid.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main(int argc, char** argv)
{
	// 创建点云指针
	PointCloudT::Ptr cloud_in(new PointCloudT);  // 需要转换的点云
	PointCloudT::Ptr cloud_tr(new PointCloudT);  // 转换后的点云
	PointCloudT::Ptr cloud_icped(new PointCloudT);  // ICP后输出点云
	PointCloudT::Ptr cloud_icped1(new PointCloudT);  // ICP后输出点云
	PointCloudT::Ptr cloud_aim(new PointCloudT); // 向其转换的目标点云
	PointCloudT::Ptr cloud_in_src(new PointCloudT);
	PointCloudT::Ptr cloud_aim_src(new PointCloudT);
	PointCloudT::Ptr cloud_in_rad(new PointCloudT);
	PointCloudT::Ptr cloud_aim_rad(new PointCloudT);
	pcl::StopWatch timeer;

	//读取pcd文件
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("test_pcd_seg_0.pcd", *cloud_aim_src) == -1)
	{
		PCL_ERROR("Couldn't read file1 \n");
		return (-1);
	}
	std::cout << "Loaded " << cloud_aim_src->size() << " data points from file1" << std::endl;
	
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("test_pcd_seg_1.pcd", *cloud_in_src) == -1)
	{
	PCL_ERROR("Couldn't read file2 \n");
	return (-1);
	}
	std::cout << "Loaded " << cloud_in_src->size() << " data points from file2" << std::endl;
	
	std::vector<int> mapping_in;
	std::vector<int> mapping_out;
	pcl::removeNaNFromPointCloud(*cloud_in_src, *cloud_in_src, mapping_in);
	pcl::removeNaNFromPointCloud(*cloud_aim_src, *cloud_aim_src, mapping_out);
	
	// Perform the actual filtering
	pcl::RadiusOutlierRemoval<PointT> outrem;
	// build the filter
	outrem.setInputCloud(cloud_in_src);
	outrem.setRadiusSearch(0.04);
	outrem.setMinNeighborsInRadius(80);
	// apply filter
	outrem.filter(*cloud_in_rad);

	// build the filter
	outrem.setInputCloud(cloud_aim_src);
	outrem.setRadiusSearch(0.04);
	outrem.setMinNeighborsInRadius(80);
	// apply filter
	outrem.filter(*cloud_aim_rad);

	// downsample clouds
	pcl::VoxelGrid<PointT> vg;
	vg.setInputCloud(cloud_in_rad);
	vg.setLeafSize(0.01f, 0.01f, 0.01f);
	vg.filter(*cloud_in);

	vg.setInputCloud(cloud_aim_rad);
	vg.setLeafSize(0.01f, 0.01f, 0.01f);
	vg.filter(*cloud_aim);
		
	timeer.reset();
	//icp配准
	//pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //创建ICP对象，用于ICP配准
	//pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	//icp.setMaximumIterations(500);    //设置最大迭代次数iterations=true
	//icp.setMaxCorrespondenceDistance(0.5);
	//icp.setTransformationEpsilon(1e-6);
	//icp.setEuclideanFitnessEpsilon(1);
	//icp.setInputCloud(cloud_in); //设置输入点云
	//icp.setInputTarget(cloud_aim); //设置目标点云（输入点云进行仿射变换，得到目标点云）
	//icp.align(*cloud_icped1);          //匹配后源点云

	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp2; //创建ICP对象，用于ICP配准
	//icp2.setEuclideanFitnessEpsilon(1);
	icp2.setMaximumIterations(200);
	icp2.setInputCloud(cloud_in); //设置输入点云
	icp2.setInputTarget(cloud_aim); //设置目标点云（输入点云进行仿射变换，得到目标点云）
	icp2.align(*cloud_icped);          //匹配后源点云
	
	//icp.setMaximumIterations(1);  // 设置为1以便下次调用
	//std::cout << "Applied " << iterations << " ICP iteration(s)" << std::endl;
	if (icp2.hasConverged())//icp.hasConverged ()=1（true）输出变换矩阵的适合性评估
	{
		std::cout << "\nICP has converged, score is: " << icp2.getFitnessScore() << std::endl;
		std::cout << "\nICP has converged, Epsilon is: " << icp2.getEuclideanFitnessEpsilon() << std::endl;
		//std::cout << "\nICP transformation " << iterations << " : cloud_in -> cloud_aim" << std::endl;
		std::cout << "\nICP transformation is \n " << icp2.getFinalTransformation() << std::endl;
		//transformation_matrix = icp.getFinalTransformation().cast<double>();
		//print4x4Matrix(transformation_matrix);
	}
	else
	{
		PCL_ERROR("\nICP has not converged.\n");
		return (-1);
	}

	std::cout << "ICP run time: " << timeer.getTimeSeconds() << " s" << std::endl;
	//可视化
	pcl::visualization::PCLVisualizer viewer("ICP demo");
	// 创建两个观察视点
	int v1(0);
	int v2(1);
	viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	// 定义显示的颜色信息
	float bckgr_gray_level = 0.0;  // Black
	float txt_gray_lvl = 1.0 - bckgr_gray_level;
	// 原始的点云设置为白色的
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_aim_color_h(cloud_aim, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,
		(int)255 * txt_gray_lvl);

	viewer.addPointCloud(cloud_aim, cloud_aim_color_h, "cloud_aim_v1", v1);//设置原始的点云都是显示为白色
	viewer.addPointCloud(cloud_aim, cloud_aim_color_h, "cloud_aim_v2", v2);

	// 需要被转换的点云显示为绿色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, 20, 180, 20);
	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);

	// ICP配准后的点云为红色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icped_color_h(cloud_icped, 180, 20, 20);
	viewer.addPointCloud(cloud_icped, cloud_icped_color_h, "cloud_icped_v2", v2);

	// 加入文本的描述在各自的视口界面
	//在指定视口viewport=v1添加字符串“white 。。。”其中"icp_info_1"是添加字符串的ID标志，（10，15）为坐标16为字符大小 后面分别是RGB值
	viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
	viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);


	// 设置背景颜色
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

	// 设置相机的坐标和方向
	viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
	viewer.setSize(1280, 1024);  // 可视化窗口的大小

	//显示
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return 0;
}