点云匹配 5之 正态分布变换配准(NDT)

一、概述

正态分布变换( Normal Distributions Transform )进行配准

用正态分布变换算法来确定两个大型点云（都超过 100 000个点）之间的刚体变换。正态分布变换算法是一个配准算法 , 它应用于 三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快，更多关于正态分布变换算法的详细的信息，请看 Martin Magnusson 博士的博士毕业论文“The Three-Dimensional Normal Distributions Transform – an Efficient Representation for Registration, Surface Analysis, and Loop Detection.”

二、原理

https://www.cnblogs.com/li-yao7758258/p/10705228.html

三、code?

?

?核心代码：

			// 开始配准
			// Initializing Normal Distributions Transform (NDT).
			pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
			ndt.setTransformationEpsilon(0.001);
			ndt.setStepSize(0.01);
			ndt.setResolution(0.01);
			ndt.setEuclideanFitnessEpsilon(0.001);
			ndt.setMaximumIterations(1000);
			ndt.setInputSource(cloud_source);
			ndt.setInputTarget(cloud_target);

			// Set initial alignment estimate found using robot odometry.
			Eigen::AngleAxisf init_rotation(0.1, Eigen::Vector3f::UnitZ());

			// 这里打算输入一个自定义猜测的矩阵，结果不理想
		/*	Eigen::Translation3f init_translation(0.1, 0, 0);
			Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();*/

			Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity();

			pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
			ndt.align(*output_cloud, init_guess);
			G.matrix()=ndt.getFinalTransformation();
			cout << " 初始化RT矩阵  :     " << init_guess << endl;

			cout << " ndt score  ； " << ndt.getFitnessScore()  << endl;

#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/filters\voxel_grid.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/registration/ndt.h>       // ndt配准文件头

using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

PointCloud::Ptr cloud_target(new PointCloud);
PointCloud::Ptr cloud_source(new PointCloud);
PointCloud::Ptr cloud_icp(new PointCloud);
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("global_visualizer"));

const double translation_step = 0.005;  // 设定一个平移的步长
const double rotation_step = M_PI / 72;  // 设定一个旋转的步长
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* viewer_void);
Eigen::Affine3f G = Eigen::Affine3f::Identity();

double f1, f2, f3;


#if 1
int main() {
	//加载点云
	string target_path = "C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\1.pcd";
	string source_path = "C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\2.pcd";
	pcl::io::loadPCDFile(target_path, *cloud_target);
	pcl::io::loadPCDFile(source_path, *cloud_source);

	//点云降采样
	pcl::VoxelGrid<pcl::PointXYZ> VG;
	VG.setInputCloud(cloud_target);
	VG.setLeafSize(0.001f, 0.001f, 0.001f);
	VG.filter(*cloud_target);

	VG.setInputCloud(cloud_source);
	VG.setLeafSize(0.001f, 0.001f, 0.001f);
	VG.filter(*cloud_source);

	//可视化相关
	int v1 = 1;
	viewer->createViewPort(0.0, 0.0, 1.0, 1.0, v1);
	viewer->setBackgroundColor(255, 255, 255, v1);
	viewer->addPointCloud(cloud_target, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_target, 0.0, 0.0, 0.0), "cloud_target", v1);
	viewer->addPointCloud(cloud_source, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_source, 255.0, 0.0, 0.0), "cloud_source", v1);
	viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)viewer.get());


	while (!viewer->wasStopped()) {
		// 应用变换
		pcl::transformPointCloud(*cloud_source, *cloud_source, G);
		G = Eigen::Affine3f::Identity();


		// 更新视图
		viewer->removePointCloud("cloud_source");
		viewer->addPointCloud<pcl::PointXYZ>(cloud_source, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_source, 255.0, 0.0, 0.0), "cloud_source", v1);
		viewer->removePointCloud("cloud_target");
		viewer->addPointCloud<pcl::PointXYZ>(cloud_target, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_target, 0.0, 0.0, 0.0), "cloud_target", v1);

		viewer->spinOnce(10);  // 每次更新等待10ms
	}

}

#endif // 0

