PCL：刚性物体的位姿估计

#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.h> #include <pcl/features/fpfh.h> #include <pcl/filters/filter.h> #include <pcl/filters/voxel_grid.h> #include <pcl/io/pcd_io.h> #include <pcl/registration/icp.h> #include <pcl/registration/sample_consensus_prerejective.h> #include <pcl/segmentation/sac_segmentation.h> #include <pcl/visualization/pcl_visualizer.h> #pragma comment(lib,"User32.lib") #pragma comment(lib, "gdi32.lib") // Types typedef pcl::PointNormal PointNT; typedef pcl::PointCloud<PointNT> PointCloudT; typedef pcl::FPFHSignature33 FeatureT; //FPFH特征描述子 typedef pcl::FPFHEstimation<PointNT, PointNT, FeatureT> FeatureEstimationT; //FPFH特征估计类 typedef pcl::PointCloud<FeatureT> FeatureCloudT; typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> 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(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); // } // 加载目标物体和场景点云 pcl::console::print_highlight("Loading point clouds...\n"); if (pcl::io::loadPCDFile<PointNT>("chef.pcd", *object) < 0 || pcl::io::loadPCDFile<PointNT>("rs1.pcd", *scene) < 0) { pcl::console::print_error("Error loading object/scene file!\n"); return (1); } // 下采样：使用0.005提速分辨率对目标物体和场景点云进行空间下采样 pcl::console::print_highlight("Downsampling...\n"); pcl::VoxelGrid<PointNT> grid; const float leaf = 0.005f; grid.setLeafSize(leaf, leaf, leaf); grid.setInputCloud(object); grid.filter(*object); grid.setInputCloud(scene); grid.filter(*scene); // 估计场景法线 pcl::console::print_highlight("Estimating scene normals...\n"); pcl::NormalEstimation<PointNT, PointNT> nest; //pcl::NormalEstimationOMP<PointNT, PointNT> nest; nest.setRadiusSearch(0.01); nest.setInputCloud(scene); nest.compute(*scene); // 特征估计 pcl::console::print_highlight("Estimating features...\n"); FeatureEstimationT fest; fest.setRadiusSearch(0.025);//该搜索半径决定FPFH特征描述的范围，一般设置为分辨率10倍以上 fest.setInputCloud(object); fest.setInputNormals(object); fest.compute(*object_features); fest.setInputCloud(scene); fest.setInputNormals(scene); fest.compute(*scene_features); // 实施配准 pcl::console::print_highlight("Starting alignment...\n"); pcl::SampleConsensusPrerejective<PointNT, PointNT, FeatureT> align;//基于采样一致性的位姿估计 align.setInputSource(object); align.setSourceFeatures(object_features); align.setInputTarget(scene); align.setTargetFeatures(scene_features); align.setMaximumIterations(50000); // 采样一致性迭代次数 align.setNumberOfSamples(3); // 创建假设所需的样本数，为了正常估计至少需要3点 align.setCorrespondenceRandomness(5); // 使用的临近特征点的数目 align.setSimilarityThreshold(0.9f); // 多边形边长度相似度阈值 align.setMaxCorrespondenceDistance(2.5f * 0.005); // 判断是否为内点的距离阈值 align.setInlierFraction(0.25f); //接受位姿假设所需的内点比例 { 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("点云库PCL学习教程第二版-鲁棒位姿估计"); int v1(0), v2(0); visu.createViewPort(0, 0, 0.5, 1, v1); visu.createViewPort(0.5, 0, 1, 1, v2); visu.setBackgroundColor(255, 255, 255, v1); visu.addPointCloud(scene, ColorHandlerT(scene, 0.0, 255.0, 0.0), "scene", v1); visu.addPointCloud(object_aligned, ColorHandlerT(object_aligned, 0.0, 0.0, 255.0), "object_aligned", v1); visu.addPointCloud(object, ColorHandlerT(object, 0.0, 255.0, 0.0), "object_before_aligned", v2); visu.addPointCloud(scene, ColorHandlerT(scene, 0.0, 0.0, 255.0), "scene_v2", v2); visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scene"); visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "object_aligned"); visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "object_before_aligned"); visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scene_v2"); visu.spin(); } else { pcl::console::print_error("Alignment failed!\n"); return (1); } return (0); }