0. 简介
之前我们讲过通过体素化分割，并通过判断这个栅格内的点云数目是否大于阈值。从而来鉴别出噪点。而我们学过最近邻搜索后，我们可以来学习一下更加先进的方法---半径搜索噪声滤除（Radius Search Noise Filtering）。这是点云处理中一种常见的算法，其基本思想是对每一个点周围的点进行半径搜索，如果搜索到的点数小于设定的阈值，那么就认为该点是噪声点，将其删除。
1. CUDA与Thrust
在使用CUDA和Thrust实现半径搜索噪声滤除时，首先需要将点云数据和搜索半径拷贝到GPU上，然后将点云数据按照坐标顺序进行排序，这样可以方便后续的查找和操作。接着需要构建一个空间索引结构，将点云数据分配到对应的空间桶中，这样可以快速找到每个点周围的邻居点。在构建空间索引时，可以使用CUDA的kernel函数来并行处理每个点的分配过程，提高计算效率。构建完空间索引之后，需要使用CUDA的kernel函数来进行半径搜索。对于每一个点，首先找到其对应的空间桶，并遍历该桶周围的所有桶，以找到所有在半径范围内的点。最后，需要将所有的噪声点标记出来，并将其从点云数据中删除。
而对于Thrust而言可以大大简化代码实现，提高算法效率。例如，可以使用Thrust中的排序算法对每个点的邻域进行排序，同时也可以使用Thrust的函数和算法对离群点进行标记和移除。从而快速定位和处理每个桶中的点数据，以减少内存访问的开销。
2. CUDA代码完成加速
首先我们来看一个一个GPU的kernel函数，用于对输入的点云进行处理，通过查找邻居点来标记哪些点可以保留。
该函数使用了一个哈希表和一个网格桶来加速邻居点的查找。对于每一个输入点，首先进行边界检查，如果该点不在边界框内，则将其标记为不保留；否则，计算该点所在的网格桶的索引，遍历该点所在网格桶的邻居桶，然后在这些桶中查找邻居点。
在查找邻居点时，首先判断点到目标点的距离是否小于给定的搜索半径，如果小于，则将该点加入到一个候选邻居点列表中。如果候选邻居点数量达到了指定的阈值，则直接标记该点为保留点，并跳出循环；否则，将继续在其他桶中查找邻居点。如果所有的桶都被遍历完毕，而候选邻居点数量仍然未达到指定的阈值，则将该点标记为不保留。
最终，对于每个输入点，都会有一个标记值，用于指示该点是否应该保留。这些标记值被写入到一个布尔数组中，以设备指针形式传入。
cpp
//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 调用之后检查是否有错误，并在需要时返回错误码。
cpp
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库实现了点云去噪的功能，具体实现如下：
- 定义了一个名为PointXYZI的结构体，结构体包含了x、y、z和intensity四个成员变量。
- 定义了一个名为
squared_euclidean_distance的结构体，这个结构体继承自thrust::binary_function<PointXYZI, PointXYZI, float>，表示计算两个PointXYZI点之间的欧氏距离平方，即 ( d x ) 2 + ( d y ) 2 + ( d z ) 2 (dx)^2+(dy)^2+(dz)^2 (dx)2+(dy)2+(dz)2。 - 定义了一个名为
is_noise的结构体，这个结构体继承自thrust::unary_function<PointXYZI, bool>，表示检查一个PointXYZI点是否为噪点。 - 定义了一个名为
removeNoise的函数，这个函数的输入为一组点云数据，以及去噪所需的一些参数，输出为去除噪点后的点云数据。 - 函数
removeNoise内部首先创建了一个名为sorted_indices的device_vector<int>，并用thrust::sequence函数对其进行初始化，该函数返回一个按顺序填充的序列。 - 然后使用thrust::sort_by_key对原始点云数据按照z坐标排序，并将排序后的点云数据存储在名为points的
device_vector<PointXYZI>中，排序后的点云索引存储在名为idxs的device_vector<int>中。 - 接着使用
thrust::remove_if函数移除噪点，其中，is_noise结构体的对象被作为第二个参数传递给thrust::remove_if函数，它表示判断一个PointXYZI点是否为噪点的条件。移除后的点云数据存储在名为new_end的迭代器中。 - 最后，使用
thrust::erase函数删除新点云数据中的已经移除的噪点。 - 返回最终的点云数据points。