对于实时应用或接近实时应用中，密集点云的点特征直方图（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})