类似上面的点云中值滤波结构，读取和保存pcd文件，哈夫表构建加速点云的半径滤波，c语言代码示例，不依赖pcl库

<think>我们面临的任务是：用C语言实现点云的中值滤波和半径滤波，包含PCD文件读取与保存，并且使用哈夫表（哈希表）来加速半径滤波，而不依赖PCL库。 由于PCD文件格式有多种，这里我们假设处理的是最简单的ASCII格式的PCD文件。一个典型的ASCII格式的PCD文件如下： ``` # .PCD v0.7 - Point Cloud Data file format VERSION 0.7 FIELDS x y z SIZE 4 4 4 TYPE F F F COUNT 1 1 1 WIDTH 213 HEIGHT 1 VIEWPOINT 0 0 0 1 0 0 0 POINTS 213 DATA ascii 0.93773 0.33763 0 0.90805 0.35641 0 ... (更多点) ``` 我们需要实现以下功能： 1. 读取PCD文件（ASCII格式） 2. 保存PCD文件（ASCII格式） 3. 中值滤波：对于每个点，考虑其邻域（例如，在三维空间中取一个立方体邻域），用邻域中所有点的坐标中值来替代该点坐标。 4. 半径滤波：对于每个点，计算以该点为中心、给定半径内的邻域点个数，如果个数小于某个阈值，则将该点视为离群点并移除。 5. 使用哈希表（哈夫表）加速半径滤波：在半径滤波中，最耗时的操作是搜索给定半径内的邻域点。我们可以使用空间划分数据结构（如网格）来加速。这里使用哈希表，将空间划分为网格，每个网格对应一个哈希桶，每个点根据其坐标落入某个网格。这样，在搜索邻域点时，只需查询当前点所在网格及其相邻网格中的点即可。 由于C语言没有标准哈希表，我们将自己实现一个简单的基于网格的哈希表。具体思路： - 将三维空间划分为立方体网格，网格边长设为半径（或大于半径？注意：为了确保半径内邻域点不遗漏，网格边长应至少等于半径，但这样相邻网格数会固定为27个，但实际中半径可能较大，网格划分过大会导致哈希表效率低。另一种方法是网格边长设为半径的一半，这样邻域搜索需要检查27个相邻网格，可以保证覆盖。但为了简单，我们取网格边长为半径，这样每个点只需检查自身所在网格即可？不对，因为点可能在网格边界，所以需要检查相邻网格。因此我们取网格边长为半径，那么邻域搜索需要检查3x3x3=27个网格。但这样网格划分较大，每个网格内点较多，可能效率不高。折中方法是网格边长取半径的一半，这样需要检查5x5x5=125个网格？这显然不可取。因此，我们采用网格边长等于半径，然后每个点搜索自身网格和26个相邻网格，这样总网格数为27个。这样在网格边长等于半径的情况下，可以保证以点为中心、半径为r的球完全包含在3x3x3的网格中（因为球内任意点与中心的距离不超过r，而网格边长是r，所以最远也不会超过对角线sqrt(3)*r，但我们的搜索半径是r，所以会漏掉一些点？实际上，我们需要的是球内邻域，而网格划分是立方体，所以不能完全保证球内所有点都被搜索到，因为球内点可能分布在27个网格之外？实际上不会，因为一个点如果距离中心点不超过r，那么它在三个坐标轴上的距离都不超过r，因此它一定位于中心点所在网格的相邻网格（即3x3x3网格）内。所以，这样划分是合理的。 步骤： 1. 定义点结构体（包含x,y,z坐标） 2. 读取PCD文件：跳过前面的头部，读取DATA ascii之后的数据，每一行是一个点的三个坐标（假设只有x,y,z） 3. 实现中值滤波：由于中值滤波需要邻域，我们同样可以使用网格哈希表来加速邻域搜索。但为了简单，我们先实现一个暴力方法（适用于小点云），然后再用哈希表优化（这里先实现暴力，后续再优化）。 4. 实现半径滤波（使用哈希表加速） 5. 保存点云到PCD文件 注意：中值滤波和半径滤波都会改变点云，我们分别实现。 由于时间限制，我们只实现核心功能，并尽量保证代码清晰。 计划： 1. 定义数据结构 - Point: 存储一个点 - PointCloud: 存储点云（动态数组） 2. 哈希表结构 - 我们设计一个哈希表，将网格索引（i,j,k）映射到一个点索引列表（即该网格内的点）。 哈希函数：给定一个点(x,y,z)和网格边长r，计算网格索引： i = floor(x/r), j = floor(y/r), k = floor(z/r) 然后，我们用一个哈希表（可以用链表法解决冲突，但这里我们使用一个三维数组？不，因为空间可能很大，我们用哈希表（字典））。在C语言中，我们可以用链表数组来实现哈希表，但这里为了简单，我们用一个固定大小的数组（数组元素是链表头），然后用一个哈希函数将(i,j,k)映射到数组下标。例如，使用一个简单的哈希函数： key = (i * p1 + j * p2 + k * p3) mod HASH_SIZE 其中p1,p2,p3是大质数，HASH_SIZE是哈希表大小（比如10000）。 但是，由于我们要支持动态插入和查询，且网格索引(i,j,k)是整数，我们可以用开放地址法或链表法。这里用链表法：每个桶是一个链表，存储落在该桶的网格索引（以及该网格内的点索引列表）。但注意：同一个网格索引的点都在同一个桶里，所以我们可以将网格索引作为键，每个桶存储一个网格索引和该网格内的点索引列表。 然而，为了简化，我们直接用一个链表数组，每个桶存储一个链表，链表节点包含网格索引(i,j,k)和该网格内的点索引列表（动态数组）。这样，在插入一个点时，计算其网格索引(i,j,k)，然后计算哈希值得到桶索引，在该桶的链表中查找是否有网格(i,j,k)的节点，如果有，将点索引加入该节点的点索引列表；否则，创建一个新节点。 但这样实现起来代码量较大。考虑到点云处理中我们主要需要快速查询某个网格及其相邻网格内的点，我们可以不存储点索引列表，而是存储点指针（或点索引）到网格中。然后，在半径滤波时，对于每个点，我们计算其网格索引，然后查询该网格及其26个相邻网格中的所有点，再计算这些点中哪些在半径内。 为了简化，我们采用以下策略： - 在构建哈希表时，我们只存储每个网格内的点索引列表。 - 在半径滤波时，对于每个点，获取其网格索引，然后遍历该网格以及相邻的26个网格（共27个网格），将这些网格中的所有点收集起来，然后计算这些点中距离当前点小于半径r的点。 这样，虽然可能收集到一些不在半径内的点（因为网格边界外但距离小于r的点不会被收集），但根据前面的分析，只要网格边长设为r，那么距离小于r的点一定在27个网格内。所以不会遗漏。 具体步骤： 1. 读取点云 2. 构建哈希表（网格边长=半径r） - 初始化哈希表（数组+链表） - 遍历所有点，计算每个点所在的网格索引(i,j,k)，然后将其点索引（在点云数组中的下标）加入该网格对应的节点中（如果节点不存在则创建） 3. 对于每个点，获取其网格索引，然后遍历(i-1到i+1, j-1到j+1, k-1到k+1)的网格，将这些网格中的点索引收集起来，形成一个候选点集。 4. 计算当前点与候选点集中每个点的距离，统计距离小于r的点的个数（包括自身）。 5. 如果个数小于阈值（比如5），则标记该点为离群点（移除）。 6. 移除所有标记的点。 中值滤波：类似，但中值滤波通常用于图像，在点云中，我们通常对每个点取其邻域（例如半径r内的点）的坐标中值。同样，我们可以用哈希表加速邻域搜索。但中值滤波需要邻域点的坐标，然后对x,y,z分别取中值。注意：中值滤波后，点云会变得平滑，但也会损失细节。 由于时间关系，我们只实现半径滤波的哈希表加速，中值滤波我们先用暴力方法（遍历所有点），然后如果时间允许再用哈希表优化。 代码结构： - 定义点结构体 - 定义点云结构体（动态数组） - 函数：读取PCD - 函数：保存PCD - 函数：构建哈希表（给定点云和网格边长） - 函数：销毁哈希表 - 函数：半径滤波（使用哈希表加速） - 函数：中值滤波（暴力法） 由于代码较长，我们将分块实现。 注意：由于不依赖PCL，所有功能都需要自己实现。 我们先实现PCD文件读取和保存，假设点云只有x,y,z。 开始编写代码：</think>以下是一个不依赖PCL库的C语言点云处理实现，包含中值滤波、半径滤波、PCD文件读写和哈希表加速功能： ```c #include <stdio.h> #include <stdlib.h> #include <string.h> #include <math.h> #include <time.h> // 点云数据结构 typedef struct { float x, y, z; } Point3D; typedef struct { Point3D* points; int size; int capacity; } PointCloud; // 哈希表加速结构 typedef struct HashEntry { int point_index; struct HashEntry* next; } HashEntry; typedef struct { HashEntry** buckets; int grid_size; float cell_size; } SpatialHash; // ================== PCD文件处理 ================== PointCloud read_pcd(const char* filename) { FILE* file = fopen(filename, "r"); if (!file) { perror("Error opening file"); exit(EXIT_FAILURE); } PointCloud cloud = {NULL, 0, 0}; char line[256]; int points_count = 0; int in_data_section = 0; // 解析头部 while (fgets(line, sizeof(line), file)) { if (strstr(line, "POINTS")) { sscanf(line, "POINTS %d", &points_count); } if (strstr(line, "DATA ascii")) { in_data_section = 1; break; } } if (points_count <= 0) { fclose(file); return cloud; } // 分配内存 cloud.points = malloc(points_count * sizeof(Point3D)); cloud.capacity = points_count; // 读取点数据 for (int i = 0; i < points_count; i++) { if (fscanf(file, "%f %f %f", &cloud.points[i].x, &cloud.points[i].y, &cloud.points[i].z) != 3) { break; } cloud.size++; } fclose(file); return cloud; } void write_pcd(const char* filename, PointCloud* cloud) { FILE* file = fopen(filename, "w"); if (!file) { perror("Error creating file"); return; } // 写入PCD头部 fprintf(file, "# .PCD v0.7 - Point Cloud Data\n"); fprintf(file, "VERSION 0.7\n"); fprintf(file, "FIELDS x y z\n"); fprintf(file, "SIZE 4 4 4\n"); fprintf(file, "TYPE F F F\n"); fprintf(file, "COUNT 1 1 1\n"); fprintf(file, "WIDTH %d\n", cloud->size); fprintf(file, "HEIGHT 1\n"); fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n"); fprintf(file, "POINTS %d\n", cloud->size); fprintf(file, "DATA ascii\n"); // 写入点数据 for (int i = 0; i < cloud->size; i++) { fprintf(file, "%.6f %.6f %.6f\n", cloud->points[i].x, cloud->points[i].y, cloud->points[i].z); } fclose(file); } // ================== 哈希表加速 ================== SpatialHash create_hash(float cell_size, int grid_size) { SpatialHash hash; hash.cell_size = cell_size; hash.grid_size = grid_size; hash.buckets = malloc(grid_size * grid_size * grid_size * sizeof(HashEntry*)); memset(hash.buckets, 0, grid_size * grid_size * grid_size * sizeof(HashEntry*)); return hash; } void free_hash(SpatialHash* hash) { for (int i = 0; i < hash->grid_size * hash->grid_size * hash->grid_size; i++) { HashEntry* entry = hash->buckets[i]; while (entry) { HashEntry* next = entry->next; free(entry); entry = next; } } free(hash->buckets); } int get_hash_index(SpatialHash* hash, Point3D* p) { int x = (int)(p->x / hash->cell_size); int y = (int)(p->y / hash->cell_size); int z = (int)(p->z / hash->cell_size); // 防止越界 x = (x % hash->grid_size + hash->grid_size) % hash->grid_size; y = (y % hash->grid_size + hash->grid_size) % hash->grid_size; z = (z % hash->grid_size + hash->grid_size) % hash->grid_size; return (x * hash->grid_size * hash->grid_size) + (y * hash->grid_size) + z; } void add_to_hash(SpatialHash* hash, Point3D* p, int idx) { int index = get_hash_index(hash, p); HashEntry* new_entry = malloc(sizeof(HashEntry)); new_entry->point_index = idx; new_entry->next = hash->buckets[index]; hash->buckets[index] = new_entry; } // ================== 滤波算法 ================== int compare_x(const void* a, const void* b) { Point3D* p1 = (Point3D*)a; Point3D* p2 = (Point3D*)b; return (p1->x > p2->x) - (p1->x < p2->x); } int compare_y(const void* a, const void* b) { Point3D* p1 = (Point3D*)a; Point3D* p2 = (Point3D*)b; return (p1->y > p2->y) - (p1->y < p2->y); } int compare_z(const void* a, const void* b) { Point3D* p1 = (Point3D*)a; Point3D* p2 = (Point3D*)b; return (p1->z > p2->z) - (p1->z < p2->z); } void median_filter(PointCloud* cloud, float radius) { Point3D* temp = malloc(cloud->size * sizeof(Point3D)); memcpy(temp, cloud->points, cloud->size * sizeof(Point3D)); // 创建哈希表加速邻域搜索 SpatialHash hash = create_hash(radius * 2, 50); for (int i = 0; i < cloud->size; i++) { add_to_hash(&hash, &cloud->points[i], i); } for (int i = 0; i < cloud->size; i++) { Point3D* current = &cloud->points[i]; Point3D* neighbors = malloc(cloud->size * sizeof(Point3D)); int count = 0; // 获取哈希表中的邻域点 int center_idx = get_hash_index(&hash, current); int grid_size = hash.grid_size; // 检查3x3x3邻域网格 for (int dx = -1; dx <= 1; dx++) { for (int dy = -1; dy <= 1; dy++) { for (int dz = -1; dz <= 1; dz++) { int neighbor_idx = center_idx + dx * grid_size * grid_size + dy * grid_size + dz; if (neighbor_idx < 0 || neighbor_idx >= grid_size * grid_size * grid_size) continue; HashEntry* entry = hash.buckets[neighbor_idx]; while (entry) { Point3D* p = &temp[entry->point_index]; float dist = sqrt(pow(p->x - current->x, 2) + pow(p->y - current->y, 2) + pow(p->z - current->z, 2)); if (dist <= radius) { neighbors[count++] = *p; } entry = entry->next; } } } } // 计算中值 if (count > 0) { // 分别对x,y,z坐标排序取中值 qsort(neighbors, count, sizeof(Point3D), compare_x); current->x = neighbors[count/2].x; qsort(neighbors, count, sizeof(Point3D), compare_y); current->y = neighbors[count/2].y; qsort(neighbors, count, sizeof(Point3D), compare_z); current->z = neighbors[count/2].z; } free(neighbors); } free(temp); free_hash(&hash); } void radius_filter(PointCloud* cloud, float radius, int min_neighbors) { PointCloud result = {NULL, 0, cloud->capacity}; result.points = malloc(result.capacity * sizeof(Point3D)); // 创建哈希表加速 SpatialHash hash = create_hash(radius, 50); for (int i = 0; i < cloud->size; i++) { add_to_hash(&hash, &cloud->points[i], i); } for (int i = 0; i < cloud->size; i++) { Point3D current = cloud->points[i]; int neighbor_count = 0; // 获取哈希表中的邻域点 int center_idx = get_hash_index(&hash, &current); int grid_size = hash.grid_size; // 检查3x3x3邻域网格 for (int dx = -1; dx <= 1; dx++) { for (int dy = -1; dy <= 1; dy++) { for (int dz = -1; dz <= 1; dz++) { int neighbor_idx = center_idx + dx * grid_size * grid_size + dy * grid_size + dz; if (neighbor_idx < 0 || neighbor_idx >= grid_size * grid_size * grid_size) continue; HashEntry* entry = hash.buckets[neighbor_idx]; while (entry && neighbor_count < min_neighbors) { Point3D p = cloud->points[entry->point_index]; float dist = sqrt(pow(p.x - current.x, 2) + pow(p.y - current.y, 2) + pow(p.z - current.z, 2)); if (dist <= radius && dist > 0.0001f) { neighbor_count++; } entry = entry->next; } } } } // 保留满足条件的点 if (neighbor_count >= min_neighbors) { result.points[result.size++] = current; } } // 更新点云 free(cloud->points); cloud->points = result.points; cloud->size = result.size; cloud->capacity = result.capacity; free_hash(&hash); } int main() { // 1. 读取点云 PointCloud cloud = read_pcd("input.pcd"); printf("原始点数: %d\n", cloud.size); // 2. 中值滤波 clock_t start = clock(); median_filter(&cloud, 0.1f); // 邻域半径0.1米 printf("中值滤波耗时: %.2fms\n", (double)(clock()-start)*1000/CLOCKS_PER_SEC); // 3. 半径滤波 start = clock(); radius_filter(&cloud, 0.15f, 5); // 半径0.15米，最小邻域点数5 printf("半径滤波耗时: %.2fms\n", (double)(clock()-start)*1000/CLOCKS_PER_SEC); printf("滤波后点数: %d\n", cloud.size); // 4. 保存结果 write_pcd("output_filtered.pcd", &cloud); // 清理内存 free(cloud.points); return 0; } ``` ### 关键实现说明： 1. **PCD文件处理**： - 支持ASCII格式PCD文件读写 - 自动解析文件头中的点数信息 - 保存时生成符合标准的PCD文件头 2. **哈希表加速**： - 使用三维空间哈希表将点分配到网格桶中 - 邻域搜索时只需检查3×3×3相邻网格 - 哈希函数：$H(x,y,z) = \left(\left\lfloor\frac{x}{s}\right\rfloor \mod N\right) \times N^2 + \left(\left\lfloor\frac{y}{s}\right\rfloor \mod N\right) \times N + \left(\left\lfloor\frac{z}{s}\right\rfloor \mod N\right)$ - 其中 $s$ 为网格大小，$N$ 为网格分辨率 3. **中值滤波**： - 对每个点搜索半径内的邻域点 - 分别对x/y/z坐标排序取中值 - 时间复杂度：$O(n \cdot k \log k)$，$k$ 为平均邻域点数 4. **半径滤波**： - 移除邻域点数小于阈值的离群点 - 使用哈希表加速邻域计数 - 时间复杂度：$O(n \cdot m)$，$m$ 为邻域网格内点数 ### 使用说明： 1. 将输入点云保存为`input.pcd` 2. 编译执行程序：`gcc -o pointcloud_filter pointcloud_filter.c -lm` 3. 结果输出到`output_filtered.pcd` 4. 参数调整： - `median_filter()`的半径参数控制平滑程度 - `radius_filter()`的半径和最小邻域数控制离群点去除强度 ### 性能优化： 1. 哈希表网格大小应略大于滤波半径 2. 网格分辨率根据点云空间分布调整 3. 实际应用中可增加并行计算优化 > 此实现完整实现了点云处理的核心功能，哈希表加速使半径滤波的复杂度从$O(n^2)$降至$O(n \cdot m)$，其中$m$是邻域网格内的平均点数[^1][^2]。