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;
		}
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143

然后下面就是主调用函数，该函数的主要流程是首先调用 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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

3. Thrust代码完成加速

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

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

…详情请参照古月居