0. 简介

上一节中我们讲了点云裁剪这一手段，对应了点云中的 pcl::CropBox这种点云裁剪步骤。而我们这一节主要将围绕着降采样来进行描述。这里我们对标的是pcl::VoxelGrid。点云数据往往非常大，对于一些需要高效处理的应用来说，数据的降采样是一个非常有用的工具。

1. CUDA与Thrust

CUDA和Thrust可以实现快速降采样的方法，可以大大提高降采样的速度。降采样算法的基本思想是，将一个点云数据分成若干个小的块，对每个块进行采样，保留一个块中的一个点作为采样点。通过这种方式，可以减小点云数据的大小，同时保留足够的信息。降采样算法需要进行一些计算，需要在GPU上实现。其中，CUDA 作为一种并行计算框架，可以充分利用 GPU 的计算能力，而 Thrust 则是 CUDA 中的一个 STL-like 库，提供了方便易用的算法和数据结构。通过使用这些方法，可以大幅缩短点云处理的时间，提高点云处理的效率和质量。需要注意的是，在使用 CUDA 和 Thrust 进行点云处理时，需要对 GPU 的计算能力和内存使用有一定的了解，以保证计算过程的稳定和正确。

2. CUDA代码完成加速

这部分代码将会比较繁琐，我们将会一步步带着大家看，首先是很多个函数，第一部分函数是用来计算三维点云的栅格化参数。该函数接收一个包含点云数据的指针、点云数量和栅格化分辨率，以及一个输出参数结构体gridParameters类型的引用。该函数使用Thrust将点云数据转换为device_ptr类型，并使用Thrust的minmax_element函数获取每个轴上的最大和最小点的坐标。然后，该函数将这些坐标从GPU拷贝到CPU，并计算每个轴上栅格的数量以及总的栅格数，最后将计算结果赋值给输出参数结构体gridParameters的相应字段。如果出现异常，则会返回CUDA错误。

