ICP+NDT点云配准

PCL1.8.0+VS2013编写，利用ICP进行粗略配准，利用NDT进行精确配准，当两个点云重叠率较大时有较好的效果，点云数据是用bun000和bun045，今后可能会上传NDT+ICP进行配的代码。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
//配准
#include  //ICP方法
#include 
#include 

#include 
#include 
#include 
#include 

//命名空间
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;

//定义类型的别名
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud PointCloudWithNormals;

//void pairAlign（const PointCloud::Ptr cloud_src const PointCloud::Ptr cloud_tgt PointCloud::Ptr output Eigen::Matrix4f &final_transform bool downsample = false）
//void loadData（int argc char **argv std::vector > &models）

//全局变量
//可视化对象
pcl::visualization::PCLVisualizer *p;
//左视区和右视区，可视化窗口分成左右两部分
int vp_1 vp_2;

//定义结构体，用于处理点云
struct PCD
{
	PointCloud::Ptr cloud; //点云指针
	std::string f_name; //文件名
	//构造函数
	PCD（） : cloud（new PointCloud） {}; //初始化
};


// 定义新的点表达方式< x y z curvature > 坐标+曲率
class MyPointRepresentation : public pcl::PointRepresentation  //继承关系
{
	using pcl::PointRepresentation::nr_dimensions_;
public:
	MyPointRepresentation（）
	{
		//指定维数
		nr_dimensions_ = 4;
	}

	//重载函数copyToFloatArray，以定义自己的特征向量
	virtual void copyToFloatArray（const PointNormalT &p float * out） const
	{
		//< x y z curvature > 坐标xyz和曲率
		out[0] = p.x;
		out[1] = p.y;
		out[2] = p.z;
		out[3] = p.curvature;
	}
};
// 读取一系列的PCD文件（希望配准的点云文件）
// 参数argc 参数的数量（来自main（））
// 参数argv 参数的列表（来自main（））
// 参数models 点云数据集的结果向量
void loadData（int argc char **argv std::vector > &models）
{
	std::string extension（“.pcd“）; //声明并初始化string类型变量extension，表示文件后缀名
	// 通过遍历文件名，读取pcd文件
	for （int i = 1; i < argc; i++） //遍历所有的文件名（略过程序名）
	{
		std::string fname = std::string（argv[i]）;
		if （fname.size（） <= extension.size（）） //文件名的长度是否符合要求
			continue;

		std::transform（fname.begin（） fname.end（） fname.begin（） （int（*）（int））tolower）; //将某操作（小写字母化）应用于指定范围的每个元素
		//检查文件是否是pcd文件
		if （fname.compare（fname.size（） - extension.size（） extension.size（） extension） == 0）
		{
			// 读取点云，并保存到models
			PCD m;
			m.f_name = argv[i];
			pcl::io::loadPCDFile（argv[i] *m.cloud）; //读取点云数据
			//去除点云中的NaN点（xyz都是NaN）
			std::vector indices; //保存去除的点的索引
			pcl::removeNaNFromPointCloud（*m.cloud *m.cloud indices）; //去除点云中的NaN点

			models.push_back（m）;
		}
	}
}

//简单地配准一对点云数据，并返回结果
//参数cloud_src  源点云
//参数cloud_tgt  目标点云
//参数output     输出点云
//参数final_transform 成对变换矩阵
//参数

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-05-28 20:37  ICP+NDT\
     目录           0  2018-05-28 20:38  ICP+NDT\Debug\
     文件     3512293  2018-04-13 17:55  ICP+NDT\Debug\bun000.pcd
     文件     3493771  2018-04-16 09:33  ICP+NDT\Debug\bun045.pcd
     文件      437760  2018-05-06 10:37  ICP+NDT\Debug\pclStart.exe
     文件         406  2018-05-03 15:03  ICP+NDT\Debug\pclStart.exe.manifest
     文件     1924320  2018-05-06 10:37  ICP+NDT\Debug\pclStart.ilk
     目录           0  2018-05-28 20:39  ICP+NDT\pclStart\
     目录           0  2018-05-28 20:35  ICP+NDT\pclStart\Debug\
     文件       43440  2012-05-13 17:15  ICP+NDT\pclStart\Debug\bun0.pcd
     文件     3511384  2018-05-06 10:37  ICP+NDT\pclStart\Debug\main.obj
     文件        1830  2018-05-03 15:02  ICP+NDT\pclStart\Debug\pclStart.Build.CppClean.log
     文件         381  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.exe.intermediate.manifest
     文件       11084  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.log
     目录           0  2018-05-28 20:35  ICP+NDT\pclStart\Debug\pclStart.tlog\
     文件      405378  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\CL.read.1.tlog
     文件         756  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\CL.write.1.tlog
     文件         844  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\cl.command.1.tlog
     文件       15582  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\link.command.1.tlog
     文件       37332  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\link.read.1.tlog
     文件         976  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\link.write.1.tlog
     文件         594  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\mt.command.1.tlog
     文件         444  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\mt.read.1.tlog
     文件         444  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\mt.write.1.tlog
     文件         203  2018-05-06 10:37  ICP+NDT\pclStart\Debug\pclStart.tlog\pclStart.lastbuildstate
     文件    17492958  2012-05-03 17:21  ICP+NDT\pclStart\Debug\table_scene_lms400.pcd
     文件    10275840  2018-05-06 10:37  ICP+NDT\pclStart\Debug\vc120.idb
     文件    14782464  2018-05-06 10:37  ICP+NDT\pclStart\Debug\vc120.pdb
     文件       15407  2018-04-26 10:21  ICP+NDT\pclStart\IcpPlusNdt000Plus045.cpp
     文件        6214  2018-04-09 11:04  ICP+NDT\pclStart\PCL1.8.0Debug.props
     文件     3512293  2018-04-13 17:55  ICP+NDT\pclStart\bun000.pcd
............此处省略6个文件信息