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;

效果(粗配)