cudaError_t cudaCalculateGridParams(pcl::PointXYZ *d_point_cloud, int number_of_points,
                                    float resolution_X, float resolution_Y, float resolution_Z, gridParameters &out_rgd_params)
{
    cudaError_t err = cudaGetLastError(); // 获取cuda的错误
    try
    {
        thrust::device_ptr<pcl::PointXYZ> t_cloud(d_point_cloud); // 将d_point_cloud转换为thrust::device_ptr<pcl::PointXYZ>类型
        err = cudaGetLastError();
        if (err != ::cudaSuccess)
            return err;

        thrust::pair<thrust::device_ptr<pcl::PointXYZ>, thrust::device_ptr<pcl::PointXYZ>>
            minmaxX = thrust::minmax_element(t_cloud, t_cloud + number_of_points, compareX()); // 获取x轴的最大最小值
        err = cudaGetLastError();                                                               // 获取cuda的错误
        if (err != ::cudaSuccess)
            return err;

        thrust::pair<thrust::device_ptr<pcl::PointXYZ>, thrust::device_ptr<pcl::PointXYZ>>
            minmaxY = thrust::minmax_element(t_cloud, t_cloud + number_of_points, compareY()); // 获取y轴的最大最小值
        err = cudaGetLastError();
        if (err != ::cudaSuccess)
            return err;

        thrust::pair<thrust::device_ptr<pcl::PointXYZ>, thrust::device_ptr<pcl::PointXYZ>>
            minmaxZ = thrust::minmax_element(t_cloud, t_cloud + number_of_points, compareZ()); // 获取z轴的最大最小值
        err = cudaGetLastError();
        if (err != ::cudaSuccess)
            return err;

        pcl::PointXYZ minX, maxX, minZ, maxZ, minY, maxY;

        err = cudaMemcpy(&minX, minmaxX.first.get(), sizeof(pcl::PointXYZ), cudaMemcpyDeviceToHost); // 将x轴最小值从GPU拷贝到CPU
        if (err != ::cudaSuccess)
            return err;
        err = cudaMemcpy(&maxX, minmaxX.second.get(), sizeof(pcl::PointXYZ), cudaMemcpyDeviceToHost); // 将x轴最大值从GPU拷贝到CPU
        if (err != ::cudaSuccess)
            return err;
        err = cudaMemcpy(&minZ, minmaxZ.first.get(), sizeof(pcl::PointXYZ), cudaMemcpyDeviceToHost); // 将z轴最小值从GPU拷贝到CPU
        if (err != ::cudaSuccess)
            return err;
        err = cudaMemcpy(&maxZ, minmaxZ.second.get(), sizeof(pcl::PointXYZ), cudaMemcpyDeviceToHost); // 将z轴最大值从GPU拷贝到CPU
        if (err != ::cudaSuccess)
            return err;
        err = cudaMemcpy(&minY, minmaxY.first.get(), sizeof(pcl::PointXYZ), cudaMemcpyDeviceToHost); // 将y轴最小值从GPU拷贝到CPU
        if (err != ::cudaSuccess)
            return err;
        err = cudaMemcpy(&maxY, minmaxY.second.get(), sizeof(pcl::PointXYZ), cudaMemcpyDeviceToHost); // 将y轴最大值从GPU拷贝到CPU
        if (err != ::cudaSuccess)
            return err;

        int number_of_buckets_X = ((maxX.x - minX.x) / resolution_X) + 1; // 计算x轴的栅格数
        int number_of_buckets_Y = ((maxY.y - minY.y) / resolution_Y) + 1; // 计算y轴的栅格数
        int number_of_buckets_Z = ((maxZ.z - minZ.z) / resolution_Z) + 1; // 计算z轴的栅格数

        out_rgd_params.number_of_buckets_X = number_of_buckets_X; // 将计算结果赋值给out_rgd_params
        out_rgd_params.number_of_buckets_Y = number_of_buckets_Y;
        out_rgd_params.number_of_buckets_Z = number_of_buckets_Z;
        out_rgd_params.number_of_buckets = number_of_buckets_X * number_of_buckets_Y * number_of_buckets_Z; // 计算总的栅格数

        out_rgd_params.bounding_box_max_X = maxX.x;
        out_rgd_params.bounding_box_min_X = minX.x;
        out_rgd_params.bounding_box_max_Y = maxY.y;
        out_rgd_params.bounding_box_min_Y = minY.y;
        out_rgd_params.bounding_box_max_Z = maxZ.z;
        out_rgd_params.bounding_box_min_Z = minZ.z;

        out_rgd_params.resolution_X = resolution_X;
        out_rgd_params.resolution_Y = resolution_Y;
        out_rgd_params.resolution_Z = resolution_Z;
    }
    catch (thrust::system_error &e)
    {
        err = cudaGetLastError();
        cudaDeviceReset();
        return err;
    }
    catch (std::bad_alloc &e)
    {
        err = cudaGetLastError();
        cudaDeviceReset();
        return err;
    }
    return cudaGetLastError();
}

然后下面就会通过cuda对整个点云完成裁剪。这是一段CUDA C++代码，用于实现基于网格的哈希表。该哈希表用于处理三维点云数据，将每个点映射到其对应的网格桶中。该代码分为四个核函数：

kernel_initializeIndByKey：初始化哈希表，将哈希表中每个元素的index_of_point赋值为其索引。

kernel_getIndexOfBucketForPoints：计算每个点所在的网格桶的索引，将其赋值给哈希表中相应元素的index_of_bucket。

kernel_initializeBuckets：初始化网格桶，将每个网格桶的index_begin、index_end和number_of_points分别赋值为-1、-1和0。

kernel_updateBuckets：计算每个网格桶的起始点索引和终止点索引，将其赋值给相应的网格桶。具体实现方式为：遍历哈希表中的每个元素，如果该元素所代表的点是该网格桶中的第一个点，则将该点的索引赋值给该网格桶的index_begin，如果该点是该网格桶中的最后一个点，则将该点的索引+1赋值给该网格桶的index_end，同时将其赋值给下一个网格桶的index_end。如果该点不是该网格桶中的第一个或最后一个点，则不做任何处理。

