#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?<
	cout?<	cout?<	cout?<}

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?<	pcl::io::loadPCDFile（“E:/vs13/pcldata/bun/rabbit_1.pcd“?*cloud_target）;
	std::cout?<
	clock_t?start?=?clock（）;

	//去除NAN點

	std::vector?indices_src;?//保存去除的點的索引
	pcl::removeNaNFromPointCloud（*cloud_source?*cloud_source?indices_src）;
	std::cout?<
	std::vector?indices_tgt;?//保存去除的點的索引
	pcl::removeNaNFromPointCloud（*cloud_target?*cloud_target?indices_tgt）;
	std::cout?<
	//下采樣濾波
	pcl::VoxelGrid?v

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

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

-----------?---------??----------?-----??----

?????????????????7466????????????????????1