【点云处理】点云法向量估计及其加速(5)_nanoflann r-tree_昌山小屋的博客

相关文章推荐

直爽的柿子 · 【python量化】将Informer用于股 ...· 1 年前 ·

痴情的凉面 · javascript - ...· 1 年前 ·

阳刚的伏特加 · Xlwings相关知识点以及自动刷excel ...· 1 年前 ·

在上一篇文章【点云处理】点云法向量估计及其加速(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.hpp>即可方便快捷地使用它。好了，下面使用nanoflann对我们的代码进行改写。

#include <time.h>
#include <cmath>
#include <functional>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/console/time.h>
#include <pcl/gpu/features/features.hpp>
#include <omp.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <spdlog/spdlog.h>
#include "KDTreeTableAdaptor.h"
int main(int argc, char** argv) {
    ros::init(argc, argv, "n_lidar_gpu_normal");
    ros::NodeHandle node;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    const boost::function<void (const boost::shared_ptr<sensor_msgs::PointCloud2 const>&)> 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; i<cloud_size; i++) {
            float* p = points + i*dim;
            p[0] = cloud->points[i].x;
            p[1] = cloud->points[i].y;
            p[2] = cloud->points[i].z;
        auto t1 = std::chrono::steady_clock::now();
        KDTreeTableAdaptor<float,float>  kdtree(cloud_size, dim, points, 64);
        kdtree.index->buildIndex();        
std::vector<std::vector<int>> neighbors_all(cloud_size,std::vector<int>(k));
        std::vector<int> sizes(cloud_size,k);
        for (int i=0; i<cloud_size; i++) {
            std::vector<size_t> out_ids(k);
            std::vector<float> out_dists_sqr(k);
            nanoflann::KNNResultSet<float> 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<k; j++) {
                neighbors_all[i][j] = out_ids[j];
        auto t2 = std::chrono::steady_clock::now();
        delete []points;
        std::vector<int> flatten_neighbors_all(k * cloud_size);
        pcl::gpu::PtrStep<int> ps(&flatten_neighbors_all[0], k * pcl::gpu::PtrStep<int>::elem_size);
	    for (size_t i=0; i<cloud_size; ++i) {
		    std::copy(neighbors_all[i].begin(), neighbors_all[i].end(), ps.ptr(i));
        pcl::gpu::NeighborIndices gpu_neighbor_indices;
        pcl::gpu::NormalEstimation::PointCloud gpu_cloud;
        gpu_cloud.upload(cloud->points);
        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<double,std::milli>(t3 - t2);
	    std::vector<pcl::PointXYZ> normals;
	    gpu_normals.download(normals);
        auto t4 = std::chrono::steady_clock::now();
        auto knn_time = std::chrono::duration<double,std::milli>(t2-t1);
        auto total_time = std::chrono::duration<double,std::milli>(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<sensor_msgs::PointCloud2>("/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 for
        for (int i=0; i<cloud_size; i++) {
            std::vector<size_t> out_ids(k);
            std::vector<float> out_dists_sqr(k);
            nanoflann::KNNResultSet<float> 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<k; j++) {
                neighbors_all[i][j] = out_ids[j];
 编译运行，测试结果如下： 
哇，虽然有波动，但常能在50ms左右徘徊，相比曾几何时的400ms提速了8倍。 多核算力分配也很均衡，完美! 
 所以，knn加速哪家强，nanoflann+OpenMP当称王!!! 
【参考文献】 
在C++中使用openmp进行多线程编程_线上幽灵的博客-CSDN博客_c++ omp 
PCL GPU实现计算法线和曲率 - 知乎 
                    中我们尝试对pcl自带的KDTree的k近邻搜索过程使用OpenMP加速，但是结果不太理想。在这篇文章中我们暂时放弃pcl自带的KDTree，转而使用另一大杀器nanflann库提供的KDTree。nanoflann是一个c++11标准库，用于构建具有不同拓扑（R2，R3（点云），SO(2)和SO(3)（2D和3D旋转组））的KD树。nanoflann不需要编译或安装，你只需要在你的代码中加入#include 即可方便快捷地使用它。尽管没有并行，造成单核几乎被打满。
				纳米法兰
nanoflann是一个C ++ 11库，用于构建具有以下拓扑的数据集的KD树：R 2 ，R 3 （点云），SO（2）和SO（3）（2D和3D旋转组）。 不提供对近似NN的支持。 nanoflann不需要编译或安装。 您只需要在代码中#include <nanoflann> 。
 该库是Marius Muja和David G. Lowe的（  ）的分支，并作为子项目而。 遵循原始许可条款， nanoflann根据BSD许可进行分发。 请，对于bug，请使用问题按钮或fork并打开请求请求。
 @misc{blanco2014nanoflann,
  title        = {nanoflann: a {C}++ header-only fork of {FLANN}, a library for Nearest Neighbor ({NN}) with KD-trees},
  author       = {Blanco, Jose Luis and Rai, Pranjal Kumar},
  howpublished = {\url{
				记录使用nanoflann时候的坑
nanoflann库的中文博客介绍 nanoflann中文博客:
nanoflann库的英文介绍 nanoflann的github:
对nanoflann的使用评价
及其轻量化： ，只用将极少代码问价copy到现有项目中即可实现KNN搜索；
简单易学： github上贴心的提供了实例代码，轻松上手；
**功能强大：**实现多个空间的近邻搜索，对L1，L2，SO2和SO3空间进行距离度量 ；
实例代码容易引起歧义： 没有对输入和输出参数的具体描述，很容
sh compile_op.sh
运行后我报了：error: could not create 'build/temp.linux-x86_64-3.5'的错，网上的解决方法包括：
（1）chmod -R 777 xxx
（2）sudo python sh xxx
（3）python sh xxx --user
其中，s...
				typedef KDTreeSingleIndexAdaptor<L2_Simple_Adaptor<double, PointCloud_flann<double> >, PointCloud_flann<double>, 3> my_kd_tree;
pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud(new pcl::PointCloud<pcl::PointXYZ>);
// pcl kdtr
				Binary Search TreeNearest Neighbor (NN) ProblemK-NNFixed Radius-NNCore IdeasBST
Nearest Neighbor (NN) Problem
Given a set of points SSS in a space MMM, a query point q∈Mq{\in}Mq∈M, find the kkk closest points in SSS
Fixed Radius-NN
Given a set of p
功能：进行点云匹配，完成运动估计（帧间匹配）。
具体实现：利用ScanRegistration中提取到的特征点，建立相邻时间点云数据之间的关联，由此推断lidar的运动。
备注：绿色的函数指在下面解释，红色的函数指本文单独的函数解释
代码流程：
1、主函数： main
/** Main node entry point. */
int main(int argc, char ...
* Developed by Jakob Engel <engelj at in dot tum dot de>,
* for more information see <http://vision.in.tum.de/dso>.
* If you use this code, please ci.
				计算点云法向量是许多点云处理任务的基础操作之一。PCL提供了多种方法来计算点云的法向量，其中一种常用的方法是使用最近邻搜索（Nearest Neighbor Search）。
具体来说，可以使用`pcl::NormalEstimation`类来计算点云的法向量。该类将点云视为一个无序点云，并为每个点计算一个法向量。`pcl::NormalEstimation`类内部使用最近邻搜索来查找每个点周围的邻居，并使用主成分分析（PCA）来计算每个点的法向量。可以使用以下代码片段来计算点云的法向量：
```cpp
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(tree);
ne.setKSearch(20);
ne.compute(*normals);
在上面的代码中，我们首先创建了一个`pcl::PointCloud<pcl::PointXYZ>`对象，它表示输入点云。然后，我们创建了一个`pcl::PointCloud<pcl::Normal>`对象，它将存储计算出的法向量。接下来，我们创建了一个`pcl::search::KdTree<pcl::PointXYZ>`对象，它将用于最近邻搜索。然后，我们创建了一个`pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal>`对象，并将输入点云和搜索方法设置为最近邻搜索。我们还设置了`setKSearch()`函数，以便每个点的20个最近邻将用于计算法向量。最后，我们调用`compute()`函数来计算点云的法向量，并将结果存储在`normals`对象中。
值得注意的是，这种方法计算的法向量是基于每个点周围的邻居计算得到的，并且可能会受到点云密度和采样间隔等因素的影响。如果需要更精确的法向量计算，可以考虑使用基于曲率的方法或基于深度学习的方法等。