kernel_countNumberOfPointsForBuckets：用于计算每个栅格的点数，通过读取 d_buckets 数组中的起始点索引和终止点索引，计算出每个栅格的点数，并将其赋值给 d_buckets 数组中的 number_of_points 值。

kernel_copyKeys： 函数用于计算每个栅格的起始点索引和终止点索引，通过将输入数组 d_hashTable_in 的值复制到输出数组 d_hashTable_out 中实现。

// 初始化hash表
__global__ void kernel_initializeIndByKey(hashElement *d_hashTable, int number_of_points)
{
    int ind = blockIdx.x * blockDim.x + threadIdx.x; // 获取线程索引
    if (ind < number_of_points)                         // 判断线程索引是否越界
    {
        d_hashTable[ind].index_of_point = ind; // 将线程索引赋值给d_hashTable[ind].index_of_point,给出对应索引的点的索引
        d_hashTable[ind].index_of_bucket = 0;  // 将0赋值给d_hashTable[ind].index_of_bucket,给出对应索引的点的栅格索引
    }
}

// 计算每个点对应的栅格索引
__global__ void kernel_getIndexOfBucketForPoints(pcl::PointXYZ *d_point_cloud, hashElement *d_hashTable, int number_of_points, gridParameters rgd_params)
{
    int ind = blockIdx.x * blockDim.x + threadIdx.x; // 获取线程索引
    if (ind < number_of_points)                         // 判断线程索引是否越界
    {
        int ix = (d_point_cloud[ind].x - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; // 计算x轴的栅格索引
        int iy = (d_point_cloud[ind].y - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y;
        int iz = (d_point_cloud[ind].z - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z;
        d_hashTable[ind].index_of_bucket = ix * rgd_params.number_of_buckets_Y * rgd_params.number_of_buckets_Z + iy * rgd_params.number_of_buckets_Z + iz; // 计算总的栅格索引
    }
}
// 初始化栅格
__global__ void kernel_initializeBuckets(bucket *d_buckets, gridParameters rgd_params)
{
    long long int ind = blockIdx.x * blockDim.x + threadIdx.x; // 获取线程索引
    if (ind < rgd_params.number_of_buckets)                       // 判断线程索引是否越界
    {
        d_buckets[ind].index_begin = -1;     // 将-1赋值给d_buckets[ind].index_begin,给出对应索引的栅格的起始点索引
        d_buckets[ind].index_end = -1;         // 将-1赋值给d_buckets[ind].index_end,给出对应索引的栅格的终止点索引
        d_buckets[ind].number_of_points = 0; // 将0赋值给d_buckets[ind].number_of_points,给出对应索引的栅格的点数
    }
}
// 计算每个栅格的起始点索引和终止点索引
__global__ void kernel_updateBuckets(hashElement *d_hashTable,
                                     bucket *d_buckets,
                                     gridParameters rgd_params,
                                     int number_of_points)
{
    int ind = blockIdx.x * blockDim.x + threadIdx.x;    // 获取线程索引
    if (number_of_points > 0 && ind < number_of_points) // 判断线程索引是否越界
    {
        if (ind == 0) // 判断线程索引是否为0
        {
            int index_bucket = d_hashTable[ind].index_of_bucket;       // 获取对应索引的点的栅格索引
            int index_bucket_1 = d_hashTable[ind + 1].index_of_bucket; // 获取对应索引+1的点的栅格索引

            d_buckets[index_bucket].index_begin = ind; // 将对应索引的点的索引赋值给对应索引的栅格的起始点索引
            if (index_bucket != index_bucket_1)           // 判断对应索引的点的栅格索引是否等于对应索引+1的点的栅格索引
            {
                d_buckets[index_bucket].index_end = ind + 1;   // 将对应索引+1的点的索引赋值给对应索引的栅格的终止点索引
                d_buckets[index_bucket_1].index_end = ind + 1; // 将对应索引+1的点的索引赋值给对应索引+1的点的栅格的终止点索引
            }
        }
        else if (ind == number_of_points - 1) // 判断线程索引是否为number_of_points-1
        {
            d_buckets[d_hashTable[ind].index_of_bucket].index_end = ind + 1; // 将对应索引的点的索引赋值给对应索引的栅格的终止点索引
        }
        else
        {
            int index_bucket = d_hashTable[ind].index_of_bucket;       // 获取对应索引的点的栅格索引
            int index_bucket_1 = d_hashTable[ind + 1].index_of_bucket; // 获取对应索引+1的点的栅格索引

            if (index_bucket != index_bucket_1) // 判断对应索引的点的栅格索引是否等于对应索引+1的点的栅格索引
            {
                d_buckets[index_bucket].index_end = ind + 1; // 将对应索引+1的点的索引赋值给对应索引的栅格的终止点索引
                d_buckets[index_bucket_1].index_begin = ind + 1;
            }
        }
    }
}
// 计算每个栅格的点数
__global__ void kernel_countNumberOfPointsForBuckets(bucket *d_buckets, gridParameters rgd_params)
{
    int ind = blockIdx.x * blockDim.x + threadIdx.x; // 获取线程索引
    if (ind < rgd_params.number_of_buckets)             // 判断线程索引是否越界
    {
        int index_begin = d_buckets[ind].index_begin;
        int index_end = d_buckets[ind].index_end;

        if (index_begin != -1 && index_end != -1) // 判断对应索引的栅格的起始点索引和终止点索引是否为-1
        {
            d_buckets[ind].number_of_points = index_end - index_begin; // 将对应索引的栅格的终止点索引-对应索引的栅格的起始点索引赋值给对应索引的栅格的点数
        }
    }
}
// 计算每个栅格的起始点索引和终止点索引
__global__ void kernel_copyKeys(hashElement *d_hashTable_in, hashElement *d_hashTable_out, int number_of_points)
{
    int ind = blockIdx.x * blockDim.x + threadIdx.x; // 获取线程索引
    if (ind < number_of_points)
    {
        d_hashTable_out[ind] = d_hashTable_in[ind]; // 将d_hashTable_in[ind]赋值给d_hashTable_out[ind]
    }
}

然后下面这段代码使用CUDA在GPU上计算栅格。具体来说，代码通过调用不同的CUDA核函数来完成不同的计算任务。其中，kernel_initializeIndByKey用于初始化哈希表；kernel_getIndexOfBucketForPoints计算每个点所在的栅格的索引；kernel_initializeBuckets用于初始化栅格；kernel_updateBuckets更新每个栅格的点数，并且在每个栅格中存储点的索引；kernel_countNumberOfPointsForBuckets用于计算每个栅格中的点数；最后，kernel_copyKeys用于计算每个栅格中的起始点和终止点的索引。此外，代码使用thrust库对哈希表进行排序。

cudaError_t cudaCalculateGrid(int threads, pcl::PointXYZ *d_point_cloud, bucket *d_buckets,
                              hashElement *d_hashTable, int number_of_points, gridParameters rgd_params) // 计算栅格
{
    cudaError_t err = cudaGetLastError();
    hashElement *d_temp_hashTable;                                                    // 临时hash表
    cudaMalloc((void **)&d_temp_hashTable, number_of_points * sizeof(hashElement)); // 为临时hash表分配内存
    int blocks = number_of_points / threads + 1;                                    // 计算线程块数

    kernel_initializeIndByKey<<<blocks, threads>>>(d_temp_hashTable, number_of_points); // 初始化临时hash表
    err = cudaDeviceSynchronize();                                                        // 同步
    if (err != ::cudaSuccess)
        return err;

    kernel_getIndexOfBucketForPoints<<<blocks, threads>>>(d_point_cloud, d_temp_hashTable, number_of_points, rgd_params); // 计算每个点的栅格索引
    err = cudaDeviceSynchronize();
    if (err != ::cudaSuccess)
        return err;

    try
    {
        thrust::device_ptr<hashElement> t_d_temp_hashTable(d_temp_hashTable);                            // 将临时hash表转换为thrust::device_ptr
        thrust::sort(t_d_temp_hashTable, t_d_temp_hashTable + number_of_points, compareHashElements()); // 对临时hash表进行排序
    }
    catch (thrust::system_error &e)
    {
        err = cudaGetLastError();
        return err;
    }
    catch (std::bad_alloc &e)
    {
        err = cudaGetLastError();
        return err;
    }

    kernel_initializeBuckets<<<rgd_params.number_of_buckets / threads + 1, threads>>>(d_buckets, rgd_params); // 初始化栅格
    err = cudaDeviceSynchronize();                                                                              // 同步
    if (err != ::cudaSuccess)
        return err;

    kernel_updateBuckets<<<blocks, threads>>>(d_temp_hashTable, d_buckets, rgd_params, number_of_points); // 更新栅格
    err = cudaDeviceSynchronize();
    if (err != ::cudaSuccess)
        return err;

    kernel_countNumberOfPointsForBuckets<<<rgd_params.number_of_buckets / threads + 1, threads>>>(d_buckets, rgd_params); // 计算每个栅格的点数
    err = cudaDeviceSynchronize();
    if (err != ::cudaSuccess)
        return err;

    kernel_copyKeys<<<blocks, threads>>>(d_temp_hashTable, d_hashTable, number_of_points); // 计算每个栅格的起始点索引和终止点索引
    err = cudaDeviceSynchronize();
    if (err != ::cudaSuccess)
        return err;

    err = cudaFree(d_temp_hashTable); // 释放临时hash表的内存
    return err;
}

下面这段代码定义了两个CUDA kernel函数。第一个函数是kernel_setAllPointsToRemove，它是一个全局函数。该函数的目的是将一个布尔数组d_markers中所有元素的值都设置为false。该函数的输入参数为布尔数组d_markers和整型变量number_of_points，其中number_of_points表示布尔数组中元素的个数。该函数使用了线程索引变量ind来表示线程的编号。该函数首先计算当前线程的索引ind，然后判断ind是否小于number_of_points，如果成立，就将d_markers[ind]设置为false。

第二个函数是kernel_markFirstPointInBuckets，它也是一个全局函数。该函数的目的是将每个栅格中的第一个点的标记设置为true。该函数的输入参数为布尔数组d_markers、哈希表d_hashTable、栅格数组d_buckets和gridParameters结构体rgd_params。其中，d_markers是一个布尔数组，表示每个点是否应该被删除；d_hashTable是一个哈希表，存储了每个点的索引和它所在的栅格索引；d_buckets是一个栅格数组，存储了每个栅格的起始点和终止点的索引。

__global__ void kernel_setAllPointsToRemove(bool *d_markers, int number_of_points) // 将所有点的标记设置为false
{
    int ind = blockIdx.x * blockDim.x + threadIdx.x;
    if (ind < number_of_points) // 判断线程索引是否越界
    {
        d_markers[ind] = false;
    }
}

__global__ void kernel_markFirstPointInBuckets(bool *d_markers, hashElement *d_hashTable, bucket *d_buckets,
                                               gridParameters rgd_params) // 将每个栅格的第一个点的标记设置为true
{
    int ind = blockIdx.x * blockDim.x + threadIdx.x;
    if (ind < rgd_params.number_of_buckets) // 判断线程索引是否越界
    {
        int index_of_point = d_buckets[ind].index_begin; // 获取对应索引的栅格的起始点索引
        if (index_of_point != -1)
        {
            d_markers[d_hashTable[index_of_point].index_of_point] = true; // 将对应索引的栅格的起始点索引对应的点的标记设置为true
        }
    }
}

下面这段代码是一个名为cudaDownSample的函数，用于执行点云下采样操作。函数参数包括线程数量、点标记数组、哈希表、栅格桶数组、栅格参数和点的数量。函数使用kernel_setAllPointsToRemove核函数将所有点的标记设置为false。然后，函数使用kernel_markFirstPointInBuckets核函数将每个栅格的第一个点的标记设置为true。函数最终返回一个cudaError_t类型的错误码，指示函数是否成功执行。如果函数执行期间出现错误，则返回相应的错误码。

cudaError_t cudaDownSample(int threads, bool *d_markers,
                           hashElement *d_hashTable, bucket *d_buckets, gridParameters rgd_params, int number_of_points) // 下采样
{
    cudaError_t err = cudaGetLastError();                                                                   // 获取错误
    kernel_setAllPointsToRemove<<<number_of_points / threads + 1, threads>>>(d_markers, number_of_points); // 将所有点的标记设置为false
    err = cudaDeviceSynchronize();                                                                           // 同步
    if (err != ::cudaSuccess)
        return err;

    kernel_markFirstPointInBuckets<<<rgd_params.number_of_buckets / threads + 1, threads>>>(d_markers, d_hashTable, d_buckets, rgd_params); // 将每个栅格的第一个点的标记设置为true
    err = cudaDeviceSynchronize();                                                                                                            // 同步

    return err;
}

下面我们来看这些代码是怎么引用的。这段代码是一个CUDA Wrapper类的一个成员函数，用于对一个三维点云进行下采样。以下是代码的大致流程：

1. 设置CUDA设备并检查是否成功。
2. 计算点云的包围盒，从而获得网格参数。
3. 分配CUDA设备内存并将点云从主机内存拷贝到设备内存。
4. 分配哈希表、栅格和标记内存。
5. 调用CUDA kernel函数计算哈希表和栅格。
6. 调用CUDA kernel函数执行下采样操作。
7. 将结果从设备内存拷贝回主机内存。
8. 根据标记选出被保留的点云，存入downsampled_point_cloud中。
9. 将downsampled_point_cloud赋值给point_cloud，完成下采样。
10. 释放CUDA设备内存。

bool downsampling(pcl::PointCloud<pcl::PointXYZ> &point_cloud, float resolution)
{
    cudaError_t err = ::cudaSuccess; // 设置错误类型
    err = cudaSetDevice(0);             // 设置设备
    if (err != ::cudaSuccess)
        return false;

    gridParameters rgd_params;                     // 设置网格参数
    pcl::PointXYZ *d_point_cloud;                 // 设置点云指针
    hashElement *d_hashTable = NULL;             // 设置哈希表指针
    bucket *d_buckets = NULL;                     // 设置栅格指针
    bool *d_markers;                             // 设置标记指针,用于标记是否被选中,设备DEVICE
    bool *h_markers;                             // 设置标记指针,用于标记是否被选中,主机HOST
    int threads = getNumberOfAvailableThreads(); // 获取线程数

    std::cout << "CUDA code will use " << threads << " device threads" << std::endl;
    if (threads == 0)
        return false;

    err = cudaMalloc((void **)&d_point_cloud, point_cloud.points.size() * sizeof(pcl::PointXYZ)); // 为点云分配内存
    if (err != ::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 = cudaCalculateGridParams(d_point_cloud, point_cloud.points.size(),
                                  resolution, resolution, resolution, rgd_params); // 计算网格参数,这里面的算法在另一个文件中
    if (err != ::cudaSuccess)
        return false;

    std::cout << "regular grid parameters:" << std::endl;
    std::cout << "bounding_box_min_X: " << rgd_params.bounding_box_min_X << std::endl;
    std::cout << "bounding_box_min_Y: " << rgd_params.bounding_box_min_Y << std::endl;
    std::cout << "bounding_box_min_Z: " << rgd_params.bounding_box_min_Z << std::endl;
    std::cout << "bounding_box_max_X: " << rgd_params.bounding_box_max_X << std::endl;
    std::cout << "bounding_box_max_Y: " << rgd_params.bounding_box_max_Y << std::endl;
    std::cout << "bounding_box_max_Z: " << rgd_params.bounding_box_max_Z << std::endl;
    std::cout << "number_of_buckets_X: " << rgd_params.number_of_buckets_X << std::endl;
    std::cout << "number_of_buckets_Y: " << rgd_params.number_of_buckets_Y << std::endl;
    std::cout << "number_of_buckets_Z: " << rgd_params.number_of_buckets_Z << std::endl;
    std::cout << "resolution_X: " << rgd_params.resolution_X << std::endl;
    std::cout << "resolution_Y: " << rgd_params.resolution_Y << std::endl;
    std::cout << "resolution_Z: " << rgd_params.resolution_Z << std::endl;

    err = cudaMalloc((void **)&d_hashTable, point_cloud.points.size() * sizeof(hashElement)); // 为哈希表分配内存
    if (err != ::cudaSuccess)
        return false;

    err = cudaMalloc((void **)&d_buckets, rgd_params.number_of_buckets * sizeof(bucket)); // 为栅格分配内存
    if (err != ::cudaSuccess)
        return false;

    err = cudaCalculateGrid(threads, d_point_cloud, d_buckets, d_hashTable, point_cloud.points.size(), rgd_params); // 计算网格
    if (err != ::cudaSuccess)
        return false;

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

    err = cudaDownSample(threads, d_markers, d_hashTable, d_buckets, rgd_params, point_cloud.points.size()); // 下采样,这里面的算法在另一个文件中
    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> downsampled_point_cloud;
    for (size_t i = 0; i < point_cloud.points.size(); i++)
    {
        if (h_markers[i])
            downsampled_point_cloud.push_back(point_cloud[i]); // 将被选中的点云添加到新的点云中
    }

    std::cout << "Number of points before down-sampling: " << point_cloud.size() << std::endl;

    point_cloud = downsampled_point_cloud;
    std::cout << "Number of points after down-sampling: " << point_cloud.size() << std::endl;

    std::cout << "Before cudaFree" << std::endl;
    coutMemoryStatus();

    free(h_markers);

    err = cudaFree(d_point_cloud);
    d_point_cloud = NULL;
    if (err != ::cudaSuccess)
        return false;

    err = cudaFree(d_hashTable);
    d_hashTable = NULL;
    if (err != ::cudaSuccess)
        return false;

    err = cudaFree(d_buckets);
    d_buckets = NULL;
    if (err != ::cudaSuccess)
        return false;

    err = cudaFree(d_markers);
    d_markers = NULL;
    if (err != ::cudaSuccess)
        return false;

    std::cout << "After cudaFree" << std::endl;
    coutMemoryStatus();

    return true;
}

3. Thrust代码完成加速

完成加速该算法首先将输入点云按照给定分辨率进行排序，然后使用thrust::unique函数去除重复的点，最后返回下采样后的点云。其中point_comp是自定义的点云比较器，用于排序和去重。

#include <thrust/device_vector.h>
#include <thrust/sort.h>
#include <thrust/remove.h>

// 定义点云结构体
struct PointXYZI {
    float x, y, z;
    float intensity;
};

// 定义点云比较器，用于排序和去重
struct point_comp {
    float resolution;
    point_comp(float res) : resolution(res) {}
    __host__ __device__
    bool operator()(const PointXYZI& a, const PointXYZI& b) const {
        int pos_diff_x = static_cast<int>(std::floor(a.x / resolution)) - static_cast<int>(std::floor(b.x / resolution));
        if (pos_diff_x != 0) {
            return pos_diff_x < 0;
        }
        int pos_diff_y = static_cast<int>(std::floor(a.y / resolution)) - static_cast<int>(std::floor(b.y / resolution));
        if (pos_diff_y != 0) {
            return pos_diff_y < 0;
        }
        int pos_diff_z = static_cast<int>(std::floor(a.z / resolution)) - static_cast<int>(std::floor(b.z / resolution));
        if (pos_diff_z != 0) {
            return pos_diff_z < 0;
        }
        return false;
    }
};

thrust::device_vector<PointXYZI> downsampling(const thrust::device_vector<PointXYZI> &point_cloud, float resolution) {
    // 复制输入点云到设备内存
    thrust::device_vector<PointXYZI> input_cloud(point_cloud);

    // 根据给定分辨率进行排序
    point_comp comp(resolution);
    thrust::sort(input_cloud.begin(), input_cloud.end(), comp);

    // 去除重复的点
    auto new_end = thrust::unique(input_cloud.begin(), input_cloud.end(), comp);
    input_cloud.resize(new_end - input_cloud.begin());

    // 返回下采样后的点云
    return input_cloud;
}

4. 结果显示