0. 简介

之前我们讲过通过体素化分割，并通过判断这个栅格内的点云数目是否大于阈值。从而来鉴别出噪点。而我们学过最近邻搜索后，我们可以来学习一下更加先进的方法—-半径搜索噪声滤除（Radius Search Noise Filtering）。这是点云处理中一种常见的算法，其基本思想是对每一个点周围的点进行半径搜索，如果搜索到的点数小于设定的阈值，那么就认为该点是噪声点，将其删除。

1. CUDA与Thrust

在使用CUDA和Thrust实现半径搜索噪声滤除时，首先需要将点云数据和搜索半径拷贝到GPU上，然后将点云数据按照坐标顺序进行排序，这样可以方便后续的查找和操作。接着需要构建一个空间索引结构，将点云数据分配到对应的空间桶中，这样可以快速找到每个点周围的邻居点。在构建空间索引时，可以使用CUDA的kernel函数来并行处理每个点的分配过程，提高计算效率。构建完空间索引之后，需要使用CUDA的kernel函数来进行半径搜索。对于每一个点，首先找到其对应的空间桶，并遍历该桶周围的所有桶，以找到所有在半径范围内的点。最后，需要将所有的噪声点标记出来，并将其从点云数据中删除。

而对于Thrust而言可以大大简化代码实现，提高算法效率。例如，可以使用Thrust中的排序算法对每个点的邻域进行排序，同时也可以使用Thrust的函数和算法对离群点进行标记和移除。从而快速定位和处理每个桶中的点数据，以减少内存访问的开销。

2. CUDA代码完成加速

首先我们来看一个一个GPU的kernel函数，用于对输入的点云进行处理，通过查找邻居点来标记哪些点可以保留。

该函数使用了一个哈希表和一个网格桶来加速邻居点的查找。对于每一个输入点，首先进行边界检查，如果该点不在边界框内，则将其标记为不保留；否则，计算该点所在的网格桶的索引，遍历该点所在网格桶的邻居桶，然后在这些桶中查找邻居点。

在查找邻居点时，首先判断点到目标点的距离是否小于给定的搜索半径，如果小于，则将该点加入到一个候选邻居点列表中。如果候选邻居点数量达到了指定的阈值，则直接标记该点为保留点，并跳出循环；否则，将继续在其他桶中查找邻居点。如果所有的桶都被遍历完毕，而候选邻居点数量仍然未达到指定的阈值，则将该点标记为不保留。

最终，对于每个输入点，都会有一个标记值，用于指示该点是否应该保留。这些标记值被写入到一个布尔数组中，以设备指针形式传入。

