基于tukey算法的三维点云平面拟合算法，使用c++实现

基于Tukey's median filter的三维点云平面拟合算法通常用于从噪声数据中找出平面结构，通过统计学方法剔除异常值并计算中位数方向来确定平面对应的法线。在C++中实现这个过程，你需要做以下几个步骤：

1. **预处理**：首先对点云进行滤波，比如使用中值滤波（Median Filter），由John Tukey提出，去除离群点，提高后续平面检测的精度。

#include "Eigen/Dense"
// ...

std::vector<Vector3f> cloud_filtered;
for (size_t i = 1; i < cloud.size() - 1; ++i) {
    Vector3f point_median(cloud[i-1], cloud[i], cloud[i+1]);
    if (is_inlier(point_median)) {
        cloud_filtered.push_back(point_median);
    }
}

2. **构建邻域**：在处理后的点云中找到每个点的邻居，这通常是通过K近邻(KNN)或窗口方法来完成。

std::vector<std::vector<size_t>> neighbors;
for (const auto& point : filtered_cloud) {
    // 使用KNN算法找到点的邻居
    std::vector<size_t> nearby_indices = KNN(point, k); 
    neighbors.push_back(nearby_indices);
}

3. **计算平面**：利用邻域内的点计算平面方程。一种常见方法是计算法向量和平面上任意一点，然后求解。例如，可以取中位数点作为基准点。

Eigen::MatrixXf points_matrix(filtered_cloud.size(), 3);
std::transform(neighbors.begin(), neighbors.end(), 
               begin(filtered_cloud), 
               begin(points_matrix),
               [](const std::vector<size_t>& n, const Vector3f& p) { return Eigen::Vector3f(p.x(), p.y(), p.z()); });

Eigen::Vector4f plane_coefficients = fit_plane_to_neighbors(points_matrix);

4. **平面表示**：最后，你可以将平面表示为 `ax + by + cz + d = 0` 的形式，其中 `(a, b, c)` 是法向量，`d` 是截距。

float a = plane_coefficients(0);
float b = plane_coefficients(1);
float c = plane_coefficients(2);
float d = -plane_coefficients.dot(Eigen::Vector3f::Zero());