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 forfor (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 + bdouble 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 = ax2 + bx + cFUNC_TYPE_FY // x = ay2 + 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 = bfor (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 = ax2 + bx + cstatic 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 = ay2 + by + cstatic 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);}
};可以優化的方向
擬合時考慮距離相關權重，考慮曲率值相關的權重。
)
