PCL---点云法向量(1)

       对作者 昌山小屋 的【点云处理】点云法向量估计及其加速(3)_昌山小屋的博客-CSDN博客的实例程序进行复现学习。

PCL GPU计算点云法向量

        实例代码如下【引用原作者的实例代码】

#include <time.h>
#include <cmath>
#include <functional>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/console/time.h>
#include <pcl/kdtree/kdtree.h>
#include<pcl/gpu/features/features.hpp>
#include <pcl/features/normal_3d_omp.h>
#include <pcl_conversions/pcl_conversions.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);
        
        auto t1 = clock();
        pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointXYZ>);
        kdtree->setInputCloud(cloud);
 
        size_t cloud_size = cloud->size();
        std::vector<std::vector<int>> neighbors_all(cloud_size);
        std::vector<int> sizes(cloud_size);
        std::vector<float> dists;
        for (auto i=0; i<cloud_size; i++) {
            kdtree->nearestKSearch(cloud->points[i], 10, neighbors_all[i], dists);
            sizes[i] = neighbors_all[i].size();
        }
        auto t2 = clock();
        auto knn_time = (float)((t2-t1)*1000/CLOCKS_PER_SEC);
 
        int max_nn_size = *max_element(sizes.begin(), sizes.end());
        std::vector<int> temp_neighbors_all(max_nn_size * cloud_size);
        pcl::gpu::PtrStep<int> ps(&temp_neighbors_all[0], max_nn_size * 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(temp_neighbors_all, sizes, max_nn_size);
 
        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 = clock();
        auto compute_normal_time = (float)((t3-t2)*1000/CLOCKS_PER_SEC);
 
	    std::vector<pcl::PointXYZ> normals;
	    gpu_normals.download(normals);
 
        auto t4 = clock();
        auto total_time = (float)((t4-t1)*1000/CLOCKS_PER_SEC);
        printf("cloud size:%zu, knn_time:%.2f ms,compute_normal_time:%.2f ms, total_time:%.2f ms\n", cloud->size(), knn_time, compute_normal_time, total_time);
    };
    ros::Subscriber pc_sub = node.subscribe<sensor_msgs::PointCloud2>("/BackLidar/lslidar_point_cloud", 1, callback);
    ros::spin();
    return 0;
}

        运行结果如下所示：

cloud size:32585, knn_time:64.00 ms,compute_normal_time:4.00 ms, total_time:68.00 ms
cloud size:32563, knn_time:63.00 ms,compute_normal_time:4.00 ms, total_time:68.00 ms
cloud size:32606, knn_time:64.00 ms,compute_normal_time:4.00 ms, total_time:69.00 ms
cloud size:25288, knn_time:48.00 ms,compute_normal_time:3.00 ms, total_time:51.00 ms
cloud size:32651, knn_time:63.00 ms,compute_normal_time:4.00 ms, total_time:68.00 ms
cloud size:32621, knn_time:65.00 ms,compute_normal_time:5.00 ms, total_time:71.00 ms
cloud size:32439, knn_time:67.00 ms,compute_normal_time:4.00 ms, total_time:72.00 ms
cloud size:32675, knn_time:66.00 ms,compute_normal_time:4.00 ms, total_time:71.00 ms
cloud size:32109, knn_time:63.00 ms,compute_normal_time:4.00 ms, total_time:67.00 ms
cloud size:32642, knn_time:64.00 ms,compute_normal_time:5.00 ms, total_time:70.00 ms
cloud size:29241, knn_time:56.00 ms,compute_normal_time:4.00 ms, total_time:61.00 ms
cloud size:32599, knn_time:63.00 ms,compute_normal_time:4.00 ms, total_time:68.00 ms
cloud size:32607, knn_time:64.00 ms,compute_normal_time:4.00 ms, total_time:68.00 ms
cloud size:32640, knn_time:66.00 ms,compute_normal_time:4.00 ms, total_time:70.00 ms
cloud size:31342, knn_time:61.00 ms,compute_normal_time:4.00 ms, total_time:66.00 ms

        复现结果与原作者的运行结果差距较大，主要差距在knn_time上。

        其中GPU利用率如下所示：

         附上CMakeLists文件

cmake_minimum_required(VERSION 2.8.3)
project(n_lidar_gpu_normal)

add_compile_options(-std=c++14)


find_package(catkin REQUIRED
  roscpp
  std_msgs
  message_generation
  sensor_msgs
)
    
catkin_package(
  CATKIN_DEPENDS 
  message_runtime
)

include_directories(
 include
 ${catkin_INCLUDE_DIRS}
)

find_package(PCL 1.11 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

aux_source_directory(./src PROJECT_SRC)
add_executable(${PROJECT_NAME} 
  ${PROJECT_SRC} 
)

add_dependencies(${PROJECT_NAME} 
  ${${PROJECT_NAME}_EXPORTED_TARGETS} 
  ${catkin_EXPORTED_TARGETS}
)

target_link_libraries(${PROJECT_NAME}
  ${catkin_LIBRARIES}
  ${PCL_LIBRARIES}
)

【参考】

        【点云处理】点云法向量估计及其加速(3)_昌山小屋的博客-CSDN博客