//键盘回调函数
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* viewer_void)
{
	if (event.keyDown())
	{
		if (event.getKeySym() == "u")
		{
			// 开始配准
			// Initializing Normal Distributions Transform (NDT).
			pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
			ndt.setTransformationEpsilon(0.001);
			ndt.setStepSize(0.01);
			ndt.setResolution(0.01);
			ndt.setEuclideanFitnessEpsilon(0.001);
			ndt.setMaximumIterations(1000);
			ndt.setInputSource(cloud_source);
			ndt.setInputTarget(cloud_target);

			// Set initial alignment estimate found using robot odometry.
			Eigen::AngleAxisf init_rotation(0.1, Eigen::Vector3f::UnitZ());

			// 这里打算输入一个自定义猜测的矩阵，结果不理想
		/*	Eigen::Translation3f init_translation(0.1, 0, 0);
			Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();*/

			Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity();

			pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
			ndt.align(*output_cloud, init_guess);
			G.matrix()=ndt.getFinalTransformation();
			cout << " 初始化RT矩阵  :     " << init_guess << endl;

			cout << " ndt score  ； " << ndt.getFitnessScore()  << endl;
		}else if (event.getKeySym() == "space")
		{
			pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
			icp.setInputSource(cloud_source);
			icp.setInputTarget(cloud_target);
			icp.setTransformationEpsilon(1e-10);
			icp.setMaxCorrespondenceDistance(100);
			icp.setEuclideanFitnessEpsilon(0.001); 
			icp.setMaximumIterations(10000);
			icp.setUseReciprocalCorrespondences(false);

			icp.align(*cloud_icp);
			G = icp.getFinalTransformation();

			cout <<" icp score  ； " << icp.getFitnessScore()<< endl;

		}else if(event.getKeySym() == "m" || event.getKeySym() == "l" )  // 
		{ 
		       pcl::PointCloud<pcl::PointNormal>::Ptr source(new pcl::PointCloud<pcl::PointNormal>);
		       pcl::PointCloud<pcl::PointNormal>::Ptr target(new pcl::PointCloud<pcl::PointNormal>);
		       pcl::PointCloud<pcl::PointNormal>::Ptr icp_normal(new pcl::PointCloud<pcl::PointNormal>);
		        // 计算点云的法向量
		       pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
		       pcl::PointCloud<pcl::Normal>::Ptr normals_source(new pcl::PointCloud<pcl::Normal>);
		       //建立kdtree来进行近邻点集搜索
		       pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
		       n.setNumberOfThreads(10);//设置openMP的线程数 我一般是10条就够了
		        //主要是改变这个值来看看点云的法向量的方向变换
		       n.setViewPoint(0, 0, 0);//设置视点，默认为（0，0，0）
		       n.setInputCloud(cloud_source);
		       n.setSearchMethod(tree);
		       n.setKSearch(10);//点云法向计算时，需要所搜的近邻点大小
		       //n.setRadiusSearch(0.03);//半径搜素
		       n.compute(*normals_source);//开始进行法向计
		       pcl::concatenateFields(*cloud_source, *normals_source, *source);
		       
		       n.setInputCloud(cloud_target);
		       pcl::PointCloud<pcl::Normal>::Ptr normals_target(new pcl::PointCloud<pcl::Normal>);
		       n.compute(*normals_target);//开始进行法向计
		       pcl::concatenateFields(*cloud_target, *normals_target, *target);


			   if (event.getKeySym() == "m")
			   {
				   // 开始匹配
				   pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> icp;
				   icp.setInputSource(source);
				   icp.setInputTarget(target);
				   //icp.setCorrespondenceEstimation(0.1f);  // 计算点云的共同部分  对应关系估计
				   icp.setTransformationEpsilon(1e-10);  //两个连续转换之间的最大允许平移平方差
				   icp.setMaxCorrespondenceDistance(100); //目标中两个对应点之间的最大距离阈值。如果距离大于此阈值，则在对齐过程中将忽略这些点
				   icp.setEuclideanFitnessEpsilon(0.001);   // 算法收敛之前最大误差
				   icp.setMaximumIterations(10000);
				   icp.setUseReciprocalCorrespondences(true);
				   icp.align(*icp_normal);
				   G = icp.getFinalTransformation();
				   f1 = icp.getFitnessScore(); //getFitnessScore()函数
			   }
			   else if (event.getKeySym() == "l")
			   {
				   pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> icp;
				   icp.setInputSource(source);
				   icp.setInputTarget(target);
				   //icp.setCorrespondenceEstimation(0.1f);  // 计算点云的共同部分  对应关系估计
				   icp.setTransformationEpsilon(1e-10);  //两个连续转换之间的最大允许平移平方差
				   icp.setMaxCorrespondenceDistance(100); //目标中两个对应点之间的最大距离阈值。如果距离大于此阈值，则在对齐过程中将忽略这些点
				   icp.setEuclideanFitnessEpsilon(0.001);   // 算法收敛之前最大误差
				   icp.setMaximumIterations(10000);
				//   icp.setUseReciprocalCorrespondences(false);
				   icp.align(*icp_normal);
				   G = icp.getFinalTransformation();
				   cout <<" IterativeClosestPointWithNormals  :    "<< icp.getFitnessScore() << endl;
			   }
			  
		}
		else
		{
			switch (event.getKeySym()[0])
			{
			case 'w':
				G.translation() += Eigen::Vector3f(translation_step, 0, 0);
				break;
			case 's':
				G.translation() -= Eigen::Vector3f(translation_step, 0, 0);
				break;
			case 'a':
				G.translation() -= Eigen::Vector3f(0, translation_step, 0);
				break;
			case 'd':
				G.translation() += Eigen::Vector3f(0, translation_step, 0);
				break;
			case 'z':
				G.translation() += Eigen::Vector3f(0, 0, translation_step);
				break;
			case 'c':
				G.translation() -= Eigen::Vector3f(0, 0, translation_step);
				break;
			case 'p':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitY()));
				break;  // Pitch
			case 'y':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitZ()));
				break;  // Yaw
			default: break;
			}
		}
	}
	cout << "变换矩阵   " << endl;
	cout << G.matrix() << endl;
	cout << endl;
	cout << endl;
	cout << endl;
}