0. 简介

我们在上一节主要讲了基础的点云变换，对应了pcl::transformPointCloud这种方法。而我们这一章将会讲解 pcl::CropBox这种点云裁剪步骤。点云裁剪是在点云中选择出一部分区域的过程，以便于后续的处理和分析。在点云处理领域，点云裁剪是一个非常常见的任务，因为在现实中，往往只需要关注某些区域的点云数据，而不需要处理整个点云数据集。点云裁剪通常是通过一个定义良好的几何形状来实现，比如长方体、球体等等。

1. CUDA与Thrust

在点云裁剪中，CUDA和Thrust都提供了不同的方法来实现。使用CUDA时，一般会借助于CUDA中的点云裁剪算法来实现，CUDA中的点云裁剪算法需要使用CUDA提供的数据结构和API，以及CUDA的线程和内存模型等相关知识。在GPU上运行，加快点云处理的速度。而Thrust是一个基于C++ STL的高性能并行编程库，它可以在CPU和GPU上运行，并提供了许多通用的算法和数据结构，可以轻松地实现点云裁剪。CUDA 和 Thrust 提供了一种高效的方式来处理大规模的点云数据。CUDA 提供了低级别的 GPU 计算能力，需要程序员手动管理内存和线程。而 Thrust 为开发者提供了一组高级别的算法和数据结构，可以更方便地编写 CUDA 代码，而无需手动管理内存和线程。

2. CUDA代码完成加速

下面我们来看一下这个点云裁剪部分的操作。该代码实现了使用CUDA并行计算来移除在球体内部的点云数据。该代码包含了一个名为 kernel_cudaRemovePointsInsideSphere 的函数，该函数使用了 CUDA 的并行计算功能。该函数中首先通过线程索引计算每个线程要处理的点云数据的位置，然后获取该点云数据的坐标，接着计算该点云数据到原点的距离，并将其与球体半径的平方进行比较。如果该点云数据距离原点的距离小于球体半径的平方，则将其标记为 false，表示该点云数据在球体内部。如果该点云数据距离原点的距离大于等于球体半径的平方，则将其标记为 true，表示该点云数据不在球体内部。最后该函数使用了 CUDA 的并行计算功能来同时处理多个点云数据，并通过同步函数 cudaDeviceSynchronize 来保证计算结果的正确性。此外还包含了一个名为 cudaRemovePointsInsideSphere 的函数，该函数则负责调用 kernel_cudaRemovePointsInsideSphere 函数，设置线程块和线程数，并调用 kernel 函数来完成点云数据的移除。

__global__ void kernel_cudaRemovePointsInsideSphere(pcl::PointXYZ *d_point_cloud, bool *d_markers, int number_of_points, float sphere_radius)
{
    int ind = blockIdx.x * blockDim.x + threadIdx.x; // 线程索引

    if (ind < number_of_points) // 线程索引小于点云数
    {
        float x = d_point_cloud[ind].x; // 获取点云数据
        float y = d_point_cloud[ind].y;
        float z = d_point_cloud[ind].z;

        float distance = (x * x + y * y + z * z); // 计算点云数据到原点的距离

        if (distance < sphere_radius * sphere_radius) // 判断点云数据是否在球体内
        {
            d_markers[ind] = false; // 将点云数据标记为false
        }
        else
        {
            d_markers[ind] = true;
        }
    }
}

cudaError_t cudaRemovePointsInsideSphere(int threads, pcl::PointXYZ *d_point_cloud,
                                         bool *d_markers, int number_of_points, float sphere_radius)
{
    kernel_cudaRemovePointsInsideSphere<<<number_of_points / threads + 1, threads>>>(d_point_cloud, d_markers, number_of_points, sphere_radius); // 设置线程块和线程数,并调用kernel来完成remove移除

    cudaDeviceSynchronize(); // 同步
    return cudaGetLastError();
}

然后下面的代码为调用上述代码的函数，该函数首先设置线程数和球半径等参数，并为点云、标记在设备上分配内存空间。将主机上的点云数据复制到设备上。调用函数cudaRemovePointsInsideSphere，该函数使用kernel_cudaRemovePointsInsideSphere内核函数来移除点云中的在球体内的点。将标记数据从设备复制到主机上，并将标记为true的点添加到新的点云new_point_cloud中。

