基于曲率的点云降采样_曲率降采样（PCL+GPU)

#include <iostream>
#include <fstream>
#include <vector>
#include <sstream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <thread>
#include <chrono>
#include <random>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/gpu/features/features.hpp>
#include <pcl/features/principal_curvatures.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <algorithm>
#include <mutex>
#include <iomanip>  // For std::setprecision
#include <ctime>
#include <pcl/console/time.h>

using namespace pcl::gpu;

std::mutex mutex_file;

//这段代码的作用是计算一个浮动数值数组的平均值。通过遍历 data 数组，累加所有元素的值，然后除以数组的大小来求得均值。
float calculateMean(const std::vector<float>& data) {
    float sum = 0.0;
    for (float num : data) {
        sum += num;
    }
    return sum / data.size();
}

//这段代码的作用是计算一个数值数组的总体标准差。它首先计算数据的均值，然后计算每个元素与均值之间的差异的平方，
//最后返回这些差异平方的平均值的平方根，即标准差。如果数据为空，函数将返回 0.0。
float calculatePopulationStandardDeviation(const std::vector<float>& data) {
    if (data.empty()) {
        return 0.0;
    } 
    float mean = calculateMean(data);
    //声明并初始化一个 float 类型的变量 sumSquaredDiff，其初始值为 0.0。该变量用于存储所有数据点与均值之间差异的平方和。
    float sumSquaredDiff = 0.0;
    for (float num : data) {
        float diff = num - mean;
        //将差值 diff 的平方（即 diff * diff）累加到 sumSquaredDiff 上，得到所有元素与均值差异的平方和。
        sumSquaredDiff += diff * diff;
    }
    //返回平方和除以数据个数 data.size() 后的平方根，这就是数据的总体标准差。标准差是衡量数据离散程度的指标。
    return std::sqrt(sumSquaredDiff / data.size());
}

//一参为要分割的字符串  二参为分割字符
std::vector<std::string> split(const std::string& s, char delimiter) {
    //声明一个 std::vector<std::string> 类型的变量 tokens，用于存储分割后的字符串片段。
    std::vector<std::string> tokens;
    //声明一个 std::string 类型的变量 token，用于存储每次从输入字符串 s 中提取出的子字符串。
    std::string token;
    std::istringstream tokenStream(s);

    //使用 std::getline 从 tokenStream 中按 delimiter 分隔符读取数据。每次读取到的子字符串被存入 token 变量，直到字符串流中的内容读取完毕为止。
    while (std::getline(tokenStream, token, delimiter)) {
        tokens.push_back(token);
    }

    return tokens;
}

