使用正态分布变换进行配准
2015-10-22 10:20
1031 查看
#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> int main(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 visualizer boost::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 visualizer viewer_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); }
相关文章推荐
- django实现tab页思路!
- win7 + VS2013 出现Cannot open include file: 'SDKDDKVer.h'问题 附地址
- Centos tmux 安装 与 配置
- 数据库为何要有复合主键(多主键)
- Eclipse系列: Eclipse设置Tomcat启动超时时间
- webApi实践:开始WebApi 2
- Esper系列(四)Output
- test
- 如何在 Ubuntu 中设置 IonCube Loaders
- Linux学习16_linux上ln命令详细说明
- Flip Game【POJ--1753】【枚举】【高斯消元】
- 获取服务器信息
- HTTP协议 (七) Cookie(转)
- 有关数据库设计原则(转载)
- maven 编码 UTF-8 的不可映射字符
- oracle 不要建空表再导入数据,直接根据原表建成分区表
- 【JavaScript】利用滚动事件window.onscroll与position:fixed写兼容IE6的回到顶部组件
- webpack 用法
- 13.6 2013计算机视觉代码合集一
- 转载_那些年,我追过的绘图工具