thrust工程化学习(五)----噪声滤除

0. 简介

上一节中我们讲了pcl::VoxelGrid数据中的降采样，至此为止最简单的三个部分就已经讲解完毕了，下面我们来更进阶一点，这一讲主要来看一下噪声滤除这部分的操作。噪声滤除是点云处理中的一项重要任务，可以提高点云的质量和准确性。在噪声滤除过程中，通常需要根据点云密度来估计合适的邻域半径，并在邻域内对点进行平滑处理，以消除噪声点。

1. CUDA与Thrust

CUDA是一种并行计算平台，可以利用GPU的高并发能力加速计算。在CUDA中，可以使用CUDA C++语言编写GPU核函数，然后在主机代码中调用这些核函数。对于噪声滤除任务，可以使用CUDA实现基于体素网格的滤波器，如VoxelGrid滤波器。该算法将点云分成小体素，并在每个体素内对点进行平均化，从而消除噪声点。使用CUDA可以加速体素网格的计算过程，从而加速噪声滤除过程。

Thrust是一个高性能的C++模板库，用于并行算法设计。Thrust提供了一组高效的并行算法，可以用于STL容器和CUDA设备。在噪声滤除中，算法的核心思想是将点云数据划分为不同的体素（或栅格），在每个体素中只选择一个代表点作为保留点。由于点云数据的空间结构非常重要，所以该算法将点云数据转换为三维栅格。每个栅格代表一个体素，并存储了栅格中所有点的信息。。这些滤波器可以通过查找每个点的邻居来估计合适的半径，并在邻域内对点进行平滑处理。使用Thrust可以轻松地编写高效的并行滤波器，同时可以利用Thrust的算法库提供的优化来加速滤波器计算。

2. CUDA代码完成加速

首先我们来看一下，下面的两个CUDA核函数。第一个函数kernel_setAllPointsToRemove的作用是将一个长度为number_of_points的布尔型数组d_markers中的所有元素都设置为false。它接受两个参数：d_markers表示需要操作的布尔型数组的指针，number_of_points表示数组的长度。该函数使用CUDA线程并行地对数组进行操作。

第二个函数kernel_markFirstPointInBuckets的作用是将每个栅格的第一个点的标记设置为true。它接受四个参数：d_markers表示需要操作的布尔型数组的指针，d_hashTable表示哈希表，d_buckets表示栅格索引，rgd_params表示栅格化参数。该函数使用CUDA线程并行地对哈希表和栅格索引行操作，遍历每个栅格索引，找到对应的栅格的起始点索引，并将该索引对应的点的标记设置为true。

__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
		}
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

下面这段代码是使用CUDA加速的噪声去除函数，它的主要功能是将点云中的噪声点标记为待移除点。该函数的输入参数包括：

• threads：每个线程块中的线程数。
• d_markers：一个大小为number_of_points的布尔数组，用于标记哪些点是噪声点。
• d_point_cloud：指向点云数据的指针。
• d_hashTable：哈希表数据结构的指针。
• d_buckets：存储哈希表的桶的数组的指针。
• rgd_params：栅格化参数。
• number_of_points：点云中点的数量。
• number_of_points_in_bucket_threshold：每个桶中最少点的数量，用于判断是否需要继续计算。

该函数的主要流程如下：

1. 调用kernel_setAllPointsToRemove函数，将所有点的标记设置为false。
2. 调用kernel_markPointsToRemain函数，将每个栅格的第一个点的标记设置为true。
3. 返回CUDA运行时的错误码。

总的来说，该函数是一个相对简单的噪声去除算法，它使用了哈希表和栅格化技术来提高效率。在kernel_markPointsToRemain函数中，每个线程负责处理一个点，根据该点所在的栅格位置来判断它是否应该被保留。如果一个栅格中的第一个点数量小于指定的阈值，则该栅格中的所有点都被标记为待移除点。

// 将每个栅格的最后一个点的标记设置为true
cudaError_t cudaRemoveNoiseNaive(int threads, bool *d_markers, pcl::PointXYZ *d_point_cloud,
								 hashElement *d_hashTable, bucket *d_buckets, gridParameters rgd_params, int number_of_points,
								 int number_of_points_in_bucket_threshold)
{
	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_markPointsToRemain<<<number_of_points / threads + 1, threads>>>(d_markers, d_point_cloud, d_hashTable, d_buckets, rgd_params, number_of_points, number_of_points_in_bucket_threshold); // 将每个栅格的第一个点的标记设置为true

	err = cudaDeviceSynchronize(); // 同步
	return err;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

然后就是如何调用点云降噪的CUDA实现。它将点云从主机复制到设备，并计算栅格和哈希表。之后使用CUDA核函数计算噪声标记，并将没有噪声的点云添加到新的点云中。最后，释放已分配的内存。其中，包含使用CUDA核函数计算噪声标记和计算栅格和哈希表的函数都是在我们上一节中就已经提到了，大家感兴趣的可以去看一下上一章的内容。

bool removeNoiseNaive(pcl::PointCloud<pcl::PointXYZ> &point_cloud, float resolution, int number_of_points_in_bucket_threshold)
{
	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 = cudaRemoveNoiseNaive(threads, d_markers, d_point_cloud, d_hashTable, d_buckets, rgd_params, point_cloud.points.size(), number_of_points_in_bucket_threshold); // 计算噪点的d_markers标记，,这里面的算法在另一个文件中
	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> filtered_point_cloud;
	for (size_t i = 0; i < point_cloud.points.size(); i++)
	{
		if (h_markers[i])
			filtered_point_cloud.push_back(point_cloud[i]); // 将没有噪点的点云添加到filtered_point_cloud中
	}

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

	point_cloud = filtered_point_cloud;
	std::cout << "Number of points after filtering: " << 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;
}
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

3. Thrust代码完成加速

…详情请参照古月居