【点云处理】Jetson平台pcl计算全局点云k近邻

        传统激光点云聚类算法如欧式聚类、区域生长等都需要获取点云的邻域信息，PCL中提供了KD树来进行点云的邻域搜索。最近的工作需要在Jetson agx xavier平台上完成，訪平台性能规格参数如下：

部分工作需要一个高效的全局点云k近邻查找算法，也就基于訪平台做了一些研究和实验，在此记录下来。这里使用pcl自带的kdtree来构造全局点云的k近邻表，并测试其性能表现，核心代码如下：

#include <chrono>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>

int main (int argc, char** argv) {
    // Read in the cloud data
    pcl::PCDReader reader;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    reader.read (argv[1], *cloud);
    std::cout << "PointCloud size: " << cloud->points.size () << std::endl;

    int k=10;
    int n_points = cloud->size();
    std::vector<std::vector<int>> points_k_near_index(n_points, std::vector<int>(k));
    std::vector<std::vector<float>> points_k_near_dist(n_points, std::vector<float>(k));

    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double, std::ratio<1, 1000>> time_span = std::chrono::duration_cast<std::chrono::duration<double, std::ratio<1, 1000>>>(t2 - t1);

    t1 = std::chrono::steady_clock::now();
    // Creating the KdTree object for the search method of the extraction
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud);

    for (int i=0; i<n_points; i++) {
        auto& point = cloud->points.at(i);
        int n_near = tree->nearestKSearch(point, k, points_k_near_index[i], points_k_near_dist[i]);
        //printf("point[%d] x:%.3f y:%.3f z:%.3f n_near:%d\n",i,point.x,point.y,point.z,n_near);
    }
    t2 = std::chrono::steady_clock::now();
    time_span = std::chrono::duration_cast<std::chrono::duration<double, std::ratio<1, 1000>>>(t2 - t1);
    printf("time cost: %.3f ms.\n", time_span.count());
    return (0);
}

我选用了一个包含56815个点的pcd文件进行测试，运行如下：
nvidia@nvidia-desktop $ ./cluster_extraction 2021-01-06-11-45-45_9_frame_000326.pcd
PointCloud size: 56815
time cost: 223.494 ms.

显然，对于要满足实时性要求来说，200多毫秒的建表时间过于耗时。

【参考文献】

AI 助力的大批自主机器 | NVIDIA Jetson AGX Xavier