读取和保存pcd文件，网格搜索加速完成点云中值滤波，c语言代码示例，不依赖pcl库

<think>我们使用C语言实现点云的中值滤波，并且不依赖PCL库。主要步骤包括： 1. 读取PCD文件（通常为ASCII格式） 2. 将点云数据存储到三维数组中（这里我们使用网格搜索加速，需要将点云分配到三维网格中） 3. 对每个点，在其邻域（网格中的相邻网格）内收集点，然后计算邻域内点的中值作为该点的滤波结果 4. 保存滤波后的点云到PCD文件 注意：由于点云可能很大，我们使用网格化（空间划分）来加速邻域搜索。我们将空间划分为小立方体（体素），然后每个点根据其坐标分配到对应的体素中。这样，寻找一个点的邻域点时，只需要查找相邻的体素即可。 我们假设PCD文件是ASCII格式，格式如下： ``` # .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 ... ``` 中值滤波的邻域大小可以定义为以当前点为中心的一个立方体（例如，边长为0.1米）。我们使用网格（体素）划分，网格的大小可以与邻域大小相同，但为了加速，我们通常将网格大小设置为邻域大小的一半左右，这样邻域搜索只需查找相邻的2x2x2网格（如果网格大小等于邻域大小则只需查找当前网格，但为了覆盖整个邻域，网格大小应小于等于邻域大小的一半）。这里我们设定网格大小为邻域大小的一半，这样邻域搜索需要检查3x3x3的网格（因为一个点可能位于网格边界，所以向每个方向扩展一个网格）。 步骤： 1. 读取PCD文件头，获取点云数量，并读取所有点。 2. 确定点云的边界（最小和最大x,y,z）以确定网格划分。 3. 根据网格大小（voxel_size）计算每个点所在的体素坐标。 4. 将点索引分配到体素中（每个体素存储该体素内的点的索引）。 5. 对于每个点，计算其所在体素以及相邻的26个体素（共27个，包括自身），然后收集这些体素内的所有点作为邻域点（注意：邻域点必须在以当前点为中心、指定半径的立方体或球体内，这里我们使用立方体邻域，因为网格是立方体，但为了精确，我们也可以检查距离，但为了加速，我们只检查相邻网格内的点，然后进一步筛选在立方体邻域内的点）。 6. 对邻域点按某一坐标（比如z坐标）排序，取中值作为当前点滤波后的z值（这里我们仅对z坐标进行中值滤波，也可以对每个坐标分别滤波，但通常中值滤波用于去除噪声，可以分别对x,y,z进行，或者对每个点用一个标量（如深度）进行滤波。这里我们假设对每个坐标分别进行中值滤波，即分别取邻域点的x,y,z的中值作为当前点的滤波值）。 注意：中值滤波可能会改变点的位置，因此通常用于去除离群点或噪声，但会改变点云形状。 由于点云可能很大，我们需要注意内存和计算效率。 我们将编写以下函数： - 读取PCD文件：read_pcd_file - 保存PCD文件：write_pcd_file - 构建网格索引：build_voxel_grid - 中值滤波：median_filter 由于邻域大小和网格大小需要事先设定，我们假设邻域立方体边长为`neighborhood_size`，网格大小为`voxel_size`（这里我们取`voxel_size = neighborhood_size / 2`）。 注意：网格大小不能为0，且邻域大小应该大于点云中相邻点的平均距离。 代码结构： 1. 定义点结构体（x,y,z） 2. 读取PCD文件，将点存储到数组中 3. 构建网格索引（使用哈希表，键为体素坐标（i,j,k）） 4. 对每个点，根据其坐标找到所在体素，然后收集相邻27个体素（3x3x3）内的点，再从中筛选出在邻域立方体内的点（立方体边长为neighborhood_size，以当前点为中心） 5. 对邻域点分别按x,y,z排序，取中值作为当前点滤波后的坐标 6. 保存滤波后的点云到PCD文件 注意：中值滤波可能会使点云变得稀疏，因为如果邻域内没有点，则无法计算。这里我们只处理那些邻域内点数大于0的点，否则保留原值（或者跳过？但这里我们保留原值）。 由于中值滤波计算量较大，我们通过网格搜索来加速邻域点的查找。 下面是一个简化的代码示例（注意：为了简化，我们假设点云数据只有x,y,z，且PCD文件是ASCII格式，没有颜色等信息）：</think>以下是一个使用C语言实现的点云中值滤波代码示例，不依赖PCL库。该代码实现了PCD文件读取/保存、网格搜索加速和中值滤波功能： ```c #include <stdio.h> #include <stdlib.h> #include <string.h> #include <math.h> // 点云结构体 typedef struct { float x, y, z; } Point3D; // 网格单元结构体 typedef struct { int count; int capacity; Point3D* points; } Voxel; // 网格结构体 typedef struct { float min_x, min_y, min_z; float max_x, max_y, max_z; float voxel_size; int size_x, size_y, size_z; Voxel**** voxels; // 四维指针: [x][y][z][voxel] } VoxelGrid; // 读取PCD文件 Point3D* read_pcd(const char* filename, int* point_count) { FILE* file = fopen(filename, "r"); if (!file) { perror("文件打开失败"); return NULL; } char line[256]; int points_found = 0; *point_count = 0; // 读取文件头 while (fgets(line, sizeof(line), file)) { if (strncmp(line, "POINTS", 6) == 0) { sscanf(line, "POINTS %d", point_count); } if (strncmp(line, "DATA ascii", 10) == 0) { points_found = 1; break; } } if (*point_count == 0 || !points_found) { fclose(file); return NULL; } // 分配内存 Point3D* cloud = (Point3D*)malloc(*point_count * sizeof(Point3D)); if (!cloud) { fclose(file); return NULL; } // 读取点数据 for (int i = 0; i < *point_count; i++) { if (fscanf(file, "%f %f %f", &cloud[i].x, &cloud[i].y, &cloud[i].z) != 3) { free(cloud); fclose(file); return NULL; } } fclose(file); return cloud; } // 保存PCD文件 void save_pcd(const char* filename, Point3D* cloud, int point_count) { FILE* file = fopen(filename, "w"); if (!file) { perror("文件创建失败"); return; } // 写入PCD头 fprintf(file, "# .PCD v0.7 - Point Cloud Data file format\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", point_count); fprintf(file, "HEIGHT 1\n"); fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n"); fprintf(file, "POINTS %d\n", point_count); fprintf(file, "DATA ascii\n"); // 写入点数据 for (int i = 0; i < point_count; i++) { fprintf(file, "%.6f %.6f %.6f\n", cloud[i].x, cloud[i].y, cloud[i].z); } fclose(file); } // 初始化网格 VoxelGrid* init_voxel_grid(Point3D* cloud, int point_count, float voxel_size) { VoxelGrid* grid = (VoxelGrid*)malloc(sizeof(VoxelGrid)); if (!grid) return NULL; // 计算点云边界 grid->min_x = grid->max_x = cloud[0].x; grid->min_y = grid->max_y = cloud[0].y; grid->min_z = grid->max_z = cloud[0].z; for (int i = 1; i < point_count; i++) { if (cloud[i].x < grid->min_x) grid->min_x = cloud[i].x; if (cloud[i].x > grid->max_x) grid->max_x = cloud[i].x; if (cloud[i].y < grid->min_y) grid->min_y = cloud[i].y; if (cloud[i].y > grid->max_y) grid->max_y = cloud[i].y; if (cloud[i].z < grid->min_z) grid->min_z = cloud[i].z; if (cloud[i].z > grid->max_z) grid->max_z = cloud[i].z; } // 计算网格尺寸 grid->voxel_size = voxel_size; grid->size_x = (int)ceil((grid->max_x - grid->min_x) / voxel_size) + 1; grid->size_y = (int)ceil((grid->max_y - grid->min_y) / voxel_size) + 1; grid->size_z = (int)ceil((grid->max_z - grid->min_z) / voxel_size) + 1; // 分配网格内存 grid->voxels = (Voxel****)malloc(grid->size_x * sizeof(Voxel***)); for (int x = 0; x < grid->size_x; x++) { grid->voxels[x] = (Voxel***)malloc(grid->size_y * sizeof(Voxel**)); for (int y = 0; y < grid->size_y; y++) { grid->voxels[x][y] = (Voxel**)malloc(grid->size_z * sizeof(Voxel*)); for (int z = 0; z < grid->size_z; z++) { grid->voxels[x][y][z] = (Voxel*)malloc(sizeof(Voxel)); grid->voxels[x][y][z]->count = 0; grid->voxels[x][y][z]->capacity = 10; grid->voxels[x][y][z]->points = (Point3D*)malloc(10 * sizeof(Point3D)); } } } // 将点分配到网格 for (int i = 0; i < point_count; i++) { int x = (int)((cloud[i].x - grid->min_x) / voxel_size); int y = (int)((cloud[i].y - grid->min_y) / voxel_size); int z = (int)((cloud[i].z - grid->min_z) / voxel_size); if (x >= 0 && x < grid->size_x && y >= 0 && y < grid->size_y && z >= 0 && z < grid->size_z) { Voxel* voxel = grid->voxels[x][y][z]; if (voxel->count >= voxel->capacity) { voxel->capacity *= 2; voxel->points = (Point3D*)realloc(voxel->points, voxel->capacity * sizeof(Point3D)); } voxel->points[voxel->count++] = cloud[i]; } } return grid; } // 释放网格内存 void free_voxel_grid(VoxelGrid* grid) { for (int x = 0; x < grid->size_x; x++) { for (int y = 0; y < grid->size_y; y++) { for (int z = 0; z < grid->size_z; z++) { free(grid->voxels[x][y][z]->points); free(grid->voxels[x][y][z]); } free(grid->voxels[x][y]); } free(grid->voxels[x]); } free(grid->voxels); free(grid); } // 比较函数用于排序 int compare_x(const void* a, const void* b) { Point3D* p1 = (Point3D*)a; Point3D* p2 = (Point3D*)b; return (p1->x > p2->x) ? 1 : -1; } int compare_y(const void* a, const void* b) { Point3D* p1 = (Point3D*)a; Point3D* p2 = (Point3D*)b; return (p1->y > p2->y) ? 1 : -1; } int compare_z(const void* a, const void* b) { Point3D* p1 = (Point3D*)a; Point3D* p2 = (Point3D*)b; return (p1->z > p2->z) ? 1 : -1; } // 中值滤波（使用网格加速） void median_filter_with_voxel(Point3D* cloud, int point_count, VoxelGrid* grid, int search_radius) { // 临时存储邻域点 Point3D* neighborhood = (Point3D*)malloc(1000 * sizeof(Point3D)); int neighborhood_size; for (int i = 0; i < point_count; i++) { Point3D* current = &cloud[i]; neighborhood_size = 0; // 计算当前点所在网格 int cx = (int)((current->x - grid->min_x) / grid->voxel_size); int cy = (int)((current->y - grid->min_y) / grid->voxel_size); int cz = (int)((current->z - grid->min_z) / grid->voxel_size); // 在邻域网格中搜索点 for (int dx = -search_radius; dx <= search_radius; dx++) { for (int dy = -search_radius; dy <= search_radius; dy++) { for (int dz = -search_radius; dz <= search_radius; dz++) { int nx = cx + dx; int ny = cy + dy; int nz = cz + dz; if (nx >= 0 && nx < grid->size_x && ny >= 0 && ny < grid->size_y && nz >= 0 && nz < grid->size_z) { Voxel* voxel = grid->voxels[nx][ny][nz]; for (int j = 0; j < voxel->count; j++) { // 检查是否在邻域范围内 float dist_x = fabs(voxel->points[j].x - current->x); float dist_y = fabs(voxel->points[j].y - current->y); float dist_z = fabs(voxel->points[j].z - current->z); if (dist_x <= grid->voxel_size * search_radius && dist_y <= grid->voxel_size * search_radius && dist_z <= grid->voxel_size * search_radius) { neighborhood[neighborhood_size++] = voxel->points[j]; if (neighborhood_size >= 1000) goto median_calc; } } } } } } median_calc: if (neighborhood_size > 0) { // 分别对x,y,z排序并取中值 qsort(neighborhood, neighborhood_size, sizeof(Point3D), compare_x); current->x = neighborhood[neighborhood_size/2].x; qsort(neighborhood, neighborhood_size, sizeof(Point3D), compare_y); current->y = neighborhood[neighborhood_size/2].y; qsort(neighborhood, neighborhood_size, sizeof(Point3D), compare_z); current->z = neighborhood[neighborhood_size/2].z; } } free(neighborhood); } int main() { // 参数设置 const char* input_file = "input.pcd"; const char* output_file = "filtered.pcd"; float voxel_size = 0.1f; // 网格大小 int search_radius = 1; // 邻域搜索半径（网格单位） int point_count = 0; // 1. 读取点云 Point3D* cloud = read_pcd(input_file, &point_count); if (!cloud || point_count == 0) { printf("点云读取失败\n"); return 1; } printf("成功读取点云: %d 个点\n", point_count); // 2. 构建网格加速结构 VoxelGrid* grid = init_voxel_grid(cloud, point_count, voxel_size); if (!grid) { printf("网格初始化失败\n"); free(cloud); return 1; } printf("网格尺寸: %dx%dx%d\n", grid->size_x, grid->size_y, grid->size_z); // 3. 执行中值滤波 median_filter_with_voxel(cloud, point_count, grid, search_radius); printf("中值滤波完成\n"); // 4. 保存结果 save_pcd(output_file, cloud, point_count); printf("结果已保存到: %s\n", output_file); // 5. 清理内存 free_voxel_grid(grid); free(cloud); return 0; } ``` ### 代码说明 1. **PCD文件处理**： - `read_pcd()`：解析PCD文件头，读取点云数据 - `save_pcd()`：生成标准PCD格式文件并保存 2. **网格加速结构**： - `VoxelGrid`：三维网格数据结构 - `init_voxel_grid()`：根据点云空间分布创建网格 - `free_voxel_grid()`：释放网格内存 3. **中值滤波核心**： - `median_filter_with_voxel()`：使用网格加速邻域搜索 - 对每个点搜索周围$3×3×3$网格区域 - 分别对$x,y,z$坐标排序取中值 4. **性能优化**： - 网格搜索将时间复杂度从$O(n^2)$降为$O(n+k)$ - 动态内存管理避免过度分配 - 局部排序替代全局排序 ### 使用说明 1. 参数调整： ```c float voxel_size = 0.1f; // 网格大小（根据点云密度调整） int search_radius = 1; // 邻域搜索半径（通常1-3） ``` 2. 编译运行： ```bash gcc -O3 pointcloud_filter.c -o filter -lm ./filter ``` 3. 输入输出： - 输入文件：`input.pcd` - 输出文件：`filtered.pcd` ### 算法复杂度 - **时间复杂度**：$O(n \times k)$，其中$n$为点数，$k$为邻域平均点数 - **空间复杂度**：$O(n + g^3)$，$g$为网格维度 此实现通过空间分割显著提升邻域搜索效率，特别适合大规模点云处理[^1]。