PCL庫之快速點特征直方圖(FPFH)描述子

對于實時應用或接近實時應用中，密集點雲的點特征直方圖（PFH）的計算，是一個主要的性能瓶頸。此處為PFH計算方式的簡化形式，稱為快速點特征直方圖FPFH

Fast Point Feature Histogram

。由于大大地降低了FPFH的整體複雜性，是以FPFH有可能使用在實時應用中。

對于計算速度要求苛刻的使用者，PCL提供了一個FPFH估計的另一實作，它使用多核/多線程規範，利用OpenMP開發模式來提高計算速度。這個類的名稱是

pcl::FPFHEstimationOMP

，并且它的應用程式接口（API）100%相容單線程

pcl::FPFHEstimation

，這使它适合作為一個替換元件。在8核系統中，OpenMP的實作可以在6-8倍更快的計算時間内完全同樣單核系統上的計算。

//main.cpp
#include <iostream>
#include <string>
#include <stdio.h>
#include <Eigen/Core>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/correspondence.h>                 //對應表示兩個實體之間的比對（例如，點，描述符等）。
#include <pcl/features/normal_3d.h>             //法線

#include <pcl/filters/uniform_sampling.h>    //均勻采樣
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>   //可視化
#include <pcl/common/transforms.h>              //轉換矩陣
#include <pcl/console/parse.h>
#include <pcl/point_types.h>

#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
using namespace std;

typedef pcl::PointXYZRGB PointT;             //Point with color
typedef pcl::PointCloud<PointT> PointCloud; //PointCloud with color

int main(int argc, char *argv[]){
    string model_filename = argv[];
    bool display = true;
    bool downSampling = false;

    //load pcd/ply point cloud
    pcl::PointCloud<PointT>::Ptr model(new pcl::PointCloud<PointT>()); //模型點雲
    /*
    if (pcl::io::loadPCDFile<PointT>(model_filename, *model)<0)
    {
      return -1;
    }
    */
    if (pcl::io::loadPCDFile(model_filename, *model) < )
    {
      std::cerr << "Error loading model cloud." << std::endl;
      return (-);
    }


    if (downSampling)
    {
        //create the filtering object
        std::cout << "Number of points before downSampling: " << model->points.size() << std::endl;
        pcl::VoxelGrid<PointT> sor;
        sor.setInputCloud(model);
        sor.setLeafSize(, , );
        sor.filter(*model);
        std::cout << "Number of points after downSampling: " << model->points.size() << std::endl;
    }

    // Normal estimation
    pcl::NormalEstimation<PointT, pcl::PointNormal> ne;
    pcl::PointCloud<pcl::PointNormal>::Ptr normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
    ne.setInputCloud(model);
    ne.setSearchMethod(tree);
    ne.setKSearch();
    //boost::shared_ptr<pcl::PointIndices> indices;
    //indices->indices.push_back(200);
    //ne.setIndices(indices);
    ne.compute(*normals);
    /*
    for(int i = 100; i<101; ++i)
        printf("points[%d]=[%f, %f, %f], normal=[%f, %f, %f], curvature = %f\n",i, model->points[i].x, model->points[i].y, model->points[i].z, normals->points[i].normal_x, normals->points[i].normal_y, normals->points[i].normal_z, normals->points[i].curvature);
    //for (int i = 0; i < normals->points.size(); ++i)
    //  printf("points[%f, %f, %f], normal[%f, %f, %f]", normals->points[i].x, normals->points[i].y, normals->points[i].z, normals->points[i].normal_x, normals->points[i].normal_x, normals->points[i].y, normals->points[i].normal_z);
    */
    //create the FPFH estimation class, and pass the input dataset+normals to it
    pcl::FPFHEstimationOMP<PointT, pcl::PointNormal, pcl::FPFHSignature33> fpfh;
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh.setInputCloud(model);
    fpfh.setInputNormals(normals);
    fpfh.setSearchMethod(tree);
    fpfh.setRadiusSearch(); // Use all neighbors in a sphere of radius 5cm, the radius used here has to be larger than the radius used to estimate the surface normals!!!
    fpfh.compute(*fpfhs);// Compute the features


    FILE* fid = fopen("source.bin", "wb");
    int nV = normals->size(), nDim = ;
    fwrite(&nV, sizeof(int), , fid);            // size_t fwrite(const void *ptr, size_t size, size_t nmemb, FILE *stream)
    fwrite(&nDim, sizeof(int), , fid);
    for (int v = ; v < nV; v++)
    {
        const pcl::PointNormal &pt = normals->points[v];
        float xyz[] = { pt.normal_x, pt.normal_y, pt.normal_z };
        fwrite(xyz, sizeof(float), , fid);
        const pcl::FPFHSignature33 &feature = fpfhs->points[v];
        fwrite(feature.histogram, sizeof(float), , fid);
    }
    fclose(fid);

    if (display)
    {
        // Concatenate the XYZ and normal fields*
        //pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>());
        //pcl::concatenateFields(*model, *normals, *cloud_with_normals);
        //pcl::io::savePLYFile("result.ply", *cloud_with_normals);
        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Viewer"));
        viewer->setBackgroundColor(, , );
        viewer->addPointCloud<PointT>(model, "model");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "model");
        viewer->addPointCloudNormals<PointT, pcl::PointNormal>(model, normals, , , "normals");  //display every 1 points, and the scale of the arrow is 10
        viewer->addCoordinateSystem();
        viewer->initCameraParameters();
        while (!viewer->wasStopped())
        {
            viewer->spinOnce();
            boost::this_thread::sleep(boost::posix_time::microseconds());
        }
    }
  return ;
}
           

對應的

CMakeLists.txt

如下：

# set project's name
PROJECT( PointCloudSegmentation )

###############################################################################
# CMake settings
CMAKE_MINIMUM_REQUIRED(VERSION .)

add_definitions(-std=c++)

# OpenMP
FIND_PACKAGE(OpenMP)
IF(OPENMP_FOUND)
    OPTION(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON)
ENDIF(OPENMP_FOUND)

IF(OPENMP_FOUND AND WITH_OPENMP)
    MESSAGE(STATUS "With OpenMP ")
    SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${PROCESSOR_COUNT} -DOPENMP_NUM_THREADS=${PROCESSOR_COUNT} ${OpenMP_CXX_FLAGS} -DOPENMP")
    SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DMAX_OPENMP_NUM_THREADS=${PROCESSOR_COUNT} -DOPENMP_NUM_THREADS=${PROCESSOR_COUNT} ${OpenMP_CXX_FLAGS} ${OpenMP_C_FLAGS} -DOPENMP")
ELSE(OPENMP_FOUND AND WITH_OPENMP)
    MESSAGE(STATUS "Without OpenMP")
    SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1")
    SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1")
ENDIF(OPENMP_FOUND AND WITH_OPENMP)

# OpenCV
FIND_PACKAGE(OpenCV REQUIRED)
FIND_PACKAGE(PCL  REQUIRED)


add_definitions(${PCL_DEFINITIONS})

INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS})




add_executable(main src/main.cpp)
target_link_libraries(main  ${PCL_LIBRARIES})