PCL 遮挡场景下的点云位姿估计

这个算法能够在杂乱背景和遮挡的场景中找到刚性物体的对齐位姿。先看一下输入数据。左边是chef.pcd,这是一个厨师(chef)模型的3D扫描。右边是rs1.pcd的深度图 这是一个遮挡场景距离相机近的点显示为红色距离相机远的点显示为蓝色。红色的恐龙(近处)把后面的厨师(远处)遮挡了一部分(看不出来也没事,我一开始也没看出来)深度图距离相机越近颜色越接近红色。距离相机越远颜色越接近蓝色。程序运行后会估计出厨师在这个遮挡场景下的位姿up截了两张不同角度的图片绿色的是rs1.pcd蓝色的是程序补充/估计出来的厨师位姿下面这两张图片是参考资料里给的图片作为补充图片其实不太容易看明白最好去看看我做的视频演示。输入chef.pcd rs1.pcd输出没有保存输出结果运行编译后在cmd里运行需要带上2个参数 chef.pcd rs1.pcd示例program.exe chef.pcd rs1.pcd视频演示https://www.bilibili.com/video/BV1xwTe6sEnw/代码#include Eigen/Core #include pcl/point_types.h #include pcl/point_cloud.h #include pcl/common/time.h #include pcl/console/print.h #include pcl/features/normal_3d_omp.h #include pcl/features/fpfh_omp.h #include pcl/filters/filter.h #include pcl/filters/voxel_grid.h #include pcl/io/pcd_io.h #include pcl/registration/sample_consensus_prerejective.h #include pcl/visualization/pcl_visualizer.h // Types typedef pcl::PointNormal PointNT; typedef pcl::PointCloudPointNT PointCloudT; typedef pcl::FPFHSignature33 FeatureT; typedef pcl::FPFHEstimationOMPPointNT,PointNT,FeatureT FeatureEstimationT; typedef pcl::PointCloudFeatureT FeatureCloudT; typedef pcl::visualization::PointCloudColorHandlerCustomPointNT ColorHandlerT; // Align a rigid object to a scene with clutter and occlusions int main (int argc, char **argv) { // Point clouds PointCloudT::Ptr object (new PointCloudT); PointCloudT::Ptr object_aligned (new PointCloudT); PointCloudT::Ptr scene_before_downsampling (new PointCloudT); PointCloudT::Ptr scene (new PointCloudT); FeatureCloudT::Ptr object_features (new FeatureCloudT); FeatureCloudT::Ptr scene_features (new FeatureCloudT); // Get input object and scene if (argc ! 3) { pcl::console::print_error (Syntax is: %s object.pcd scene.pcd\n, argv[0]); return (1); } // Load object and scene pcl::console::print_highlight (Loading point clouds...\n); if (pcl::io::loadPCDFilePointNT (argv[1], *object) 0 || pcl::io::loadPCDFilePointNT (argv[2], *scene_before_downsampling) 0) { pcl::console::print_error (Error loading object/scene file!\n); return (1); } // Downsample pcl::console::print_highlight (Downsampling...\n); pcl::VoxelGridPointNT grid; const float leaf 0.005f; grid.setLeafSize (leaf, leaf, leaf); grid.setInputCloud (object); grid.filter (*object); grid.setInputCloud (scene_before_downsampling); grid.filter (*scene); // Estimate normals for scene pcl::console::print_highlight (Estimating scene normals...\n); pcl::NormalEstimationOMPPointNT,PointNT nest; nest.setRadiusSearch (0.005); nest.setInputCloud (scene); nest.setSearchSurface (scene_before_downsampling); nest.compute (*scene); // Estimate features pcl::console::print_highlight (Estimating features...\n); FeatureEstimationT fest; fest.setRadiusSearch (0.025); fest.setInputCloud (object); fest.setInputNormals (object); fest.compute (*object_features); fest.setInputCloud (scene); fest.setInputNormals (scene); fest.compute (*scene_features); // Perform alignment pcl::console::print_highlight (Starting alignment...\n); pcl::SampleConsensusPrerejectivePointNT,PointNT,FeatureT align; align.setInputSource (object); align.setSourceFeatures (object_features); align.setInputTarget (scene); align.setTargetFeatures (scene_features); align.setMaximumIterations (50000); // Number of RANSAC iterations align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose align.setCorrespondenceRandomness (5); // Number of nearest features to use align.setSimilarityThreshold (0.95f); // Polygonal edge length similarity threshold align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis { pcl::ScopeTime t(Alignment); align.align (*object_aligned); } if (align.hasConverged ()) { // Print results printf (\n); Eigen::Matrix4f transformation align.getFinalTransformation (); pcl::console::print_info ( | %6.3f %6.3f %6.3f | \n, transformation (0,0), transformation (0,1), transformation (0,2)); pcl::console::print_info (R | %6.3f %6.3f %6.3f | \n, transformation (1,0), transformation (1,1), transformation (1,2)); pcl::console::print_info ( | %6.3f %6.3f %6.3f | \n, transformation (2,0), transformation (2,1), transformation (2,2)); pcl::console::print_info (\n); pcl::console::print_info (t %0.3f, %0.3f, %0.3f \n, transformation (0,3), transformation (1,3), transformation (2,3)); pcl::console::print_info (\n); pcl::console::print_info (Inliers: %i/%i\n, align.getInliers ().size (), object-size ()); // Show alignment pcl::visualization::PCLVisualizer visu(Alignment); visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), scene); visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), object_aligned); visu.spin (); } else { pcl::console::print_error (Alignment failed!\n); return (1); } return (0); }参考刚性物体的鲁棒位姿估计Robust pose estimation of rigid objects — Point Cloud Library 0.0 documentation