PCL匹配器滤除点云方法

匹配器

2.1 ICP点云精配准

template < typename PointCloudPtr >    bool ex_segmentor::icp_registration(PointCloudPtr &input_obj, PointCloudPtr &input_scene, PointCloudPtr &output_obj, Eigen::Matrix4f &result_transform, float &result_error, uint max_iteration, float max_distance, float ransac_th) // icp匹配   
 {        pcl::IterativeClosestPoint< PointXYZRGB, PointXYZRGB > icp; // icp对象       
 icp.setInputSource(input_obj);                            //设置输入源点云       
 icp.setInputTarget(input_scene);                          //设置输入目标点云        // 
Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)        icp.setMaxCorrespondenceDistance(max_distance); //设置最大匹配距离 0.05        // 
Set the maximum number of iterations (criterion 1)        icp.setMaximumIterations(max_iteration); //设置最大迭代次数 50        // Set the transformation epsilon (criterion 2)        // 
icp.setTransformationEpsilon (1e-5);  //1e-8        // 
Set the euclidean distance difference epsilon (criterion 3)        //
 icp.setEuclideanFitnessEpsilon (0.000001);  //1       
 icp.setRANSACOutlierRejectionThreshold(ransac_th); //设置RANSAC阈值       
 icp.align(*output_obj);                            // icp匹配        if (icp.hasConverged())                            //如果icp匹配成功     
   {            result_transform = icp.getFinalTransformation(); //获取最终变换矩阵            
result_error = icp.getFitnessScore();            //获取最终匹配误差            
return true;        } 
       else      
  {            result_transform = Eigen::Matrix4f::Identity(4, 4);            result_error = 1.0;            return false;        }    }/**    icp_registration(object_aligned, cluster, Final, icp_result_transform, icp_error);**/

2.2 FPFH点云粗配准

void FPFH_generation(pcl::PointCloud<PointXYZRGB>::Ptr &input, FPFHCloud::Ptr &output) // FPFH特征提取    {        // 首先,生成法线       
 pcl::NormalEstimationOMP<PointNormal, PointNormal> nest;                  // OMP线程数       
 pcl::PointCloud<PointNormal>::Ptr temp(new pcl::PointCloud<PointNormal>); //构建暂时点云       
 pcl::copyPointCloud(*input, *temp);                                       //拷贝点云        
nest.setRadiusSearch(0.01);                                               //设置半径搜索范围       
 nest.setInputCloud(temp);                                                 //设置输入点云       
 nest.compute(*temp);                                                      //计算暂时点云        // 然后生成FPFH点云       pcl::FPFHEstimationOMP<PointNormal, PointNormal, FPFH> fest; // OMP线程数       
 fest.setRadiusSearch(0.01);                                  // 0.025        
fest.setInputCloud(temp);       
 fest.setInputNormals(temp);        fest.compute(*output);    }    
template <typename PointType, typename PointCloudPtr>    bool 
FPFH_matching(PointCloudPtr &object, FPFHCloud::Ptr &object_feature, PointCloudPtr &scene, FPFHCloud::Ptr &scene_feature, PointCloudPtr &result_cloud, Eigen::Matrix4f &result_transformation) // FPFH粗配准方法   
 {        pcl::SampleConsensusPrerejective<PointType, PointType, FPFH> align; //采样一致性预排除算法        
align.setInputSource(object);                                       //设置输入源点云        
align.setSourceFeatures(object_feature);                            //设置输入源特征点云       
 align.setInputTarget(scene);                                        //设置输入目标点云       
 align.setTargetFeatures(scene_feature);                             //设置输入目标特征点云       
 align.setMaximumIterations(5000);                                   // 设置最大迭代次数       
 align.setNumberOfSamples(7);                                        // 设置采样点数       
 align.setCorrespondenceRandomness(10);                              // 设置随机匹配点数       
 align.setSimilarityThreshold(0.5f);                                 // 设置相似度阈值       
 align.setMaxCorrespondenceDistance(0.01f);                          // 设置最大匹配距离       
 align.setInlierFraction(0.05f);                                     // 设置内点比例        align.align(*result_cloud);        if (align.hasConverged())      
  {            result_transformation = align.getFinalTransformation(); //获取最终变换矩阵            // pcl::console::print_info("Inliers: %i/%i , %in", align.getInliers().size(), scene->size(), object->size());            // return (float(align.getInliers().size()) / float(object->size()));            return true;        }        // return 0.0f;        return false;    }/**   
 FPFHCloud::Ptr cluster_feature(new FPFHCloud);    FPFH_generation(cluster, cluster_feature);   
 ROS_INFO("cluster_size : %d, feature size : %d", cluster->size(), cluster_feature->size());   
 bool FPFH_match_success = FPFH_matching<PointXYZRGB>(object_, object_feature_, cluster, cluster_feature, object_aligned, align_result_transform);**/

2.3 PCA点云粗配准

void ex_segmentor::PCA_registration(pcl::PointCloud< PointXYZRGB >::Ptr