3d激光雷达开发（ndt匹配）

除了icp匹配之外，ndt匹配也是使用比较多的一种方法。相比较icp而言，ndt匹配花的时间要少一些。此外，ndt匹配还需要输入估计的yaw、pitch、roll、x、y、z，这个可以根据经验值给出。猜的越准，算法收敛的越快。参考的代码出处在这，https://pcl.readthedocs.io/projects/tutorials/en/master/normal_distributions_transform.html#normal-distributions-transform

1、准备normal_distributions_transform.cpp文件

#include <iostream>
#include <thread>#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>using namespace std::chrono_literals;int
main ()
{// Loading first scan of room.pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1){PCL_ERROR ("Couldn't read file room_scan1.pcd \n");return (-1);}std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;// Loading second scan of room from new perspective.pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1){PCL_ERROR ("Couldn't read file room_scan2.pcd \n");return (-1);}std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;// Filtering input scan to roughly 10% of original size to increase speed of registration.pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);approximate_voxel_filter.setInputCloud (input_cloud);approximate_voxel_filter.filter (*filtered_cloud);std::cout << "Filtered cloud contains " << filtered_cloud->size ()<< " data points from room_scan2.pcd" << std::endl;// Initializing Normal Distributions Transform (NDT).pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;// Setting scale dependent NDT parameters// Setting minimum transformation difference for termination condition.ndt.setTransformationEpsilon (0.01);// Setting maximum step size for More-Thuente line search.ndt.setStepSize (0.1);//Setting Resolution of NDT grid structure (VoxelGridCovariance).ndt.setResolution (1.0);// Setting max number of registration iterations.ndt.setMaximumIterations (35);// Setting point cloud to be aligned.ndt.setInputSource (filtered_cloud);// Setting point cloud to be aligned to.ndt.setInputTarget (target_cloud);// Set initial alignment estimate found using robot odometry.Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());Eigen::Translation3f init_translation (1.79387, 0.720047, 0);Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();// Calculating required rigid transform to align the input cloud to the target cloud.pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);ndt.align (*output_cloud, init_guess);std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()<< " score: " << ndt.getFitnessScore () << std::endl;// Transforming unfiltered, input cloud using found transform.pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());// Saving transformed input cloud.pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);// Initializing point cloud visualizerpcl::visualization::PCLVisualizer::Ptrviewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));viewer_final->setBackgroundColor (0, 0, 0);// Coloring and visualizing target cloud (red).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");// Coloring and visualizing transformed input cloud (green).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");// Starting visualizerviewer_final->addCoordinateSystem (1.0, "global");viewer_final->initCameraParameters ();// Wait until visualizer window is closed.while (!viewer_final->wasStopped ()){viewer_final->spinOnce (100);std::this_thread::sleep_for(100ms);}return (0);
}

2、准备CMakeLists.txt文件

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)project(normal_distributions_transform)find_package(PCL 1.5 REQUIRED)include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable(normal_distributions_transform normal_distributions_transform.cpp)
target_link_libraries (normal_distributions_transform ${PCL_LIBRARIES})

3、生成sln工程，准备编译

4、执行normal_distribution_transform.exe文件

注意，执行过程中需要room_scan1.pcd和room_scan2.pcd这两个文件。

输入normal_distribution_transform.exe room_scan1.pcd room_scan2.pcd,

效果如下，

3d激光雷达开发（ndt匹配）

3d激光雷达开发（ndt匹配）相关推荐

最新文章

热门文章