匹配器

2.1 ICP點(diǎn)云精配準(zhǔn)

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對(duì)象       
 icp.setInputSource(input_obj);                            //設(shè)置輸入源點(diǎn)云       
 icp.setInputTarget(input_scene);                          //設(shè)置輸入目標(biāo)點(diǎn)云        // 
Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)        icp.setMaxCorrespondenceDistance(max_distance); //設(shè)置最大匹配距離 0.05        // 
Set the maximum number of iterations (criterion 1)        icp.setMaximumIterations(max_iteration); //設(shè)置最大迭代次數(shù) 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); //設(shè)置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點(diǎn)云粗配準(zhǔn)

void FPFH_generation(pcl::PointCloud<PointXYZRGB>::Ptr &input, FPFHCloud::Ptr &output) // FPFH特征提取    {        // 首先,生成法線       
 pcl::NormalEstimationOMP<PointNormal, PointNormal> nest;                  // OMP線程數(shù)       
 pcl::PointCloud<PointNormal>::Ptr temp(new pcl::PointCloud<PointNormal>); //構(gòu)建暫時(shí)點(diǎn)云       
 pcl::copyPointCloud(*input, *temp);                                       //拷貝點(diǎn)云        
nest.setRadiusSearch(0.01);                                               //設(shè)置半徑搜索范圍       
 nest.setInputCloud(temp);                                                 //設(shè)置輸入點(diǎn)云       
 nest.compute(*temp);                                                      //計(jì)算暫時(shí)點(diǎn)云        // 然后生成FPFH點(diǎn)云       pcl::FPFHEstimationOMP<PointNormal, PointNormal, FPFH> fest; // OMP線程數(shù)       
 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粗配準(zhǔn)方法   
 {        pcl::SampleConsensusPrerejective<PointType, PointType, FPFH> align; //采樣一致性預(yù)排除算法        
align.setInputSource(object);                                       //設(shè)置輸入源點(diǎn)云        
align.setSourceFeatures(object_feature);                            //設(shè)置輸入源特征點(diǎn)云       
 align.setInputTarget(scene);                                        //設(shè)置輸入目標(biāo)點(diǎn)云       
 align.setTargetFeatures(scene_feature);                             //設(shè)置輸入目標(biāo)特征點(diǎn)云       
 align.setMaximumIterations(5000);                                   // 設(shè)置最大迭代次數(shù)       
 align.setNumberOfSamples(7);                                        // 設(shè)置采樣點(diǎn)數(shù)       
 align.setCorrespondenceRandomness(10);                              // 設(shè)置隨機(jī)匹配點(diǎn)數(shù)       
 align.setSimilarityThreshold(0.5f);                                 // 設(shè)置相似度閾值       
 align.setMaxCorrespondenceDistance(0.01f);                          // 設(shè)置最大匹配距離       
 align.setInlierFraction(0.05f);                                     // 設(shè)置內(nèi)點(diǎn)比例        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點(diǎn)云粗配準(zhǔn)

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