thrust工程化学习(七)----噪声滤除进阶版

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库实现了点云去噪的功能，具体实现如下：

定义了一个名为PointXYZI的结构体，结构体包含了x、y、z和intensity四个成员变量。
定义了一个名为squared_euclidean_distance的结构体，这个结构体继承自thrust::binary_function<PointXYZI, PointXYZI, float>，表示计算两个PointXYZI点之间的欧氏距离平方，即 $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。