pcl之FPFH配准

什么是fpfh特征

有关快速点云直方图（fpfh）特征的数学描述，在这里不做过多介绍，可以查看fpfh。也可以查看PCL的官网解释，中文版可直接搜索pcl中国fpfh。

主程序

首先还是一堆头文件(当然好多头文件在这里没用到，可自行删除)

#include <pcl/io/pcd_io.h>
#include <ctime>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/fpfh_omp.h> //包含fpfh加速计算的omp(多核并行计算)
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_features.h> //特征的错误对应关系去除
#include <pcl/registration/correspondence_rejection_sample_consensus.h> //随机采样一致性去除
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h

为了方便记：

using namespace std;
typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;
typedef pcl::PointCloud<pcl::Normal> pointnormal;
typedef pcl::PointCloud<pcl::FPFHSignature33> fpfhFeature;

为了使用fpfp特征匹配，声明一个计算fpfh特征点的函数：

fpfhFeature::Ptr compute_fpfh_feature(pointcloud::Ptr input_cloud,pcl::search::KdTree<pcl::PointXYZ>::Ptr tree)
{
        //法向量
        pointnormal::Ptr point_normal (new pointnormal);
        pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> est_normal;
        est_normal.setInputCloud(input_cloud);
        est_normal.setSearchMethod(tree);
        est_normal.setKSearch(10);
        est_normal.compute(*point_normal);
        //fpfh 估计
        fpfhFeature::Ptr fpfh (new fpfhFeature);
        //pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> est_target_fpfh;
        pcl::FPFHEstimationOMP<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> est_fpfh;
        est_fpfh.setNumberOfThreads(4); //指定4核计算
        // pcl::search::KdTree<pcl::PointXYZ>::Ptr tree4 (new pcl::search::KdTree<pcl::PointXYZ> ());
        est_fpfh.setInputCloud(input_cloud);
        est_fpfh.setInputNormals(point_normal);
        est_fpfh.setSearchMethod(tree);
        est_fpfh.setKSearch(10);
        est_fpfh.compute(*fpfh);

        return fpfh;
        
}

可以看出，在计算Fpfh特征时，首先需要计算点集的法向量（法向量是点云的一个非常重要的特征，本该单独处理，仅在这里为了方便，少写两行代码，将其封装在FPFH特征的计算中），根据计算好的法向量，计算FPFH特征。计算fpfh特征时，近邻点集个数不易取得过大，，否则一则导致计算量增大，二会使得fpfh的计算失去意义（通其他特征计算一样，过大的近邻点集合不能反映局部特征）。

主函数:

int main (int argc, char **argv)
{
        if (argc < 3)
        {
                cout<<"please input two pointcloud"<<endl;
                return -1;
        }
        clock_t start,end,time;
        start  = clock();
        pointcloud::Ptr source (new pointcloud);
        pointcloud::Ptr target (new pointcloud);
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());

        fpfhFeature::Ptr source_fpfh =  compute_fpfh_feature(source,tree);
        fpfhFeature::Ptr target_fpfh =  compute_fpfh_feature(target,tree);
        
         //对齐(占用了大部分运行时间)
        pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
        sac_ia.setInputSource(source);
        sac_ia.setSourceFeatures(source_fpfh);
        sac_ia.setInputTarget(target);
        sac_ia.setTargetFeatures(target_fpfh);
        pointcloud::Ptr align (new pointcloud);
        //  sac_ia.setNumberOfSamples(20);  //设置每次迭代计算中使用的样本数量（可省）,可节省时间
        sac_ia.setCorrespondenceRandomness(6); //设置计算协方差时选择多少近邻点，该值越大，协防差越精确，但是计算效率越低.(可省)
        sac_ia.align(*align); 
        end = clock();
        cout <<"calculate time is: "<< float (end-start)/CLOCKS_PER_SEC<<endl;
        
         //可视化
        boost::shared_ptr<pcl::visualization::PCLVisualizer> view (new pcl::visualization::PCLVisualizer("fpfh test"));
        int v1;
        int v2;
        
        view->createViewPort(0,0.0,0.5,1.0,v1);
        view->createViewPort(0.5,0.0,1.0,1.0,v2);
        view->setBackgroundColor(0,0,0,v1);
        view->setBackgroundColor(0.05,0,0,v2);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> sources_cloud_color(source,250,0,0);
        view->addPointCloud(source,sources_cloud_color,"sources_cloud_v1",v1);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_cloud_color (target,0,250,0);
        view->addPointCloud(target,target_cloud_color,"target_cloud_v1",v1);
        view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"sources_cloud_v1");

        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>aligend_cloud_color(final,255,0,0);
        view->addPointCloud(align,aligend_cloud_color,"aligend_cloud_v2",v2);
        view->addPointCloud(target,target_cloud_color,"target_cloud_v2",v2);
        view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,4,"aligend_cloud_v2");
        view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"target_cloud_v2");

        //   view->addCorrespondences<pcl::PointXYZ>(source,target,*cru_correspondences,"correspondence",v1);//添加显示对应点对
        while (!view->wasStopped())
        {
                // view->spin();
                view->spinOnce(100);
                boost::this_thread::sleep (boost::posix_time::microseconds (100000));
                  

        }
         pcl::io::savePCDFile ("crou_output.pcd", *align);
         //  pcl::io::savePCDFile ("final_align.pcd", *final);
        
        return 0;
}

采用FPFH特征配准，效果不错，但是计算效率非常低，尤其针对大规模点云数据时。所以，很多时候，都先对原始点云进行简化，对简化后的数据做配准计算，在将所获得的配准参数应用到原始点云，以提高计算效率。

体素网格简化

主要程序：

        //pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_grid;
        pcl::VoxelGrid<pcl::PointXYZ> approximate_voxel_grid;
        approximate_voxel_grid.setLeafSize(0.5,0.5,0.5); //网格边长.这里的数值越大，则精简的越厉害（剩下的数据少）
        pointcloud::Ptr source (new pointcloud);
        pointcloud::Ptr sample_sources (new pointcloud);
        approximate_voxel_grid.setInputCloud(source);
        approximate_voxel_grid.filter(*sample_source);
        cout << "source voxel grid  Filte cloud size is " << sample_source->size()<<endl;
        // pcl::io::savePCDFile("voxelgrid.pcd",*out);

针对体素网格简化，PCL提供了两种方法：其一，pcl::ApproximateVoxelGrid<pcl::PointXYZ> 类；其二， pcl::VoxelGrid<pcl::PointXYZ>类。可以看出，第二中比第一中少了“大约”approximate，也就是说第二种某些情况下比第一种更精确。原因是：第一种方法是利用体素网格的中心（长方体的中心）代替原始点，而第二种则是对体素网格中所有点求均值，以期望均值点代替原始点集

可视化

在如上主程序中，已经包含了可视化的功能，更过可视化可看我的博客pcl可视化那些事，在这里，细致的讲一下如何添加对应点对的可视化功能。
要可视化对应关系，首先需要计算对应关系,本文配准为例：

      pcl::registration::CorrespondenceEstimation<pcl::FPFHSignature33,pcl::FPFHSignature33> crude_cor_est;

      boost::shared_ptr<pcl::Correspondences> cru_correspondences (new pcl::Correspondences);
      crude_cor_est.setInputSource(source_fpfh);
      crude_cor_est.setInputTarget(target_fpfh);
        //  crude_cor_est.determineCorrespondences(cru_correspondences);
      crude_cor_est.determineReciprocalCorrespondences(*cru_correspondences);
      cout<<"crude size is:"<<cru_correspondences->size()<<endl;

效果(粗配)