基于PCL开源库fpfh+icp算法实现点云配准

利用PCL开源库编写代码FPFH+ICP算法实现点云高精度配准，并计算配准误差！基于PCL库版本1.9！

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

using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;

//点云可视化
void visualize_pcd（PointCloud::Ptr pcd_src PointCloud::Ptr pcd_tgt PointCloud::Ptr pcd_final）
{

	//创建初始化目标
	pcl::visualization::PCLVisualizer viewer（“registration Viewer“）;


	pcl::visualization::PointCloudColorHandlerCustom src_h（pcd_src 0 255 0）;
	pcl::visualization::PointCloudColorHandlerCustom tgt_h（pcd_tgt 255 0 0）;
	pcl::visualization::PointCloudColorHandlerCustom final_h（pcd_final 0 0 255）;
	viewer.setBackgroundColor（255 255 255）;
	viewer.addPointCloud（pcd_src src_h “source cloud“）;
	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））;
	}
}
//由旋转平移矩阵计算旋转角度
void matrix2angle（Eigen::Matrix4f &result_trans Eigen::Vector3f &result_angle）
{
	double ax ay az;
	if （result_trans（2 0） == 1 || result_trans（2 0） == -1）
	{
		az = 0;
		double dlta;
		dlta = atan2（result_trans（0 1） result_trans（0 2））;
		if （result_trans（2 0） == -1）
		{
			ay = M_PI / 2;
			ax = az + dlta;
		}
		else
		{
			ay = -M_PI / 2;
			ax = -az + dlta;
		}
	}
	else
	{
		ay = -asin（result_trans（2 0））;
		ax = atan2（result_trans（2 1） / cos（ay） result_trans（2 2） / cos（ay））;
		az = atan2（result_trans（1 0） / cos（ay） result_trans（0 0） / cos（ay））;
	}
	result_angle << ax ay az;

	cout << “x轴旋转角度：“ << ax << endl;
	cout << “y轴旋转角度：“ << ay << endl;
	cout << “z轴旋转角度：“ << az << endl;
}

int main（int argc char** argv）
{
	//加载点云文件
	PointCloud::Ptr cloud_src_o（new PointCloud）;//原点云，待配准
	pcl::io::loadPCDFile（“E:/vs13/pcldata/bun/rabbit.pcd“ *cloud_src_o）;
	PointCloud::Ptr cloud_tgt_o（new PointCloud）;//目标点云
	pcl::io::loadPCDFile（“E:/vs13/pcldata/bun/rabbit_1.pcd“ *cloud_tgt_o）;

	clock_t start = clock（）;

	//去除NAN点
	std::vector indices_src; //保存去除的点的索引
	pcl::removeNaNFromPointCloud（*cloud_src_o *cloud_src_o indices_src）;
	std::cout << “remove *cloud_src_o nan“ << endl;

	std::vector indices_tgt;
	pcl::removeNaNFromPointCloud（*cloud_tgt_o *cloud_tgt_o indices_tgt）;
	std::cout << “remove *cloud_tgt_o nan“ << endl;

	//下采样滤波
	pcl::VoxelGrid voxel_grid;
	voxel_grid.setLeafSize（0.012 0.012 0.012）;
	voxel_grid.setInputCloud（cloud_src_o）;
	PointCloud::Ptr cloud_src（new PointC

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       8813  2019-03-04 20:04  fpfh+icp.cpp

----------- ---------  ---------- -----  ----

                 8813                    1