在上一篇文章【点云处理】点云法向量估计及其加速(4)中我们尝试对pcl自带的KDTree的k近邻搜索过程使用OpenMP加速，效果比较明显，有将近1倍的提速。在这篇文章中我们暂时放弃pcl自带的KDTree，转而使用另一大杀器nanflann库提供的KDTree。nanoflann是一个c++11标准库，用于构建具有不同拓扑（R2，R3（点云），SO(2)和SO(3)（2D和3D旋转组））的KD树。nanoflann 算法对fastann进行了改进，效率以及内存使用等方面都进行了优化，而且代码十分轻量级且开源。nanoflann不需要编译或安装，你只需要在你的代码中加入#include 即可方便快捷地使用它。好了，下面使用nanoflann对我们的代码进行改写。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "KDTreeTableAdaptor.h"int main(int argc, char** argv) {ros::init(argc, argv, "n_lidar_gpu_normal");ros::NodeHandle node;pcl::PointCloud::Ptr cloud(new pcl::PointCloud);const boost::function&)> callback =[&](sensor_msgs::PointCloud2::ConstPtr msg_pc_ptr) {pcl::fromROSMsg(*msg_pc_ptr, *cloud);size_t cloud_size = cloud->size();int dim=3,k=10; float* points = new float[cloud_size*dim];for (int i=0; ipoints[i].x;p[1] = cloud->points[i].y;p[2] = cloud->points[i].z;}auto t1 = std::chrono::steady_clock::now();KDTreeTableAdaptor  kdtree(cloud_size, dim, points, 64);std::vector> neighbors_all(cloud_size,std::vector(k));std::vector sizes(cloud_size,k);for (int i=0; i out_ids(k);std::vector out_dists_sqr(k);nanoflann::KNNResultSet result_set(k);result_set.init(&out_ids[0], &out_dists_sqr[0]);kdtree.index->findNeighbors(result_set, &points[i*dim], nanoflann::SearchParams(k));for (int j=0; j flatten_neighbors_all(k * cloud_size);pcl::gpu::PtrStep ps(&flatten_neighbors_all[0], k * pcl::gpu::PtrStep::elem_size);for (size_t i=0; ipoints);gpu_neighbor_indices.upload(flatten_neighbors_all, sizes, k);pcl::gpu::NormalEstimation::Normals gpu_normals;pcl::gpu::NormalEstimation::computeNormals(gpu_cloud, gpu_neighbor_indices, gpu_normals);pcl::gpu::NormalEstimation::flipNormalTowardsViewpoint(gpu_cloud, 0.f, 0.f, 0.f, gpu_normals);auto t3 = std::chrono::steady_clock::now();auto compute_normal_time = std::chrono::duration(t3 - t2);std::vector normals;gpu_normals.download(normals);auto t4 = std::chrono::steady_clock::now();auto knn_time = std::chrono::duration(t2-t1);auto total_time = std::chrono::duration(t4-t1);spdlog::info("cloud size:{:d}, knn_time:{:.3f} ms,compute_normal_time:{:.3f} ms, total_time:{:.3f} ms", cloud->size(), knn_time.count(), compute_normal_time.count(), total_time.count());};ros::Subscriber pc_sub = node.subscribe("/BackLidar/lslidar_point_cloud", 1, callback);ros::spin();return 0;
}

对于for循环遍历查找k近邻索引部分我们先不加"# pragma omp parallel for"，编译运行。

  哇,加速效果明显,8w点云knn时间从400ms降到150ms左右。比pcl自带KDTree使用上OpenMP并行加速还要快。

要是能利用OpenMP做并行加速，岂不是要起飞??!!加上OpenMP加速试一试。

# pragma omp parallel forfor (int i=0; i out_ids(k);std::vector out_dists_sqr(k);nanoflann::KNNResultSet result_set(k);result_set.init(&out_ids[0], &out_dists_sqr[0]);kdtree.index->findNeighbors(result_set, &points[i*dim], nanoflann::SearchParams(k));for (int j=0; j