使用C++写一个点云滤波误差统计代码

以下是使用C编写的点云滤波误差统计代码示例：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define MAX_POINTS 1000 // 点云最大点数

typedef struct {
    float x;
    float y;
    float z;
} Point3D; // 三维点结构体

Point3D points[MAX_POINTS]; // 点云数组
int num_points; // 实际点云点数

float filter_distance = 0.1; // 滤波距离阈值

int main() {
    // 读入点云数据
    FILE *fp = fopen("point_cloud.txt", "r");
    if (fp == NULL) {
        printf("文件打开失败\n");
        return -1;
    }
    while (fscanf(fp, "%f %f %f", &points[num_points].x, &points[num_points].y, &points[num_points].z) != EOF) {
        num_points++;
    }
    fclose(fp);

    // 点云滤波
    int num_filtered_points = 0; // 滤波后的点数
    Point3D filtered_points[MAX_POINTS]; // 滤波后的点云数组
    for (int i = 0; i < num_points; i++) {
        int num_nearby_points = 0; // 邻近点数
        float sum_x = 0.0, sum_y = 0.0, sum_z = 0.0; // 邻近点坐标和
        for (int j = 0; j < num_points; j++) {
            if (i != j && sqrt(pow(points[i].x - points[j].x, 2) + pow(points[i].y - points[j].y, 2) + pow(points[i].z - points[j].z, 2)) <= filter_distance) {
                num_nearby_points++;
                sum_x += points[j].x;
                sum_y += points[j].y;
                sum_z += points[j].z;
            }
        }
        if (num_nearby_points > 0) {
            filtered_points[num_filtered_points].x = sum_x / num_nearby_points;
            filtered_points[num_filtered_points].y = sum_y / num_nearby_points;
            filtered_points[num_filtered_points].z = sum_z / num_nearby_points;
            num_filtered_points++;
        }
    }

    // 计算误差
    float sum_error = 0.0;
    for (int i = 0; i < num_points; i++) {
        float min_distance = INFINITY;
        for (int j = 0; j < num_filtered_points; j++) {
            float distance = sqrt(pow(points[i].x - filtered_points[j].x, 2) + pow(points[i].y - filtered_points[j].y, 2) + pow(points[i].z - filtered_points[j].z, 2));
            if (distance < min_distance) {
                min_distance = distance;
            }
        }
        sum_error += min_distance;
    }
    float avg_error = sum_error / num_points;

    // 输出结果
    printf("点云滤波后的点数：%d\n", num_filtered_points);
    printf("滤波距离阈值：%f\n", filter_distance);
    printf("平均误差：%f\n", avg_error);

    return 0;
}

该代码实现了以下功能：

1. 从文件中读入点云数据
2. 对点云进行距离滤波
3. 计算滤波后的点云与原始点云之间的误差

其中，点云数据存储在一个结构体数组中，滤波距离阈值和点云最大点数通过宏定义设置。点云滤波使用暴力搜索实现，计算误差使用最近邻搜索。最终输出滤波后的点数、滤波距离阈值和平均误差。