PCL库之快速点特征直方图(FPFH)描述子

对于实时应用或接近实时应用中，密集点云的点特征直方图（PFH）的计算，是一个主要的性能瓶颈。此处为PFH计算方式的简化形式，称为快速点特征直方图FPFHFast 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[1];
    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) < 0)
    {
      std::cerr << "Error loading model cloud." << std::endl;
      return (-1);
    }


    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(0.01, 0.01, 0.01);
        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(10);
    //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(0.02); // 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 = 33;
    fwrite(&nV, sizeof(int), 1, fid);            // size_t fwrite(const void *ptr, size_t size, size_t nmemb, FILE *stream)
    fwrite(&nDim, sizeof(int), 1, fid);
    for (int v = 0; v < nV; v++)
    {
        const pcl::PointNormal &pt = normals->points[v];
        float xyz[3] = { pt.normal_x, pt.normal_y, pt.normal_z };
        fwrite(xyz, sizeof(float), 3, fid);
        const pcl::FPFHSignature33 &feature = fpfhs->points[v];
        fwrite(feature.histogram, sizeof(float), 33, 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(0, 0, 0);
        viewer->addPointCloud<PointT>(model, "model");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "model");
        viewer->addPointCloudNormals<PointT, pcl::PointNormal>(model, normals, 10, 0.05, "normals");  //display every 1 points, and the scale of the arrow is 10
        viewer->addCoordinateSystem(1.0);
        viewer->initCameraParameters();
        while (!viewer->wasStopped())
        {
            viewer->spinOnce(100);
            boost::this_thread::sleep(boost::posix_time::microseconds(100000));
        }
    }
  return 0;
}

对应的CMakeLists.txt如下：

# set project's name
PROJECT( PointCloudSegmentation )

###############################################################################
# CMake settings
CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3)

add_definitions(-std=c++11)

# 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 1.8 REQUIRED)


add_definitions(${PCL_DEFINITIONS})

INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS})




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