当前位置：首页 > news >正文

2D曲线点云平滑去噪

news 来源：原创 2025/6/22 14:17:34

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

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

分两种情况进行讨论：

1）曲线前进方向与Y轴偏离较远；采用 $y=ax^{2}+bx+c$

2）曲线前进方向与Y轴较近；采用 $x=ay^{2}+by+c$

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

代码如下：

#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 = ax² + bx + cFUNC_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 = 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 = ax² + 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 = ay² + 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);}
};

可以优化的方向

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