圆拟合 --- 点到圆距离绝对值之和最小

https://www.researchgate.net/publication/220146330_Robust_Fitting_of_Circle_Arcs 根据这篇文章介绍的算法进行了实现，经过验证结果与论文中结果一致

AGE.h

#pragma once
#include <vector>

struct Point2d {
    double x;
    double z;
};

struct Circle2d
{
    Point2d center;
    double radius;
};

// https://www.researchgate.net/publication/220146330_Robust_Fitting_of_Circle_Arcs
bool fitCircleAGE(const std::vector<Point2d>& points, const double epsilon, double lambda,
    int iterations, Circle2d& circle);

AGE.cpp

#include <cmath>
#include <algorithm>
#include <numeric>

#include "AGE.h"

bool calcCircleCoefficients(const std::vector<Point2d>& points, Circle2d& circle)
{
    if (points.size() != 3)
        return false;

    const Point2d p1 = points[0], p2 = points[1], p3 = points[2];

    const double a = 2 * (p2.x - p1.x), b = 2 * (p2.z - p1.z);
    const double c = p2.x * p2.x + p2.z * p2.z - p1.x * p1.x - p1.z * p1.z;
    const double d = 2 * (p3.x - p2.x), e = 2 * (p3.z - p2.z);
    const double f = p3.x * p3.x + p3.z * p3.z - p2.x * p2.x - p2.z * p2.z;

    const double x = (b * f - e * c) / (b * d - e * a);
    const double z = (d * c - a * f) / (b * d - e * a);
    const double r = sqrt((x - p1.x) * (x - p1.x) + (z - p1.z) * (z - p1.z));

    circle = { {x, z}, r };
    return true;
}

double calcDistancePointToPoint(const Point2d& p1, const Point2d& p2)
{
    return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.z - p2.z, 2));
}

double calcMedian(const std::vector<double>& values)
{
    auto vec = values;
    const int size = vec.size();
    const int mid = size / 2;
    if (size & 1) {
        std::nth_element(vec.begin(), vec.begin() + mid, vec.end());
        return vec[mid];
    }
    std::nth_element(vec.begin(), vec.begin() + mid - 1, vec.end());
    return (vec[mid - 1] + *std::min_element(vec.begin() + mid, vec.end())) / 2.0;
}

// https://www.researchgate.net/publication/220146330_Robust_Fitting_of_Circle_Arcs
bool fitCircleAGE(const std::vector<Point2d>& points, const double epsilon, double lambda,
    int iterations, Circle2d& circle)
{
    if (points.size() < 3)
        return false;

    // step 1: initail center of circle.
    std::vector<Point2d> points3 = { points[0], points[1], points[2] };
    calcCircleCoefficients(points3, circle);
    lambda = 1;

    const int pointsNum = points.size();
    std::vector<double> distances(pointsNum);

    std::vector<int> I, O, C;
    I.reserve(pointsNum);
    O.reserve(pointsNum);
    C.reserve(pointsNum);

    std::vector<double> cosTheta(pointsNum), sinTheta(pointsNum);

    double error = DBL_MAX;

    while (iterations-- > 0) {
        // step 2: compute the distances between each point to the circle
        // center and the median r of these distances.
#pragma omp parallel for
        for (int i = 0; i < pointsNum; i++)
            distances[i] = calcDistancePointToPoint(circle.center, points[i]);

        circle.radius = calcMedian(distances);

        // step 3: determine the sets I = {i : distance[i] < r}, O = {i : distance[i] > r}, C = {i :
        // distance[i] == r}.
        I.clear();
        O.clear();
        C.clear();
#pragma omp parallel
        {
            std::vector<int> localI, localO, localC;

#pragma omp for
            for (int i = 0; i < pointsNum; i++) {
                if (distances[i] < circle.radius)
                    localI.push_back(i);
                else if (distances[i] > circle.radius)
                    localO.push_back(i);
                else
                    localC.push_back(i);
            }
#pragma omp critical
            {
                I.insert(I.end(), localI.begin(), localI.end());
                O.insert(O.end(), localO.begin(), localO.end());
                C.insert(C.end(), localC.begin(), localC.end());
            }
        }

        // step 4: calculate the values cos(θ_i).
#pragma omp parallel for
        for (int i = 0; i < pointsNum; i++) {
            cosTheta[i] = (points[i].x - circle.center.x) / distances[i];
            sinTheta[i] = (points[i].z - circle.center.z) / distances[i];
        }

        // step 5: compute EaPos, EaNeg, EbPos, EbNeg.
        double sumCosThetaI = std::accumulate(I.begin(), I.end(), 0.0, [&](double a, int i) { return a + cosTheta[i]; });
        double sumSinThetaI = std::accumulate(I.begin(), I.end(), 0.0, [&](double a, int i) { return a + sinTheta[i]; });
        double sumCosThetaO = std::accumulate(O.begin(), O.end(), 0.0, [&](double a, int i) { return a + cosTheta[i]; });
        double sumSinThetaO = std::accumulate(O.begin(), O.end(), 0.0, [&](double a, int i) { return a + sinTheta[i]; });
        double sumCosThetaC = std::accumulate(C.begin(), C.end(), 0.0, [&](double a, int i) { return a + abs(cosTheta[i]); });
        double sumSinThetaC = std::accumulate(C.begin(), C.end(), 0.0, [&](double a, int i) { return a + abs(sinTheta[i]); });

		double EaPos = sumCosThetaI - sumCosThetaO + sumCosThetaC;
        double EaNeg = sumCosThetaI - sumCosThetaO - sumCosThetaC;
        double EbPos = sumSinThetaI - sumSinThetaO + sumSinThetaC;
        double EbNeg = sumSinThetaI - sumSinThetaO - sumSinThetaC;
        
        // step 6: determine the new center.
        double alpha = EaNeg >= 0 ? 1 : EaPos <= 0 ? 0 : 0.5;
        double beta = EbNeg >= 0 ? 1 : EbPos <= 0 ? 0 : 0.5;
        double d1 = -(alpha * EaNeg + (1 - alpha) * EaPos);
        double d2 = -(beta * EbNeg + (1 - beta) * EbPos);
        circle.center.x += d1 * lambda;
        circle.center.z += d2 * lambda;

        if (std::sqrt(d1 * d1 + d2 * d2) * lambda < epsilon)
            return true;

        double newError = std::accumulate(
            distances.begin(), distances.end(), 0.0,
            [&](double sum, double b) { return sum + std::abs(b - circle.radius); });
        if (newError < error) {
            lambda *= 1.1;
            error = newError;
        }
        else {
            lambda *= 0.9;
        }
    }
    return true;
}

用了一些加速手段，但好像不明显