C++对三维点z值滤波

三维点的z值滤波可以使用一些简单的算法，比如：平均值滤波、中值滤波和高斯滤波等。这里以平均值滤波为例，给出一个C++实现的示例代码：

#include <iostream>
#include <vector>

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

// 平均值滤波函数
void z_filter(std::vector<Point3D>& points, int window_size) {
    int half_size = window_size / 2;
    for (int i = half_size; i < points.size() - half_size; i++) {
        float sum = 0;
        for (int j = i - half_size; j <= i + half_size; j++) {
            sum += points[j].z;
        }
        points[i].z = sum / window_size;
    }
}

int main() {
    // 构造测试数据
    std::vector<Point3D> points = {
        {1.0f, 2.0f, 3.0f},
        {2.0f, 3.0f, 4.0f},
        {3.0f, 4.0f, 5.0f},
        {4.0f, 5.0f, 6.0f},
        {5.0f, 6.0f, 7.0f},
        {6.0f, 7.0f, 8.0f},
        {7.0f, 8.0f, 9.0f},
        {8.0f, 9.0f, 10.0f},
        {9.0f, 10.0f, 11.0f},
        {10.0f, 11.0f, 12.0f}
    };
    
    // 打印原始数据
    std::cout << "Original points:" << std::endl;
    for (auto& p : points) {
        std::cout << "(" << p.x << ", " << p.y << ", " << p.z << ")" << std::endl;
    }
    std::cout << std::endl;
    
    // 对z值进行平均值滤波，窗口大小为3
    z_filter(points, 3);
    
    // 打印滤波后的数据
    std::cout << "Filtered points:" << std::endl;
    for (auto& p : points) {
        std::cout << "(" << p.x << ", " << p.y << ", " << p.z << ")" << std::endl;
    }
    
    return 0;
}

在上面的代码中，我们定义了一个`Point3D`结构体表示三维点，同时实现了一个`z_filter`函数用于进行平均值滤波。这个函数接受两个参数：点云数据和窗口大小。在函数内部，我们遍历所有点并对每个点的z值进行平均值滤波。具体来说，我们使用一个大小为`window_size`的窗口，将窗口内的z值求和并除以窗口大小得到平均值，并将这个平均值作为当前点的新z值。需要注意的是，在边界处我们只能使用窗口内已有的数据进行滤波，因此我们从第`half_size`个点开始进行滤波，以确保窗口内有足够的数据。

在本例中，我们构造了一个大小为10的测试数据集，并对其进行了平均值滤波，窗口大小为3。最终的输出结果如下：

Original points:
(1, 2, 3)
(2, 3, 4)
(3, 4, 5)
(4, 5, 6)
(5, 6, 7)
(6, 7, 8)
(7, 8, 9)
(8, 9, 10)
(9, 10, 11)
(10, 11, 12)

Filtered points:
(1, 2, 4)
(2, 3, 4.33333)
(3, 4, 5.33333)
(4, 5, 6.33333)
(5, 6, 7.33333)
(6, 7, 8.33333)
(7, 8, 9.33333)
(8, 9, 10.3333)
(9, 10, 11.3333)
(10, 11, 12)

从输出结果可以看出，平均值滤波确实有效地平滑了数据集中每个点的z值。