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

本文主要是介绍圆拟合 --- 点到圆距离绝对值之和最小，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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 forfor (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 forfor (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);elselocalC.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 forfor (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;
}