pcl NDT 点云配准

NDT算法在点云数量较多的大场景点云配准方面有速度快，鲁棒性好等优点，源码如下：

#include <pcl/common/transforms.h>
#include<pcl/keypoints/uniform_sampling.h>
#include<pcl/registration/ndt.h>
#include<pcl/registration/transformation_estimation_lm.h>

 void sampl_down( pcl::PointCloud<pcl::PointXYZI>::Ptr& src, pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_down, double radius)
 {
   pcl::UniformSampling<pcl::PointXYZI> us;
   us.setInputCloud(src);
   us.setRadiusSearch(radius);
   us.filter(*cloud_down);
 }

 void ndt_process(pcl::PointCloud<pcl::PointXYZI>::Ptr& src, pcl::PointCloud<pcl::PointXYZI>::Ptr& tar,pcl::PointCloud<pcl::PointXYZI>::Ptr& total)
 {
   //均匀降采样
   pcl::PointCloud<pcl::PointXYZI>::Ptr mid1_down(new pcl::PointCloud<pcl::PointXYZI>);
   pcl::PointCloud<pcl::PointXYZI>::Ptr mid2_down(new pcl::PointCloud<pcl::PointXYZI>);
   sampl_down(tar, mid1_down, 0.1);//0.15
   sampl_down(src, mid2_down, 0.1);

   //ndt
   pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
   const pcl::registration::TransformationEstimationLM<pcl::PointXYZI,pcl::PointXYZI,float>::Ptr LM
   (new pcl::registration::TransformationEstimationLM<pcl::PointXYZI,pcl::PointXYZI,float>);
   clock_t start = clock();
   pcl::NormalDistributionsTransform<pcl::PointXYZI,pcl::PointXYZI> ndt;
   ndt.setTransformationEstimation(LM);//基于LM优化
   ndt.setStepSize(0.1);//位姿调整步长
   ndt.setResolution(1.5f);//分割为正方体栅格大小，场景面积越大，参数值越大
   ndt.setMaximumIterations(2500);//最大迭代次数
   ndt.setInputSource(mid2_down);//源点云
   ndt.setInputTarget(mid1_down);//目标点云
   ndt.setTransformationEpsilon(0.005);//迭代截至误差阈值
   ndt.align(*output_cloud);//输出配准后的源点云
   clock_t end = clock();
   std::cout<<"ndt_process_time:"<<(double)(end - start)/CLOCKS_PER_SEC<<std::endl;
   std::cout<<"ndt_hasconver:"<<ndt.hasConverged()<<std::endl;
   std::cout<<"ndt_score:"<<ndt.getFitnessScore()<<std::endl;
   std::cout<<"ndt_itera_nums:"<<ndt.getMaximumIterations()<<std::endl;
   std::cout<<"ndt_tf:"<<ndt.getFinalTransformation()<<std::endl;


   //合并
   *total = *output_cloud + *mid1_down;//配准后的源点云和目标点云合并

 }