int main() {

    pcl::console::TicToc time_1;
    time_1.tic();

    //这行代码通过 time(nullptr) 获取当前的时间戳，将其转换为 unsigned int 类型，然后用这个时间戳作为种子来初始化随机数生成器。
    srand(static_cast<unsigned int>(time(nullptr)));
    // 打开文件
    std::ifstream file("cloud.txt");
    if (!file.is_open()) {
        std::cerr << "无法打开文件" << std::endl;
        return -1;
    }

    // 直接将读入的点存入sampled_cloud容器
    //这行代码的作用是创建一个 pcl::PointCloud<pcl::PointXYZ> 类型的指针，并初始化为空的点云对象。
    pcl::PointCloud<pcl::PointXYZ>::Ptr sampled_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 读取文件
    std::string line;
    while (std::getline(file, line)) {
        auto strs = split(line, ',');
        if (strs.size() != 3) {
            std::cerr << "Invalid line format: " << line << std::endl;
            continue;  // 如果行格式不正确，则跳过
        }

        pcl::PointXYZ point;
        point.x = stof(strs[0]);
        point.y = stof(strs[1]);
        point.z = stof(strs[2]);

        //这行代码将一个名为 point 的三维点添加到 sampled_cloud 指向的点云数据中。
        //sampled_cloud 是一个 pcl::PointCloud<pcl::PointXYZ> 类型的智能指针，而 points 是这个点云的一个成员，
            //它是一个容器，用于存储多个三维点。通过 push_back(point)，新的点 point 被加入到点云数据中。
        sampled_cloud->points.push_back(point);
    }

    // 关闭文件
    file.close();

    std::cout << "After reading file, sampled_cloud size: " << sampled_cloud->points.size() << std::endl;

    // 将点云上传到GPU
    //声明一个 pcl::gpu::PrincipalCurvaturesEstimation::PointCloud 类型的对象 cloud_gpu，这是一个专门用于在 GPU 上处理的点云类型。
    //将 sampled_cloud（一个普通的 pcl::PointCloud<pcl::PointXYZ> 对象）中的点数据上传到 GPU 中，存储到 cloud_gpu 变量中。
    pcl::gpu::PrincipalCurvaturesEstimation::PointCloud cloud_gpu;
    cloud_gpu.upload(sampled_cloud->points);

    //声明另一个 pcl::gpu::NormalEstimation::PointCloud 类型的对象 cloud_n，用于存储点云数据，并准备在 GPU 上计算法线。
    //将 sampled_cloud 中的点云数据上传到 GPU，并存储在 cloud_n 中。
    pcl::gpu::NormalEstimation::PointCloud cloud_n;
    cloud_n.upload(sampled_cloud->points);

    // 使用GPU计算法线
    //声明一个 pcl::gpu::NormalEstimation 类型的对象 ne_device，它用于在 GPU 上计算法线。
    pcl::gpu::NormalEstimation ne_device;
    ne_device.setInputCloud(cloud_n);  //设置输入点云  
    ne_device.setRadiusSearch(5.0, 50);  //设置搜索半径与个数

    //声明一个 pcl::gpu::NormalEstimation::Normals 类型的对象 normals_device，用于存储计算得到的法线数据。
    //调用 compute() 方法来执行法线估计计算，并将结果存储到 normals_device 中。
    pcl::gpu::NormalEstimation::Normals normals_device;
    ne_device.compute(normals_device);

    // 从GPU下载法线到CPU并转换为 pcl::PointCloud<pcl::Normal> 格式
    //声明一个 std::vector<pcl::PointXYZ> 类型的变量 gpu_normals，用于存储从 GPU 中下载的法线数据。
    //通过调用 download() 方法，从 GPU 中下载法线数据到 gpu_normals 向量。
    std::vector<pcl::PointXYZ> gpu_normals;
    normals_device.download(gpu_normals);

    //声明一个智能指针 normals_for_cpu，它指向一个新的 pcl::PointCloud<pcl::Normal> 对象。这个对象将用于存储法线数据的 CPU 版本。
    //将 normals_for_cpu 点云的大小调整为 gpu_normals.size()，即与下载的法线数据的大小一致。
    pcl::PointCloud<pcl::Normal>::Ptr normals_for_cpu(new pcl::PointCloud<pcl::Normal>());
    normals_for_cpu->resize(gpu_normals.size());

    //这段代码通过遍历 gpu_normals 中的每一个点，将其转换为 pcl::Normal 类型的法线，并将法线存储到 normals_for_cpu 点云的对应位置。
    for (size_t i = 0; i < gpu_normals.size(); ++i) {
        pcl::Normal normal;
        normal.normal_x = gpu_normals[i].x;
        normal.normal_y = gpu_normals[i].y;
        normal.normal_z = gpu_normals[i].z;
        normals_for_cpu->points[i] = normal;
    }

    // 使用法线进行曲率计算
    //声明一个 pcl::gpu::DeviceArray<pcl::PrincipalCurvatures> 类型的对象 pc_features，用于存储计算出的曲率特征。
    //pcl::gpu::DeviceArray 是 PCL 中用于在 GPU 上存储数据的类。
    pcl::gpu::DeviceArray<pcl::PrincipalCurvatures> pc_features;
    //声明一个 pcl::gpu::PrincipalCurvaturesEstimation 类型的对象 pc_gpu，用于在 GPU 上计算点云的主曲率特征。
    pcl::gpu::PrincipalCurvaturesEstimation pc_gpu;
    pc_gpu.setInputCloud(cloud_gpu);
    pc_gpu.setInputNormals(normals_device);
    pc_gpu.setRadiusSearch(5.0, 50);
    pc_gpu.compute(pc_features);

    // 从GPU下载曲率特征到CPU
    //声明一个 std::vector<pcl::PrincipalCurvatures> 类型的变量 downloaded_curvatures，用于存储从 GPU 下载到 CPU 的曲率特征数据。
    //通过调用 download() 方法，将计算得到的曲率特征从 GPU 下载到 downloaded_curvatures 向量中。
    std::vector<pcl::PrincipalCurvatures> downloaded_curvatures;
    pc_features.download(downloaded_curvatures);

    // 计算邻域内的曲率变化
    float max_variance = 0.1;
    float min_variance = 0.0;

    //声明两个 std::vector 数据结构。ss 用于存储每个点的随机值，flags 用于标记每个点是否符合保留条件。
    std::vector<float> ss(sampled_cloud->points.size());
    std::vector<bool> flags(sampled_cloud->points.size());

    //定义一个常数 scale2，它将 rand() 生成的整数随机值缩放到 [0, 1] 之间。
    //通过循环生成与点云大小相等的随机数，并将这些随机数存储到 ss 数组中。
    const float scale2 = 1.0 / static_cast<float>(RAND_MAX);
    for (size_t i = 0; i < sampled_cloud->points.size(); ++i) {
        ss[i] = static_cast<float>(rand()) * scale2;
    }

    //声明一个 cloud_device 对象，它是 pcl::gpu::Octree::PointCloud 类型，并将 sampled_cloud 中的点云数据上传到 GPU。
    pcl::gpu::Octree::PointCloud cloud_device;
    cloud_device.upload(sampled_cloud->points);

    //创建一个 Octree 对象 octree_device，并将点云数据传递给它。随后调用 build() 方法构建 Octree 数据结构。
    pcl::gpu::Octree octree_device;
    octree_device.setCloud(cloud_device);
    octree_device.build();

    //定义每次查询时考虑的点数为 50000。这是一个查询的批次大小。
    //设置最大查询答案数为 500。即每次查询时，最多返回 500 个邻域点。
    int cnt_per_query = 50000;
    const int max_answers = 500;
    //创建一个包含 cnt_per_query 个元素的 radius 数组，每个元素的值为 1.0，表示每个查询的半径大小。
    std::vector<float> radius(cnt_per_query, 1.0);
    pcl::gpu::Octree::Radiuses radiuses_device;
    //radiuses_device.upload(radius) 将这些半径值上传到 GPU 上，以便在 Octree 查询过程中使用。
    radiuses_device.upload(radius);

    //获取点云的总大小，并将其存储在 len 中。
    int len = sampled_cloud->points.size();

    //声明两个 pcl::PointCloud<pcl::PointXYZ> 类型的智能指针，to_query 和 result，分别用于存储查询的点和查询结果。
    pcl::PointCloud<pcl::PointXYZ>::Ptr to_query(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
    

    //这是一个外部循环，负责将点云数据分批处理。每批处理最多 cnt_per_query 个点。 (len + cnt_per_query - 1) / cnt_per_query 计算了总共需要多少批次。
    //len 是点云数据的大小，cnt_per_query 是每批次处理的点数。
    for (int i = 0; i < (len + cnt_per_query - 1) / cnt_per_query; i++) {
        //声明一个 Octree 查询对象 queries_device，它将用于存储当前批次中要查询的点。
        pcl::gpu::Octree::Queries queries_device;

        to_query->clear();
        //在内部循环中，按照批次大小 cnt_per_query 将点从 sampled_cloud 中提取到 to_query 中。idx 是当前点的索引，j 是当前批次的计数器。
        int j = 0;
        for (; j < cnt_per_query; j++) {
            int idx = i * cnt_per_query + j;
            if (idx >= len)
                break;
            to_query->points.push_back(sampled_cloud->points[idx]);
        }
        //将 to_query 中存储的点上传到 GPU 上的 queries_device 中。这一步确保我们在 GPU 上进行查询时使用正确的点。
        queries_device.upload(to_query->points);

        //创建 result_device，它用于存储查询的结果。j 是当前批次的点数，max_answers 是每次查询的最大邻域数。
        //octree_device.radiusSearch() 方法在 octree_device 中执行半径邻域搜索。它会查找每个查询点周围的邻域点，并将结果存储在 result_device 中。
        pcl::gpu::NeighborIndices result_device(j, max_answers);
        octree_device.radiusSearch(queries_device, radiuses_device, max_answers, result_device);

        //从 GPU 中下载查询结果。sizes 存储了每个查询点找到的邻域点数量，data 存储了邻域点的索引。
        //result_device.sizes.download(sizes) 将查询结果中每个点的邻域点数目下载到 sizes 数组。
            //result_device.data.download(data) 将邻域点的索引下载到 data 数组。
        std::vector<int> sizes, data;
        result_device.sizes.download(sizes);
        result_device.data.download(data);

#pragma omp parallel for
        for (int k = 0; k < j; k++) {
            if (sizes[k] <= 1)
                continue;

            //对每个查询点的邻域点计算曲率方差。首先遍历邻域点并计算其曲率（pc1），然后计算这些曲率的均值和标准差。
            //sizes[k] 表示第 k 个查询点的邻域点数量。
            //data[k * max_answers + m] 是 downloaded_curvatures 数组中存储曲率特征的索引，data[k * max_answers + m] 获取当前邻域点的索引。
                //downloaded_curvatures[data[k * max_answers + m]].pc1 获取邻域点的第一主曲率值（pc1）。
            double sum = 0, sum2 = 0;
            for (int m = 0; m < sizes[k]; m++) {
                double c = downloaded_curvatures[data[k * max_answers + m]].pc1;
                sum += c;
                sum2 += (c * c);
            }
            sum /= sizes[k];
            double stdCur = std::sqrt((sum2 / sizes[k]) - sum * sum);

           // a[i * cnt_per_query + k] = stdCur;

            //根据计算得到的标准差 stdCur 计算保留该点的概率。stdCur 范围在 [min_variance, max_variance] 之间。
            //keep_probability 是保留该点的概率。标准差越大，保留该点的概率越高。
            //std::max(0.3f, std::min(1.0f, keep_probability)) 确保概率值保持在[0.3, 1.0] 之间。
            float keep_probability = (stdCur - min_variance) / (max_variance - min_variance);
            keep_probability = std::max(0.3f, std::min(1.0f, keep_probability));
            //通过与随机值 ss[i * cnt_per_query + k] 进行比较，决定是否保留该点。如果随机值小于保留概率，
            //则将该点标记为保留（flags[i * cnt_per_query + k] = true）。
            if (ss[i*cnt_per_query+k] < keep_probability) {
                flags[i * cnt_per_query + k] = true;
            }
        }
    }

  
    //创建一个新的点云对象 sampled_cloud_1，它将用于存储筛选后的点。这里 PointXYZ 表示点云中每个点有 x, y, 和 z 三个坐标。
    pcl::PointCloud<pcl::PointXYZ>::Ptr sampled_cloud_1(new pcl::PointCloud<pcl::PointXYZ>());
    //通过 flags 数组（前面计算得到的筛选标志）来筛选点云数据。只有当 flags[i] 为 true 时，点 sampled_cloud->points[i] 会被加入到新的点云 sampled_cloud_1 中。
    for (int i = 0; i < sampled_cloud->points.size(); i++) {
        if (flags[i])
            sampled_cloud_1->points.push_back(sampled_cloud->points[i]);
    }

    std::cout << "Sampled cloud size: " << sampled_cloud_1->points.size() << std::endl;

    //打开一个输出文件流 outfile，用于将筛选后的点云保存到文件 sampled_cloud.txt 中。
    std::ofstream outfile("sampled_cloud.txt");
    if (!outfile.is_open()) {
        std::cerr << "Failed to open file for writing!" << std::endl;
        return -1;
    }

    //遍历 sampled_cloud_1 中的每个点，将每个点的 x、y、z 坐标以空格分隔的形式写入文件 sampled_cloud.txt。
    for (const auto& point : sampled_cloud_1->points)
    {
        outfile << point.x << " " << point.y << " " << point.z << "\n";
    }
    outfile.close();
    std::cout << "Sampled point cloud saved to 'sampled_cloud.txt'" << std::endl;

    cout << "-> 时间： " << time_1.toc() / 1000 << "s" << endl;

    return 0;
}

注意：

1. 所使用的PCL版本是1.13.1，不能使用PCL-1.13.1-AllInOne-msvc2022-win64.exe直接安装，需下载对应版本的源码，并通过CMake配置CUDA模块，若使用其他版本PCL，代码编写可能有所不同。

2. 运行此代码需确保显卡支持CUDA，且对显存大小有一定要求。可减小cnt_per_query值，以适配更小容量的显存。