高精度并行2D圆弧拟合（C++)

依赖库

Eigen3 + GLM + Ceres-2.1.0 + glog-0.6.0 + gflag-2.2.2

基本思路

Step 1： RANSAC找到圆弧，保留inliers点；

Step 2：使用ceres非线性优化的方法，拟合inliers点，得到圆心和半径；

-------------------------------------------------

PCL拟合3D圆弧的代码参见    PCL拟合空间3D圆周 fit3DCircle

PCL库拟合圆弧的问题：PCL中坐标值默认使用float类型，在某些高精度场景中不适用。

代码

pcl2d_circle.h

// 2D圆类
#include<common.h>
#include<Eigen/Dense>
#include<mutex>namespace pcl2d
{PCL2D_API class Circle2D {public:Point2d center = Point2d(0.0);double radius = 0.0;Circle2D(){center = Point2d(0.0);radius = 0.0;}Circle2D(const Point2d& c, double r) : center(c), radius(r) {}// 计算点到平面的距离double distanceTo(const Point2d& p) const {return std::abs(glm::distance(p, center) - radius);}friend std::ostream& operator<<(std::ostream& os, const Circle2D& circle) {os << "Circle2D(center: " << circle.center.x << ", " << circle.center.y<< ", radius: " << circle.radius << ")";return os;}};struct Circle2DModel{//double a, b, c, d;Circle2D param;std::vector<Point2d> inliers;// 从三个点计算平面方程bool computeFrom3Points(const Point2d& p1, const Point2d& p2, const Point2d& p3);Circle2D fitCircleAlgebraic(const std::vector<Point2d>& points);// 使用最小二乘法从内点拟合平面double refineWithLeastSquares();// 计算点到2D圆的距离double distanceTo(const Point2d& p) const {return param.distanceTo(p);}// 评估模型，收集内点int evaluate(const std::vector<Point2d>& points, double distanceThreshold);};// 输入原始二维点和圆模型，计算拟合圆的RMSEPCL2D_API double calcRMSE(std::vector<Point2d>& points, Circle2D circle);// 并行RANSAC平面拟合// inlierRatioThresh=0.9 表示内点占比超过90%，可以提前终止迭代PCL2D_API Circle2DModel parallelRansacFitCircle2D(const std::vector<Point2d>& points,int maxIterations = 1000,double distanceThreshold = 0.01,int minInliers = 0,int numThreads = 4,double inlierRatioThresh = 0.8);// 生成带噪声的圆弧PCL2D_API std::vector<Point2d> generateNoisyArc2D(Point2d center,double radius,double startAngle, double endAngle,int numPoints,double noiseLevel);}

pcl2d_circle.cpp

#include"pcl2d_circle.h"
#include<thread>
#include<random>using namespace pcl2d;double pcl2d::calcRMSE(std::vector<Point2d>& points, Circle2D circle)
{double sum = 0., dx, dy;int n = points.size();for (int i = 0; i < n; i++){dx = points[i].x - circle.center.x;dy = points[i].y - circle.center.y;sum += SQR(sqrt(dx * dx + dy * dy) - circle.radius);}return sqrt(sum / (double)n);
}// 从三个点计算平面方程
bool Circle2DModel::computeFrom3Points(const Point2d& p1, const Point2d& p2, const Point2d& p3) {double A = p2.x - p1.x;double B = p2.y - p1.y;double C = p3.x - p1.x;double D = p3.y - p1.y;double E = A * (p1.x + p2.x) + B * (p1.y + p2.y);double F = C * (p1.x + p3.x) + D * (p1.y + p3.y);double G = 2.0 * (A * (p3.y - p2.y) - B * (p3.x - p2.x));if (G == 0) { // 如果三点共线，则无法形成圆return false;}param.center.x = (D * E - B * F) / G;param.center.y = (A * F - C * E) / G;param.radius = std::sqrt(std::pow(param.center.x - p1.x, 2) +std::pow(param.center.y - p1.y, 2));return true;
}Circle2D Circle2DModel::fitCircleAlgebraic(const std::vector<Point2d>& points) 
{int n = points.size();if (n < 3) throw std::runtime_error("至少需要3个点来拟合圆");Eigen::MatrixXd A(n, 3);Eigen::VectorXd b(n);for (int i = 0; i < n; ++i) {double x = points[i].x, y = points[i].y;A(i, 0) = x;A(i, 1) = y;A(i, 2) = 1.0;b(i) = -(x * x + y * y);}// 解线性方程组 ATAx = ATbEigen::Vector3d solution = (A.transpose() * A).ldlt().solve(A.transpose() * b);double a = solution(0);double b_val = solution(1);double c = solution(2);Circle2D circle;circle.center.x = -a / 2.0;circle.center.y = -b_val / 2.0;circle.radius = std::sqrt(a * a + b_val * b_val - 4.0 * c) / 2.0;return circle;
}int Circle2DModel::evaluate(const std::vector<Point2d>& points, double distanceThreshold)
{inliers.clear();for (const auto& p : points) {if (distanceTo(p) < distanceThreshold) {inliers.push_back(p);}}return inliers.size();
}#include<ceres/ceres.h>struct CircleCostFunctor {CircleCostFunctor(double x, double y) : x_(x), y_(y) {}template <typename T>bool operator()(const T* const center, const T* const radius, T* residual) const {residual[0] = ceres::sqrt(ceres::pow(x_ - center[0], 2) + ceres::pow(y_ - center[1], 2)) - radius[0];return true;}
private:double x_, y_;
};bool refineCircleWithCeres(const std::vector<Point2d>& points,Point2d& center, double& radius) {ceres::Problem problem;double center_ceres[2] = { center.x, center.y };double radius_ceres = radius;for (const auto& p : points) {ceres::CostFunction* cost_function =new ceres::AutoDiffCostFunction<CircleCostFunctor, 1, 2, 1>(new CircleCostFunctor(p.x, p.y));problem.AddResidualBlock(cost_function, nullptr, center_ceres, &radius_ceres);}ceres::Solver::Options options;options.minimizer_progress_to_stdout = false;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);center.x = center_ceres[0];center.y = center_ceres[1];radius = radius_ceres;return summary.IsSolutionUsable();
}// 使用最小二乘法从内点拟合平面
double Circle2DModel::refineWithLeastSquares()
{int N = inliers.size();if (N < 3) {std::cerr << "At least 3 points required for circle fitting" << std::endl;return 0.0;}refineCircleWithCeres(inliers, param.center, param.radius);double err = 0.0;double e;double r2 = param.radius * param.radius;for (int pId = 0; pId < N; ++pId){auto v = inliers[pId] - param.center;e = glm::dot(v, v) - r2;if (e > err) {err = e;}}return err;
}// 并行RANSAC工作函数
void ransacWorkerFitCircle2D(const std::vector<Point2d>& points,int maxIterations,double distanceThreshold,int minInliers,std::atomic<int>& bestInliers,Circle2DModel& bestModel,std::mutex& modelMutex,std::atomic<bool>& stopFlag,double inlierRatioThresh,int threadId)
{std::random_device rd;std::mt19937 gen(rd() + threadId); // 每个线程有不同的种子std::uniform_int_distribution<> dis(0, points.size() - 1);Circle2DModel localBestModel;int localBestInliers = -1;for (int i = 0; i < maxIterations && !stopFlag; ++i) {// 随机选择3个不重复的点int idx1 = dis(gen);int idx2, idx3;do { idx2 = dis(gen); } while (idx2 == idx1);do { idx3 = dis(gen); } while (idx3 == idx1 || idx3 == idx2);// 计算2D圆模型Circle2DModel model;if (!model.computeFrom3Points(points[idx1], points[idx2], points[idx3]))continue;// 评估模型int inliers = model.evaluate(points, distanceThreshold);// 更新本地最佳模型if (inliers > localBestInliers && inliers >= minInliers) {localBestInliers = inliers;localBestModel = model;// 检查是否需要更新全局最佳模型if (localBestInliers > bestInliers) {std::lock_guard<std::mutex> lock(modelMutex);if (localBestInliers > bestInliers) {bestInliers = localBestInliers;bestModel = localBestModel;// 动态调整: 如果找到足够好的模型，可以提前停止double inlierRatio = static_cast<double>(bestInliers) / points.size();if (inlierRatio > inlierRatioThresh) { // 如果内点比例超过80%，提前停止stopFlag = true;}}}}}
}// 并行RANSAC平面拟合
// inlierRatioThresh=0.9 表示内点占比超过90%，可以提前终止迭代
Circle2DModel pcl2d::parallelRansacFitCircle2D(const std::vector<Point2d>& points,int maxIterations/* = 1000*/,double distanceThreshold/* = 0.01*/,int minInliers/* = 0*/,int numThreads/* = 4*/,double inlierRatioThresh/* = 0.8*/)
{if (points.size() < 3) {throw std::runtime_error("At least 3 points are required to fit a plane");}if (minInliers <= 0) {minInliers = static_cast<int>(points.size() * 0.3); // 默认最小内点数为30%}std::atomic<int> bestInliers(-1);Circle2DModel bestModel;std::mutex modelMutex;std::atomic<bool> stopFlag(false);// 计算每个线程的迭代次数int iterationsPerThread = maxIterations / numThreads;std::vector<std::thread> threads;for (int i = 0; i < numThreads; ++i) {threads.emplace_back(ransacWorkerFitCircle2D,std::ref(points),iterationsPerThread,distanceThreshold,minInliers,std::ref(bestInliers),std::ref(bestModel),std::ref(modelMutex),std::ref(stopFlag),inlierRatioThresh,i);}// 等待所有线程完成for (auto& t : threads) {t.join();}// 如果没有找到足够内点的模型，返回第一个模型if (bestInliers == -1) {bestModel.computeFrom3Points(points[0], points[1], points[2]);}// 使用最小二乘法优化最佳模型bestModel.evaluate(points, distanceThreshold); // 找出所有内点bestModel.refineWithLeastSquares();return bestModel;
}std::vector<Point2d> pcl2d::generateNoisyArc2D(Point2d center,double radius,double startAngle, double endAngle,int numPoints,double noiseLevel)
{std::vector<Point2d> points;points.reserve(numPoints);startAngle = startAngle * glm::pi<double>() / 180.0; // 转换为弧度endAngle = endAngle * glm::pi<double>() / 180.0; // 转换为弧度std::random_device rd;std::mt19937 gen(rd());std::uniform_real_distribution<> angleDist(startAngle, endAngle);std::normal_distribution<> noiseDist(0.0, noiseLevel);for (int i = 0; i < numPoints; ++i) {double angle = angleDist(gen);double x = center.x + radius * std::cos(angle)/* + noiseDist(gen)*/;double y = center.y + radius * std::sin(angle);points.push_back({ x, y });}for (int i = 0; i < points.size(); i++){points[i] += noiseDist(gen) * glm::normalize(points[i] - center);}return points;
}

main.cpp

	auto pts = pcl2d::generateNoisyArc2D(pcl2d::Point2d(100, 77), 2.0, 20, 120, 200, 0.1);auto model = pcl2d::parallelRansacFitCircle2D(pts, 1000, 0.2, 0, 4, 0.99);