PCL利用KDTree计算点云平均点间距

内容介绍

在很多经典的点云处理算法中，设置参数总是与点间距有关，或者也可以叫点云密度，例如在计算点云特征时，邻域范围的选择受到点间距的影响，很多情况下时不能小于点间距的，因此在算法运行前，通过计算平均点间距得到一个大致的点间距范围，用于特征参数的设置是很有必要的。

代码

#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>

int main(int argc, char** argv) {


    // 创建点云对象并加载点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("input.pcd", *cloud) == -1) {
        PCL_ERROR("Couldn't read file ");
        return -1;
    }

     // 创建KdTree用于最近邻搜索
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud);

    // 计算平均点间距
    double total_distance = 0.0;
    int valid_points = 0;
    int num_points = cloud->points.size();

    for (size_t i = 0; i < num_points; ++i) {
        std::vector<int> pointIdxNKNSearch(2);
        std::vector<float> pointNKNSquaredDistance(2);

        // 搜索最近的一个邻居（不包括自身）
        if (kdtree.nearestKSearch(cloud->points[i], 2, pointIdxNKNSearch, pointNKNSquaredDistance) > 1) {
            total_distance += sqrt(pointNKNSquaredDistance[1]); // 第一个邻居的距离
            valid_points++;
        }
    }

    if (valid_points > 0) {
        double average_distance = total_distance / valid_points;
        std::cout << "Average point distance: " << average_distance << std::endl;
    } else {
        std::cout << "No valid points found to calculate average distance." << std::endl;
    }

    return 0;
}