记录常见的配准方法（二）

#include //采样一致性
#include 
#include 
#include 
#include 
#include 
#include 
#include //
#include //
#include //icp配准
#include //可视化
#include //时间

using namespace std;

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
void visualize_pcd(PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{

	//创建初始化目标
	pcl::visualization::PCLVisualizer viewer("registration Viewer");



	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);
	viewer.setBackgroundColor(255, 255, 255);

	viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");
	viewer.addPointCloud(pcd_final, final_h, "final cloud");

	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}
int main()
{
	//--------------------加载待配准的两个点云文件
	// 1
	PointCloud::Ptr cloud_src_o(new PointCloud);
	pcl::PCDReader r1;
	r1.read("34.pcd", *cloud_src_o);
	// 2
	PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
	pcl::PCDReader r2;
	r2.read("12.pcd", *cloud_tgt_o);

	clock_t start = clock();

	//--------------------去除nan点--------------------//
	// 1 
	cerr << "去除  cloud_src_o nan点" << endl;
	vector<int> indices_src; //保存去除的点的索引
	pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);
	// 2
	cout << "去除  cloud_tgt_o nan点" << endl;
	vector<int> indices_tgt;
	pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);

	//--------------------下采样精简--------------------//
	// 1
	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_1; //创建体素滤波对象
	voxel_grid_1.setLeafSize(0.01, 0.01, 0.01); //设置体素边长
	voxel_grid_1.setInputCloud(cloud_src_o);
	PointCloud::Ptr cloud_src(new PointCloud);
	voxel_grid_1.filter(*cloud_src);  //精简后的输入点云 放入cloud_src 中
	cerr << "初始点云数：" << cloud_src_o->size() << "-----精简之后点云数： " << cloud_src->size() << endl;
	// 2
	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;
	voxel_grid_2.setLeafSize(0.01, 0.01, 0.01);
	voxel_grid_2.setInputCloud(cloud_tgt_o);
	PointCloud::Ptr cloud_tgt(new PointCloud);
	voxel_grid_2.filter(*cloud_tgt);//       cloud_tgt
	cerr << "初始点云数：" << cloud_tgt_o->size() << "-----精简之后点云数： " << cloud_tgt->size() << endl;

	//--------------------计算表面法线--------------------//
	// 1
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src; //创建法线对象
	ne_src.setInputCloud(cloud_src);
	pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>()); //创建搜索树对象
	ne_src.setSearchMethod(tree_src); //设置搜索方法
	pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>); //cloud_src_normals 存储法线向量
	ne_src.setRadiusSearch(0.02); //设置搜索半径
	ne_src.compute(*cloud_src_normals);  //计算表面法线向量存入 cloud_src_normals 中
										 // 2
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;
	ne_tgt.setInputCloud(cloud_tgt);
	pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
	ne_tgt.setSearchMethod(tree_tgt);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
	ne_tgt.setRadiusSearch(0.02);
	ne_tgt.compute(*cloud_tgt_normals);

	//--------------------计算FPFH--------------------//
	// 1
	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_src; //创建fpfh对象
	fpfh_src.setInputCloud(cloud_src); //传入点云
	fpfh_src.setInputNormals(cloud_src_normals); //传入法线向量
	pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);
	fpfh_src.setSearchMethod(tree_src_fpfh);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());
	fpfh_src.setRadiusSearch(0.05);
	fpfh_src.compute(*fpfhs_src);  //计算结果保存fpfhs_src中
	std::cout << "compute *cloud_src fpfh" << endl;
	// 2
	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;
	fpfh_tgt.setInputCloud(cloud_tgt);
	fpfh_tgt.setInputNormals(cloud_tgt_normals);
	pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);
	fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());
	fpfh_tgt.setRadiusSearch(0.05);
	fpfh_tgt.compute(*fpfhs_tgt);
	std::cout << "compute *cloud_tgt fpfh" << endl;

	//--------------------SAC-IA采样一致性粗配准--------------------//
	pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
	scia.setInputSource(cloud_src);
	scia.setInputTarget(cloud_tgt);
	scia.setSourceFeatures(fpfhs_src);
	scia.setTargetFeatures(fpfhs_tgt);
	//scia.setMinSampleDistance(1);
	//scia.setNumberOfSamples(2);
	//scia.setCorrespondenceRandomness(20);
	PointCloud::Ptr sac_result(new PointCloud);
	scia.align(*sac_result); //执行配准存储变换后的点云到sac_result中
	Eigen::Matrix4f sac_trans;
	sac_trans = scia.getFinalTransformation();
	clock_t sac_time = clock();

	//--------------------ICP精配准--------------------//
	PointCloud::Ptr icp_result(new PointCloud);
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_src);
	icp.setInputTarget(cloud_tgt_o);
	//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
	icp.setMaxCorrespondenceDistance(0.04);
	// 最大迭代次数
	icp.setMaximumIterations(100);
	// 两次变化矩阵之间的差值
	icp.setTransformationEpsilon(1e-10);
	// 均方误差
	icp.setEuclideanFitnessEpsilon(0.01);
	icp.align(*icp_result, sac_trans);

	Eigen::Matrix4f icp_trans;
	icp_trans = icp.getFinalTransformation();
	pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);

	pcl::PCDWriter w;
	w.write<pcl::PointXYZ>("final.pcd", *icp_result, false);
	visualize_pcd(cloud_tgt_o, icp_result);
	return 0;
}