3d-ndt算法在pcl上的实现完整项目文件

#include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/registration/ndt.h> #include <pcl/filters/approximate_voxel_grid.h> #include <pcl/visualization/pcl_visualizer.h> #include <boost/thread/thread.hpp> typedef pcl::PointXYZ PointT; typedef pcl::PointCloud<PointT> PointCloud; int main(int argc, char** argv) { //加载目标点云 pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>); //std::string filename = "point_source/bun000_s100.pcd";//0度扫描的兔子 std::string filename = "point_source/hippo1-1224_s100.pcd";//hippo1-1224_s100 if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *target_cloud) == -1) { PCL_ERROR("Error loading cloud %s.\n", filename); return (-1); } std::cout << "Loaded " << target_cloud->size() << " data points from "<< filename << std::endl;//显示点云的数量 //加载源点云（待配准点云） pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>); //filename = "point_source/bun045_s100.pcd"; filename = "point_source/hippo2-1224_s100.pcd";//hippo1-1224_s100 if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *input_cloud) == -1) { PCL_ERROR("Error loading cloud %s.\n", filename); return (-1); } std::cout << "Loaded " << input_cloud->size() << " data points from " << filename << std::endl;//显示点云的数量 pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);//定义降采样后点云 *filtered_cloud = *input_cloud; //体素降采样 pcl::VoxelGrid<pcl::PointXYZ> voxel_grid; voxel_grid.setLeafSize(2.66, 2.66, 2.66);//bunny-0.5 hippo-2.66 voxel_grid.setInputCloud(input_cloud); voxel_grid.filter(*filtered_cloud); std::cout << "down size input_cloud to" << filtered_cloud->size() << endl;//显示降采样后的点云数量 //pcl::io::savePCDFileBinary("point_source/bun045_voxel_0.5.pcd", *filtered_cloud);//储存降采样点 clock_t start = clock();//算法计时 //初始化正态分布变换（NDT） pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt; //设置依赖尺度NDT参数 /** \brief Set/change the point cloud outlier ratio. * \param[in] outlier_ratio outlier ratio */ ndt.setOulierRatio(0.5);//离群点云比 //为终止条件设置最小转换差异 ndt.setTransformationEpsilon(0.001); //为More-Thuente线搜索设置最大步长 ndt.setStepSize(2.9);//Bunny-0.5 hippo-2.9 //设置NDT网格结构的分辨率（VoxelGridCovariance） ndt.setResolution(5.8);//bunny-1.0 hippo-5.8 //设置匹配迭代的最大次数 ndt.setMaximumIterations(100); // 设置要配准的点云 ndt.setInputCloud(filtered_cloud); //设置点云配准目标 ndt.setInputTarget(target_cloud); //计算需要的刚体变换以便将输入的点云匹配到目标点云 pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>); ndt.align(*output_cloud);//配准，将配准后的点云放至output_cloud clock_t end1 = clock(); cout << "ndt time" << (double)(end1 - start) / CLOCKS_PER_SEC << endl;//打印配准时间 std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged() << " score: " << ndt.getFitnessScore() << std::endl; cout << "ndt matrix:" << endl << ndt.getFinalTransformation() << endl;//打印变换矩阵 // 初始化点云可视化界面 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer")); viewer_final->setBackgroundColor(0, 0, 0);//背景颜色黑 //添加目标点云到viewer pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);//红色 viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud"); viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud"); //添加源点云到viewer pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_color(input_cloud, 0, 255, 255);//天蓝色 viewer_final->addPointCloud<pcl::PointXYZ>(input_cloud, input_color, "input cloud"); viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud"); //使用求得的变换的矩阵对源点云进行变换，结果放至output_cloud pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation()); //添加配准后的点云到viewer pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(output_cloud, 0, 255, 0);//绿色 viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud"); viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud"); //pcl::io::savePCDFileBinary("point_source/bun045to000_ndt.pcd", *output_cloud);//储存降采样点 // 启动可视化 viewer_final->addCoordinateSystem(1.0); viewer_final->initCameraParameters(); //等待直到可视化窗口关闭。 while (!viewer_final->wasStopped()) { viewer_final->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } return (0); }