使用正态分布变换进行配准

#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>intmain(int argc, char** argv){// 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 visualizerboost::shared_ptr<pcl::visualization::PCLVisualizer>viewer_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);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}return (0);}