//d_point_cloud：输入点云，以设备指针形式传入。
//number_of_points：输入点云的点数。
//d_hashTable：哈希表，用于加速邻居点查找。
//d_buckets：存储每个哈希桶信息的数组。
//rgd_params：格网参数，包括网格分辨率、边界框大小等。
//search_radius：用于查找邻居点的搜索半径。
//number_of_points_in_search_sphere_threshold：设定的邻居点数量阈值。
//max_number_considered_in_INNER_bucket：内部桶中最多考虑的点数。
//max_number_considered_in_OUTER_bucket：外部桶中最多考虑的点数。
//d_markers_out：输出结果，用于标记点是否保留，以设备指针形式传入。
__global__ void kernel_markPointsToRemain(pcl::PointXYZ *d_point_cloud,
        int number_of_points,
        hashElement *d_hashTable,
        bucket *d_buckets,
        gridParameters rgd_params,
        float search_radius,
        int number_of_points_in_search_sphere_threshold,
        int max_number_considered_in_INNER_bucket,
        int max_number_considered_in_OUTER_bucket,
        bool *d_markers_out)
{
    int index_of_point = blockIdx.x*blockDim.x+threadIdx.x;//根据线程号和块号计算当前处理的点的索引

    if(index_of_point < number_of_points)
    {
        int number_of_found_points_in_search_sphere_threshold = 0;

        float x = d_point_cloud[index_of_point].x;
        float y = d_point_cloud[index_of_point].y;
        float z = d_point_cloud[index_of_point].z;

        if(x < rgd_params.bounding_box_min_X || x > rgd_params.bounding_box_max_X)//对该点进行边界检查，如果不在边界框内
        {
            d_markers_out[index_of_point] = false;
            return;
        }
        if(y < rgd_params.bounding_box_min_Y || y > rgd_params.bounding_box_max_Y)//对该点进行边界检查，如果不在边界框内
        {
            d_markers_out[index_of_point] = false;
            return;
        }
        if(z < rgd_params.bounding_box_min_Z || z > rgd_params.bounding_box_max_Z)
        {
            d_markers_out[index_of_point] = false;
            return;
        }

        int ix=(x - rgd_params.bounding_box_min_X)/rgd_params.resolution_X;//通过该点的坐标以及格网参数计算该点所在的格网桶的索引
        int iy=(y - rgd_params.bounding_box_min_Y)/rgd_params.resolution_Y;
        int iz=(z - rgd_params.bounding_box_min_Z)/rgd_params.resolution_Z;

        int index_bucket = ix*rgd_params.number_of_buckets_Y *
                rgd_params.number_of_buckets_Z + iy * rgd_params.number_of_buckets_Z + iz;//计算该点所在的格网桶的索引


        if(index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets)//如果该点所在的格网桶索引合法,rgd_params：三维空间的参数，包括空间的大小和每个桶的大小。
        {
            int sx, sy, sz, stx, sty, stz;//当前点所在的桶的邻居桶的位置，用于遍历周围桶中的点
            if(ix == 0)sx = 0; else sx = -1;
            if(iy == 0)sy = 0; else sy = -1;
            if(iz == 0)sz = 0; else sz =- 1;

            if(ix == rgd_params.number_of_buckets_X - 1)stx = 1; else stx = 2;
            if(iy == rgd_params.number_of_buckets_Y - 1)sty = 1; else sty = 2;
            if(iz == rgd_params.number_of_buckets_Z - 1)stz = 1; else stz = 2;

            int index_next_bucket;//当前桶的邻居桶的索引
            int iter;
            int number_of_points_in_bucket;
            int l_begin;
            int l_end;

            for(int i = sx; i < stx; i++)
            {
                for(int j = sy; j < sty; j++)
                {
                    for(int k = sz; k < stz; k++)//首先判断当前点所在的桶是否存在（即索引是否在合法范围内）
                    {
                        index_next_bucket = index_bucket +
                                i * rgd_params.number_of_buckets_Y * rgd_params.number_of_buckets_Z +
                                j * rgd_params.number_of_buckets_Z + k;
                        if(index_next_bucket >= 0 && index_next_bucket < rgd_params.number_of_buckets)//如果当前点所在的桶存在
                        {
                            number_of_points_in_bucket = d_buckets[index_next_bucket].number_of_points;
                            if(number_of_points_in_bucket <= 0)continue;

                            int max_number_considered_in_bucket;
                            if(index_next_bucket == index_bucket)
                            {
                                max_number_considered_in_bucket = max_number_considered_in_INNER_bucket;
                            }else
                            {
                                max_number_considered_in_bucket = max_number_considered_in_OUTER_bucket;
                            }
                            if(max_number_considered_in_bucket <= 0)continue;

                            if(max_number_considered_in_bucket >= number_of_points_in_bucket)
                            {
                                iter=1;
                            }else
                            {
                                iter = number_of_points_in_bucket / max_number_considered_in_bucket;
                                if(iter <= 0)iter = 1;
                            }

                            l_begin = d_buckets[index_next_bucket].index_begin;//计算当前桶中的点的索引范围
                            l_end = d_buckets[index_next_bucket].index_end;

                            for(int l = l_begin; l < l_end; l += iter)
                            {
                                if(l >= 0 && l < number_of_points)
                                {
                                    int hashed_index_of_point = d_hashTable[l].index_of_point;

                                    float nn_x  = d_point_cloud[hashed_index_of_point].x;
                                    float nn_y  = d_point_cloud[hashed_index_of_point].y;
                                    float nn_z  = d_point_cloud[hashed_index_of_point].z;

                                    float dist  = (x - nn_x) * (x - nn_x) +
                                                  (y - nn_y) * (y - nn_y) +
                                                  (z - nn_z) * (z - nn_z);//计算当前点与周围点的距离

                                    if(dist <= search_radius * search_radius)//计算当前点所在桶的邻居桶的位置，遍历周围桶中的点
                                    {
                                        number_of_found_points_in_search_sphere_threshold++;//如果距离小于阈值，则计数器加1
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }

        if(number_of_found_points_in_search_sphere_threshold >= number_of_points_in_search_sphere_threshold + 1)//如果计数器大于阈值，则保留该点
        {
            d_markers_out[index_of_point] = true;//将该点的标记设为true
        }else
        {
            d_markers_out[index_of_point] = false;
        }
    }
}

然后下面就是主调用函数，该函数的主要流程是首先调用 kernel_setAllPointsToRemove 函数将所有点标记为噪声（这个函数可以在第五章找到）。然后调用 kernel_markPointsToRemain 函数，使用哈希表和桶进行空间搜索，标记不是噪声的点。并在每次 CUDA 调用之后检查是否有错误，并在需要时返回错误码。

cudaError_t cudaRemoveNoise(
            int threads,
            pcl::PointXYZ *d_point_cloud,
            int number_of_points,
            hashElement *d_hashTable,
            bucket *d_buckets,
            gridParameters rgd_params,
            float search_radius,
            int number_of_points_in_search_sphere_threshold,
            int max_number_considered_in_INNER_bucket,
            int max_number_considered_in_OUTER_bucket,
            bool *d_markers_out)
{
    cudaError_t err = cudaGetLastError();

    kernel_setAllPointsToRemove<<<number_of_points/threads+1,threads>>>(number_of_points, d_markers_out);
    err = cudaDeviceSynchronize();    if(err != ::cudaSuccess)return err;

    kernel_markPointsToRemain<<<number_of_points/threads+1,threads>>>
                   (d_point_cloud,
                    number_of_points,
                    d_hashTable,
                    d_buckets,
                    rgd_params,
                    search_radius,
                    number_of_points_in_search_sphere_threshold,
                    max_number_considered_in_INNER_bucket,
                    max_number_considered_in_OUTER_bucket,
                    d_markers_out);
    err = cudaDeviceSynchronize();

    return err;
}

3. Thrust代码完成加速

下面这段代码主要是使用Thrust库实现了点云去噪的功能，具体实现如下：

1. 定义了一个名为PointXYZI的结构体，结构体包含了x、y、z和intensity四个成员变量。
2. 定义了一个名为squared_euclidean_distance的结构体，这个结构体继承自thrust::binary_function<PointXYZI, PointXYZI, float>，表示计算两个PointXYZI点之间的欧氏距离平方，即$(dx)^2+(dy)^2+(dz)^2$。
3. 定义了一个名为is_noise的结构体，这个结构体继承自thrust::unary_function<PointXYZI, bool>，表示检查一个PointXYZI点是否为噪点。
4. 定义了一个名为removeNoise的函数，这个函数的输入为一组点云数据，以及去噪所需的一些参数，输出为去除噪点后的点云数据。
5. 函数removeNoise内部首先创建了一个名为sorted_indices的device_vector<int>，并用thrust::sequence函数对其进行初始化，该函数返回一个按顺序填充的序列。
6. 然后使用thrust::sort_by_key对原始点云数据按照z坐标排序，并将排序后的点云数据存储在名为points的device_vector<PointXYZI>中，排序后的点云索引存储在名为idxs的device_vector<int>中。
7. 接着使用thrust::remove_if函数移除噪点，其中，is_noise结构体的对象被作为第二个参数传递给thrust::remove_if函数，它表示判断一个PointXYZI点是否为噪点的条件。移除后的点云数据存储在名为new_end的迭代器中。
8. 最后，使用thrust::erase函数删除新点云数据中的已经移除的噪点。
9. 返回最终的点云数据points。

#include <thrust/device_vector.h>
#include <thrust/functional.h>
#include <thrust/iterator/zip_iterator.h>
#include <thrust/transform.h>
#include <thrust/tuple.h>
#include <thrust/remove.h>
#include <thrust/host_vector.h>
#include <thrust/device_ptr.h>
#include <thrust/sequence.h>
#include <thrust/sort.h>
#include <thrust/unique.h>
#include <thrust/execution_policy.h>

typedef struct PointXYZI {
  float x, y, z, intensity;
} PointXYZI;

// Define a functor for computing the squared Euclidean distance
struct squared_euclidean_distance : public thrust::binary_function<PointXYZI, PointXYZI, float>
{
  __host__ __device__
  float operator()(const PointXYZI& a, const PointXYZI& b) const
  {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    float dz = a.z - b.z;
    return dx*dx + dy*dy + dz*dz;
  }
};

// Define a functor for checking whether a point is noise or not
struct is_noise : public thrust::unary_function<PointXYZI, bool>
{
  const float search_radius;
  const int number_of_points_in_search_sphere_threshold;
  const int max_number_considered_in_INNER_bucket;
  const int max_number_considered_in_OUTER_bucket;

  is_noise(float sr, int nsst, int mnicib, int mnciob) :
    search_radius(sr), number_of_points_in_search_sphere_threshold(nsst),
    max_number_considered_in_INNER_bucket(mnicib), max_number_considered_in_OUTER_bucket(mnciob) {}

  __host__ __device__
  bool operator()(const PointXYZI& p) const
  {
    int count_INNER_bucket = 0, count_OUTER_bucket = 0;
    float r2 = search_radius * search_radius;

    // Iterate over points in search radius
    thrust::counting_iterator<int> search_sphere_begin(0);
    thrust::counting_iterator<int> search_sphere_end = search_sphere_begin + sorted_indices.size();
    thrust::transform(thrust::device, search_sphere_begin, search_sphere_end, sorted_indices.begin(),
                      thrust::make_zip_iterator(thrust::make_tuple(points.begin(), points.begin() + idxs.size())),
                      squared_euclidean_distance());
    thrust::for_each(thrust::device, squared_distances.begin(), squared_distances.end(),
                     [&count_INNER_bucket, &count_OUTER_bucket, r2, this] __device__ (float squared_distance) {
                       if (squared_distance < r2) {
                         ++count_INNER_bucket;
                       } else if (squared_distance < 4*r2) {
                         ++count_OUTER_bucket;
                       }
                     });

    // Check noise condition
    if (count_INNER_bucket < number_of_points_in_search_sphere_threshold) {
      return true;
    } else if (count_OUTER_bucket > max_number_considered_in_OUTER_bucket) {
      return true;
    } else {
      return false;
    }
  }
};



thrust::device_vector<PointXYZI> removeNoise(const thrust::device_vector<PointXYZI> &orig_points, float search_radius, int number_of_points_in_search_sphere_threshold, int max_number_considered_in_INNER_bucket, int max_number_considered_in_OUTER_bucket) {
// Create device vector to hold indices of sorted points
thrust::device_vector<int> sorted_indices(orig_points.size());
thrust::sequence(sorted_indices.begin(), sorted_indices.end());

// Sort points by z-coordinate
thrust::sort_by_key(thrust::device, orig_points.begin(), orig_points.end(), sorted_indices.begin(),
[] device (const PointXYZI& a, const PointXYZI& b) {
return a.z < b.z;
});

// Remove noise points
thrust::device_vector<PointXYZI> points(orig_points);
thrust::device_vector<int> idxs(sorted_indices);
thrust::device_vector<float> squared_distances(sorted_indices.size());

auto new_end = thrust::remove_if(points.begin(), points.end(), is_noise(search_radius, number_of_points_in_search_sphere_threshold, max_number_considered_in_INNER_bucket, max_number_considered_in_OUTER_bucket));

// Erase removed elements
points.erase(new_end, points.end());

return points;
}

4. 结果显示