2D曲线点云平滑去噪

2D曲线点云，含许多噪声，采用类似移动最小二乘的方法（MLS)分段拟合抛物线并投影至抛物线，进行点云平滑去噪。

更通俗的说法是让有一定宽度的曲线点云，变成一条细曲线上的点。

分两种情况进行讨论：

1）曲线前进方向与Y轴偏离较远；采用

2）曲线前进方向与Y轴较近；采用

注意 这里并没有采取二阶多项式拟合的形式，，因为这样可能导致过拟合，使用抛物线拟合可以让数据分布在曲线两侧，更符合曲线平滑的需求。

代码如下：

#pragma once
#include"common.h"
#include"CommonFunctions.h"

#include <vector>
#include <cmath>
#include <algorithm>
#include <Eigen/Dense> // 需要Eigen库，用于矩阵运算

class PointCloudSmoother {
public:
    static void smooth(std::vector<pcl2d::Point2d>& points, double radius, int iterations) {
        if (points.empty() || iterations <= 0) return;

        std::vector<pcl2d::Point2d> smoothed_points = points;

        for (int iter = 0; iter < iterations; ++iter) {
            //#pragma omp parallel for
            for (size_t i = 0; i < points.size(); ++i) {
                // 1. 寻找邻域点
                std::vector<size_t> neighbors = findNeighbors(smoothed_points, i, radius);

                if (neighbors.size() < 5) continue; // 至少需要5个点才能拟合曲线

                // 2. 计算局部曲率
                double curvature = computeLocalCurvature(smoothed_points, neighbors);

                // 3. 曲率自适应MLS平滑
                smoothed_points[i] = mlsSmoothing(smoothed_points, i, neighbors, radius, curvature);
            }
        }

        points = smoothed_points;

        // 点云去重（基于距离阈值） 
        filterPointsByRadius(points, 0.02);
    }

    // 计算局部曲率
    static double computeLocalCurvature(const std::vector<pcl2d::Point2d>& points, const std::vector<size_t>& neighbors) {
        if (neighbors.size() < 3) return 0.0;

        // 计算质心
        pcl2d::Point2d centroid(0, 0);
        for (size_t idx : neighbors) {
            centroid.x += points[idx].x;
            centroid.y += points[idx].y;
        }
        centroid.x /= neighbors.size();
        centroid.y /= neighbors.size();

        // PCA分析
        Eigen::Matrix2d cov = Eigen::Matrix2d::Zero();
        for (size_t idx : neighbors) {
            double dx = points[idx].x - centroid.x;
            double dy = points[idx].y - centroid.y;
            cov(0, 0) += dx * dx;
            cov(0, 1) += dx * dy;
            cov(1, 0) += dy * dx;
            cov(1, 1) += dy * dy;
        }

        // 计算特征值
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver(cov);
        Eigen::Vector2d eigenvalues = solver.eigenvalues();

        // 曲率估计 = 最小特征值 / (最大特征值 + 最小特征值)
        double min_eig = std::min(eigenvalues[0], eigenvalues[1]);
        double max_eig = std::max(eigenvalues[0], eigenvalues[1]);
        return min_eig / (max_eig + min_eig + 1e-6); // 避免除以零
    }

private:
    // 寻找邻域点（实际应用中应使用空间索引加速）
    static std::vector<size_t> findNeighbors(const std::vector<pcl2d::Point2d>& points, size_t idx, double radius) {
        std::vector<size_t> neighbors;
        const pcl2d::Point2d& center = points[idx];

        for (size_t i = 0; i < points.size(); ++i) {
            double dx = points[i].x - center.x;
            double dy = points[i].y - center.y;
            double dist = std::sqrt(dx * dx + dy * dy);

            if (dist < radius) {
                neighbors.push_back(i);
            }
        }

        return neighbors;
    }

    // 移动最小二乘平滑
    static pcl2d::Point2d mlsSmoothing(const std::vector<pcl2d::Point2d>& points, size_t idx,
        const std::vector<size_t>& neighbors, double radius, double curvature) {
        const pcl2d::Point2d& center = points[idx];

        // 曲率自适应参数
        double sigma_s = radius * (1.0 - 0.5 * curvature); // 空间权重参数
        double sigma_r = 0.1 * radius * (1.0 + curvature); // 值域权重参数

         构建加权最小二乘问题
        //Eigen::MatrixXd A(neighbors.size(), 6);
        //Eigen::VectorXd b_x(neighbors.size());
        //Eigen::VectorXd b_y(neighbors.size());
        //Eigen::VectorXd weights(neighbors.size());

        std::vector<pcl2d::Point2d> neighbor_pos;
        for (size_t i = 0; i < neighbors.size(); ++i) {
            size_t n_idx = neighbors[i];
            const pcl2d::Point2d& p = points[n_idx];
            neighbor_pos.push_back(p);
        }

        double a = 0, b = 0, c = 0;
        auto type = fitParabola(neighbor_pos, a, b, c);

        return ((type == FUNC_TYPE_FX) ? projectToParabola_fx(points[idx], a, b, c) : projectToParabola_fy(points[idx], a, b, c));
    }

    // 判断点集拟合的直线是否接近 y 轴
    static bool isLineCloseToYAxis(const std::vector<pcl2d::Point2d>& points, double threshold = 10.0)
    {
        if (points.size() < 2) return false; // 至少需要2个点

        // 1. 最小二乘法拟合直线 y = kx + b
        double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0;
        for (const auto& p : points) {
            sum_x += p.x;
            sum_y += p.y;
            sum_xy += p.x * p.y;
            sum_xx += p.x * p.x;
        }

        double n = points.size();
        double k = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x);

        // 2. 判断斜率绝对值是否大于阈值
        return std::abs(k) > threshold;
    }

    enum FUNC_TYPE
    {
        FUNC_TYPE_FX, // y = ax² + bx + c
        FUNC_TYPE_FY  // x = ay² + by + c
    };

    // 拟合抛物线 
    static FUNC_TYPE fitParabola(const std::vector<pcl2d::Point2d>& points, double& a, double& b, double& c) 
    {
        const int n = points.size();
        Eigen::MatrixXd A(n, 3);
        Eigen::VectorXd b_vec(n);

        FUNC_TYPE type = isLineCloseToYAxis(points)? FUNC_TYPE_FY : FUNC_TYPE_FX;

        // 构建最小二乘问题 Ax = b
        for (int i = 0; i < n; ++i) 
        {
            double x = points[i].x;
            b_vec[i] = points[i].y;
            if(type == FUNC_TYPE_FY)
            {
                x = points[i].y;
                b_vec[i] = points[i].x;
            }
            A(i, 0) = x * x;
            A(i, 1) = x;
            A(i, 2) = 1.0;
        }

        // 求解最小二乘问题
        Eigen::Vector3d coeffs = A.colPivHouseholderQr().solve(b_vec);
        a = coeffs[0];
        b = coeffs[1];
        c = coeffs[2];

        return type;
    }

    // 计算点在抛物线上的投影 y = ax² + bx + c
    static pcl2d::Point2d projectToParabola_fx(const pcl2d::Point2d& point, double a, double b, double c) {
        double x = point.x;
        double y = a * x * x + b * x + c;
        return pcl2d::Point2d(x, y);
    }
    // 计算点在抛物线上的投影 x = ay² + by + c
    static pcl2d::Point2d projectToParabola_fy(const pcl2d::Point2d& point, double a, double b, double c) {
        double y = point.y;
        double x = a * y * y + b * y + c;
        return pcl2d::Point2d(x, y);
    }
};

可以优化的方向

拟合时考虑距离相关权重，考虑曲率值相关的权重。