bool removePointsInsideSphere(pcl::PointCloud<pcl::PointXYZ> &point_cloud)
{
    int threads;                  // 线程数
    float sphere_radius = 1.0f;      // 球半径
    pcl::PointXYZ *d_point_cloud; // 点云,设备DEVICE
    bool *d_markers;              // 标记,设备DEVICE
    bool *h_markers;              // 标记，主机HOST

    cudaError_t err = ::cudaSuccess; // 设置错误类型
    err = cudaSetDevice(0);             // 设置设备
    if (err != ::cudaSuccess)         // 如果类型不为cudaSuccess
        return false;                 // 返回

    threads = getNumberOfAvailableThreads(); // 获取线程数

    err = cudaMalloc((void **)&d_point_cloud, point_cloud.points.size() * sizeof(pcl::PointXYZ)); // 为点云分配内存
    if (err != ::cudaSuccess)                                                                      // 如果类型不为cudaSuccess
        return false;

    err = cudaMemcpy(d_point_cloud, point_cloud.points.data(), point_cloud.points.size() * sizeof(pcl::PointXYZ), cudaMemcpyHostToDevice); // 将点云数据从主机复制到设备
    if (err != ::cudaSuccess)
        return false;

    err = cudaMalloc((void **)&d_markers, point_cloud.points.size() * sizeof(bool)); // 为标记分配内存
    if (err != ::cudaSuccess)
        return false;

    err = cudaRemovePointsInsideSphere(threads, d_point_cloud, d_markers, point_cloud.points.size(), sphere_radius); // 移除点云中的点,这里面的算法在另一个文件中
    if (err != ::cudaSuccess)
        return false;

    h_markers = (bool *)malloc(point_cloud.points.size() * sizeof(bool)); // 为标记分配内存

    err = cudaMemcpy(h_markers, d_markers, point_cloud.points.size() * sizeof(bool), cudaMemcpyDeviceToHost); // 将标记数据从设备复制到主机
    if (err != ::cudaSuccess)
        return false;

    pcl::PointCloud<pcl::PointXYZ> new_point_cloud;
    for (size_t i = 0; i < point_cloud.points.size(); i++)
    {
        if (h_markers[i])                               // 如果标记为true
            new_point_cloud.push_back(point_cloud[i]); // 将点云中的点添加到新的点云中
    }

    std::cout << "Number of points before removing points: " << point_cloud.size() << std::endl; // 输出移除前的点云数
    point_cloud = new_point_cloud;
    std::cout << "Number of points after removing points: " << point_cloud.size() << std::endl; // 输出移除后的点云数

    free(h_markers); // 释放内存

    err = cudaFree(d_markers); // 释放内存
    d_markers = NULL;           // 将指针置空
    if (err != ::cudaSuccess)
        return false;

    err = cudaFree(d_point_cloud); // 释放内存
    d_point_cloud = NULL;
    if (err != ::cudaSuccess)
        return false;

    return true;
}

3. Thrust代码完成加速

这段代码定义了一个is_inside_sphere函数对象，它用于检查一个点是否在给定的球体内部。该函数对象接受一个float4类型的参数，该参数包含球体的中心坐标和半径。函数对象的operator()方法返回一个布尔值，表示输入点是否在球体内部。removePointsInsideSphere函数使用thrust::remove_if算法，将输入点云中位于给定球体内部的点删除。删除后，函数返回一个新的设备向量，包含了所有在球体外部的点。

#include <thrust/device_vector.h>
#include <pcl/point_types.h>
#include <cmath>
// 定义点云结构体
struct PointXYZ
{
    float x, y, z;
};

struct is_inside_sphere
{
    float4 sphere; // x,y,z, and r

    is_inside_sphere(float4 sphere) : sphere(sphere) {}

    __host__ __device__ bool operator()(const pcl::PointXYZ &pt) const
    {
        float dx = pt.x - sphere.x;
        float dy = pt.y - sphere.y;
        float dz = pt.z - sphere.z;
        float distance = sqrt(dx * dx + dy * dy + dz * dz);
        return distance > sphere.w;
    }
};

thrust::device_vector<PointXYZ> removePointsInsideSphere(const thrust::device_vector<PointXYZ> &point_cloud)
{
    // Define the sphere parameters
    float x = 0.0f;
    float y = 0.0f;
    float z = 0.0f;
    float r = 1.0f;
    float4 sphere = make_float4(x, y, z, r);

    // Create a functor that checks if a point is inside the sphere
    is_inside_sphere predicate(sphere);

    // Remove the points inside the sphere using thrust::remove_if
    thrust::device_vector<PointXYZ> output;
    output.resize(point_cloud.size());
    auto new_end = thrust::remove_if(point_cloud.begin(), point_cloud.end(), predicate);
    point_cloud.erase(new_end, point_cloud.end());
    output.resize(point_cloud.size());
    thrust::copy(point_cloud.begin(), point_cloud.end(), output.begin());
    return output;
}