基于PCL开源库ndt+icp算法实现点云配准测试代码

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

#include 
#include 
#include 
#include //icp头文件
#include  //ndt头文件
#include 
#include 
#include 
#include 
#include 
#include //去除NAN点的头文件
#include  //官网上采样过滤，先不用

using namespace std;
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 “target cloud“）;
	viewer.addPointCloud（pcd_final final_h “result 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_source（new PointCloud）;
	PointCloud::Ptr cloud_target（new PointCloud）;


	// 加载点云文件
	pcl::io::loadPCDFile（“E:/vs13/pcldata/bun/rabbit.pcd“ *cloud_source）;
	std::cout << “source loaded!“ << std::endl;
	pcl::io::loadPCDFile（“E:/vs13/pcldata/bun/rabbit_1.pcd“ *cloud_target）;
	std::cout << “target loaded!“ << std::endl;

	clock_t start = clock（）;

	//去除NAN点

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

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

	//下采样滤波
	pcl::VoxelGrid v

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

     文件       7466  2019-03-04 20:19  ndt+icp.cpp

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

                 7466                    1