高精度并行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);
        
    };


    // 输入原始二维点和圆模型，计算拟合圆的RMSE
    PCL2D_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 = ATb
    Eigen::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);