用C#最小二乘法拟合圆形,计算圆心和半径

发布于:2025-05-28 ⋅ 阅读:(33) ⋅ 点赞:(0)

用C#最小二乘法拟合圆形,计算圆心和半径

using System;
using System.Collections.Generic;

namespace ConsoleApp2
{
    internal class Program
    {
        static void Main(string[] args)
        {
            List<Tuple<double, double>> points = new List<Tuple<double, double>>();
            points.Add(Tuple.Create(0.0, 1.0));
            points.Add(Tuple.Create(1.0, 0.0));
            points.Add(Tuple.Create(2.0, 1.0));
            points.Add(Tuple.Create(1.0, 2.0));

            Operator2D.FitCircle(points,out double centerX,out double centerY, out double radius);
        }
    }


}

class Operator2D
{
    public static void FitCircle(List<Tuple<double, double>> points, out double centerX, out double centerY, out double radius)
    {
        double[,] ATA = new double[3, 3];
        double[] ATB = new double[3];

        foreach (var pt in points)
        {
            double x = pt.Item1, y = pt.Item2;
            double twoX = 2 * x, twoY = 2 * y;
            double B_i = x * x + y * y;

            // 构建ATA矩阵
            ATA[0, 0] += twoX * twoX;
            ATA[0, 1] += twoX * twoY;
            ATA[0, 2] += twoX;

            ATA[1, 0] += twoY * twoX;
            ATA[1, 1] += twoY * twoY;
            ATA[1, 2] += twoY;

            ATA[2, 0] += twoX;
            ATA[2, 1] += twoY;
            ATA[2, 2] += 1;

            // 构建ATB向量
            ATB[0] += twoX * B_i;
            ATB[1] += twoY * B_i;
            ATB[2] += B_i;
        }

        // 计算逆矩阵
        double[,] invATA = Invert3x3Matrix(ATA);

        // 求解参数
        double a = invATA[0, 0] * ATB[0] + invATA[0, 1] * ATB[1] + invATA[0, 2] * ATB[2];
        double b = invATA[1, 0] * ATB[0] + invATA[1, 1] * ATB[1] + invATA[1, 2] * ATB[2];
        double c = invATA[2, 0] * ATB[0] + invATA[2, 1] * ATB[1] + invATA[2, 2] * ATB[2];

        // 计算半径
        double rSquared = a * a + b * b + c;
        if (rSquared < 0) throw new ArgumentException("无法拟合有效圆形");
        radius = Math.Sqrt(rSquared);
        centerX = a;
        centerY = b;
    }

    // 3x3矩阵求逆
    static double[,] Invert3x3Matrix(double[,] matrix)
    {
        double a = matrix[0, 0], b = matrix[0, 1], c = matrix[0, 2];
        double d = matrix[1, 0], e = matrix[1, 1], f = matrix[1, 2];
        double g = matrix[2, 0], h = matrix[2, 1], i = matrix[2, 2];

        double det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g);
        if (Math.Abs(det) < 1e-12) throw new ArgumentException("矩阵不可逆");

        double invDet = 1.0 / det;
        return new double[,]
        {
            { (e * i - f * h) * invDet, (c * h - b * i) * invDet, (b * f - c * e) * invDet },
            { (f * g - d * i) * invDet, (a * i - c * g) * invDet, (c * d - a * f) * invDet },
            { (d * h - e * g) * invDet, (b * g - a * h) * invDet, (a * e - b * d) * invDet }
        };
    }
}

在这里插入图片描述