最小二乘法拟合圆（Python&C++实现）

陆先森不怕鬼

已于 2023-03-24 08:55:16 修改

阅读量2.9k

点赞数 8

文章标签： python 最小二乘法

于 2022-12-13 17:20:07 首次发布

本文链接：https://blog.csdn.net/weixin_44729155/article/details/128304671

版权

方法一：矩阵计算法

圆的方程可以表示为：
$R^2 = (x-A)^2+(y-B)^2\tag{1}$
展开后为：
$R^2 = x^2-2Ax+A^2+y^2-2By+B^2$
令 $a=-2A,b=-2B,c=A^2+B^2-R^2$ :
则式 $(1)$ 可表示为：
$x^2+y^2+ax+by+c=0$
$ax-by-c = x^2+y^2$
矩阵形式可表示为：
$\begin{gathered} \quad \begin{bmatrix} -x_1 & -y_1 & -1 \\ -x_2 & -y_2 & -1 \\ \cdots & \cdots & \cdots \\ -x_n & -y_n & -1 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \end{bmatrix} = \begin{bmatrix} x_1^2+y_1^2 \\ x_2^2+y_2^2 \\ \cdots \\ x_n^2+y_n^2 \end{bmatrix} \end{gathered}$
得到 $\cdot A = Y$ 的矩阵形式后，便可利用 $(R^T \cdot R)^{-1} \cdot R^T \cdot Y$ 得到 $a 、 b 、 c$ 的值。
Python代码为：

def Fit_Circle(x=None, y=None):
    if x is None:
        x = [1, 0, -1, 0]
    if y is None:
        y = [0, 1, 0, -1]

    Y = []
    R = []

    for i in range(len(x)):
        R.append([-x[i], -y[i], -1])
        Y.append(np.square(x[i]) + np.square(y[i]))

    R = np.mat(R)
    A = np.dot(np.dot(np.linalg.inv(np.dot(R.T, R)), R.T), Y)
    A = np.array(A, dtype='float32').flatten()
    return round(-(A[0] / 2), 2), round(-(A[1] / 2), 2), round(np.sqrt(np.square(A[0]) + np.square(A[1]) - 4 * A[2]) / 2, 2)

根据圆心坐标和半径画出圆的代码为：

def Show_Circle(x=0, y=0, r=2):
    theta = np.arange(0, 2 * np.pi, 0.01)
    x = x + r * np.cos(theta)
    y = y + r * np.sin(theta)
    fig = plt.figure()
    axes = fig.add_subplot(111)
    axes.plot(x, y)
    axes.axis('equal')
    plt.show()

Python完整代码为：

import matplotlib.pyplot as plt
import numpy as np


def Show_Circle(x=0, y=0, r=2):
    theta = np.arange(0, 2 * np.pi, 0.01)
    x = x + r * np.cos(theta)
    y = y + r * np.sin(theta)
    fig = plt.figure()
    axes = fig.add_subplot(111)
    axes.plot(x, y)
    axes.axis('equal')
    plt.show()


def Fit_Circle(x=None, y=None):
    if x is None:
        x = [1, 0, -1, 0]
    if y is None:
        y = [0, 1, 0, -1]

    Y = []
    R = []

    for i in range(len(x)):
        R.append([-x[i], -y[i], -1])
        Y.append(np.square(x[i]) + np.square(y[i]))

    R = np.mat(R)
    A = np.dot(np.dot(np.linalg.inv(np.dot(R.T, R)), R.T), Y)
    A = np.array(A, dtype='float32').flatten()
    return round(-(A[0] / 2), 2), round(-(A[1] / 2), 2), round(np.sqrt(np.square(A[0]) + np.square(A[1]) - 4 * A[2]) / 2, 2)


if __name__ == '__main__':
    a = [2, 0, -2, 0]
    b = [0, 2, 0, -2]
    X, Y, R = Fit_Circle(x=a, y=b)
    Show_Circle(X, Y, R)

C++完整代码为：

#include<iostream>
#include <iomanip>
#include<math.h>

using namespace std;

double a[][2] = {{2,0},{0,2},{-2,0},{0,-2}};
int M = sizeof(a) / sizeof(a[0]);

void matrix_inverse(double(*a)[3], double(*b)[3]);
	
int main() 
{
	double R[M][3] = {0};
	double R_T[3][M] = {0};
	double RR[3][3] = {0};	// R_T * R
	double RR_1[3][3] = {0};
	double RR_1R_T[3][M] = {0};	// RR_1 * R_T
	double Y[M][1] = {0};
	double A[3][1] = {0};
	
	for(int i = 0; i < M; i++)
	{
		R[i][0] = -a[i][0];
		R[i][1] = -a[i][1];
		R[i][2] = -1;
		
		R_T[0][i] = -a[i][0];
		R_T[1][i] = -a[i][1];
		R_T[2][i] = -1;
		
		Y[i][0] = pow(a[i][0], 2) + pow(a[i][1], 2);
	}
	
	for(int i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数） 
		for(int j = 0; j < 3; ++j)
		{
			RR[i][j] = 0;
			for(int k = 0; k < M; ++k)
				RR[i][j] += R_T[i][k] * R[k][j];
		}
			
	matrix_inverse(RR, RR_1);
	
	for(int i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数） 
		for(int j = 0; j < M; ++j)
		{
			RR_1R_T[i][j] = 0; 
			for(int k = 0; k < 3; ++k)
				RR_1R_T[i][j] += RR_1[i][k] * R_T[k][j];
		}
				
	for(int i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数） 
		for(int j = 0; j < 1; ++j)
		{
			A[i][j] = 0;
			for(int k = 0; k < M; ++k)
				A[i][j] += RR_1R_T[i][k] * Y[k][j];
		}
	
	double x = -(A[0][0] / 2);
	double y = -(A[1][0] / 2);
	double r = pow(pow(x, 2) + pow(y, 2) - A[2][0], 0.5);
	
	cout.setf(ios::fixed);
	cout << "圆心坐标：（" << setprecision(2) << x << "," << setprecision(2) << y << "）" << endl;
	cout << "半径：" << setprecision(2) << r << endl;
	
	return 0;
}

void matrix_inverse(double(*a)[3], double(*b)[3])			// 矩阵求逆 
{
	using namespace std;
	int i, j, k;
	double max, temp;
	// 定义一个临时矩阵t
	double t[3][3];
	// 将a矩阵临时存放在矩阵t[n][n]中
	for (i = 0; i < 3; i++)
		for (j = 0; j < 3; j++)
			t[i][j] = a[i][j];
	// 初始化B矩阵为单位矩阵
	for (i = 0; i < 3; i++)
		for (j = 0; j < 3; j++)
			b[i][j] = (i == j) ? (double)1 : 0;
	// 进行列主消元，找到每一列的主元
	for (i = 0; i < 3; i++)
	{
		max = t[i][i];
		// 用于记录每一列中的第几个元素为主元
		k = i;
		// 寻找每一列中的主元元素
		for (j = i + 1; j < 3; j++)
		{
			if (fabs(t[j][i]) > fabs(max))
			{
				max = t[j][i];
				k = j;
			}
		}
		//cout<<"the max number is "<<max<<endl;
		// 如果主元所在的行不是第i行，则进行行交换
		if (k != i)
		{
			// 进行行交换
			for (j = 0; j < 3; j++)
			{
				temp = t[i][j];
				t[i][j] = t[k][j];
				t[k][j] = temp;
				// 伴随矩阵B也要进行行交换
				temp = b[i][j];
				b[i][j] = b[k][j];
				b[k][j] = temp;
			}
		}
		if (t[i][i] == 0)
		{
			cout << "\nthe matrix does not exist inverse matrix\n";
			break;
		}
		// 获取列主元素
		temp = t[i][i];
		// 将主元所在的行进行单位化处理
		//cout<<"\nthe temp is "<<temp<<endl;
		for (j = 0; j < 3; j++)
		{
			t[i][j] = t[i][j] / temp;
			b[i][j] = b[i][j] / temp;
		}
		for (j = 0; j < 3; j++)
		{
			if (j != i)
			{
				temp = t[j][i];
				//消去该列的其他元素
				for (k = 0; k < 3; k++)
				{
					t[j][k] = t[j][k] - temp * t[i][k];
					b[j][k] = b[j][k] - temp * b[i][k];
				}
			}
		}
	}
}

方法二：直接计算法

圆的表达式为：
$R^2 = (x-A)^2+(y-B)^2\tag{1}$
化简得：
$x^2+y^2=ax+by+c=0$
样本点到拟合圆心的距离的平方与半径平方的差值为：
$δ_i=d_i^2-R^2=(X_i-A)^2+(Y_i-B)^2-R^2=X_i^2+Y_i^2+aX_i+bY_i+c$
分别对a、b、c求偏导后可得：
$\frac{∂Q(a,b,c)}{∂a}=Σ2(X_i^2+Y_i^2+aX_i+bY_i+c)X_i=0$
$\frac{∂Q(a,b,c)}{∂b}=Σ2(X_i^2+Y_i^2+aX_i+bY_i+c)Y_i=0$
$\frac{∂Q(a,b,c)}{∂c}=Σ2(X_i^2+Y_i^2+aX_i+bY_i+c)=0$
用以下参数表示：
$C=NΣX_i^2-ΣX_iΣX_i$
$D=NΣX_iY_i-ΣX_iΣY_i$
$E=NΣX_i^3+NΣX_iY_i^2-Σ(X_i^2+Y_i^2)ΣX_i$
$G=NΣY_i^2-ΣY_iΣY_i$
$H=NΣX_i^2Y_i+NΣY_i^3-Σ(X_i^2+Y_i^2)ΣY_i$
即可得到：
$a=\frac{HD-EG}{CG-D^2}$
$b=\frac{HC-ED}{D^2-CG}$
$c=-\frac{Σ(X_i^2+Y_i^2)+aΣX_i+bΣY_i}{N}$
然后利用 $A=-\frac{a}{2}$ 、 $B=-\frac{b}{2}$ 、 $R=\frac{1}{2}\sqrt{a^2+b^2-ac}$ 便可获得具体的方程表达式。
C++程序为：

#include<iostream>
#include<iomanip>
#include<math.h>
#define PAI acos(-1)

using namespace std;
double point[][2] = {{0,0},{2,0},{1,-1},{1,1}};
int M = sizeof(point) / sizeof(point[0]);

int main()
{
	double e=0,e11=0,e1=0,e12=0,e2=0,e111=0,e122=0,e1122=0,e22=0,e112=0,e222=0;//calculate
	double ti,ri,yi;
	for(int i=0;i<M;i++)
	{
		ti=point[i][0];
		ri=point[i][1];
		yi=ti*ti+ri*ri;
		e+=1;
		e1+=ti;
		e2+=ri;
		e11+=ti*ti;
		e12+=ti*ri;
		e22+=ri*ri;
		e111+=ti*ti*ti;
		e122+=ti*ri*ri;
		e1122+=yi;
		e112+=ti*ti*ri;
		e222+=ri*ri*ri;
	}
	double a = ((e*e112+e*e222-e1122*e2)*(e*e12-e1*e2)-(e*e111+e*e122-e1122*e1)*(e*e22-e2*e2))/((e*e11-e1*e1)*(e*e22-e2*e2)-(e*e12-e1*e2)*(e*e12-e1*e2));
	double b = ((e*e112+e*e222-e1122*e2)*(e*e11-e1*e1)-(e*e111+e*e122-e1122*e1)*(e*e12-e1*e2))/((e*e12-e1*e2)*(e*e12-e1*e2)-(e*e11-e1*e1)*(e*e22-e2*e2));
	double c = -(e1122+a*e1+b*e2)/e;
	double t0,r0,d0;
	t0 = -a/2;
	r0 = -b/2;
	d0 = sqrt(a*a+b*b-4*c);
	
	// 重复计算
	e=0,e11=0,e1=0,e12=0,e2=0,e111=0,e122=0,e1122=0,e22=0,e112=0,e222=0;
	for(int i=0;i<M;i++)
	{
		ti=point[i][0]-t0;
		ri=point[i][1]-r0;
		yi=ti*ti+ri*ri;
		e+=1;
		e1+=ti;
		e2+=ri;
		e11+=ti*ti;
		e12+=ti*ri;
		e22+=ri*ri;
		e111+=ti*ti*ti;
		e122+=ti*ri*ri;
		e1122+=yi;
		e112+=ti*ti*ri;
		e222+=ri*ri*ri;
	}
	a = ((e*e112+e*e222-e1122*e2)*(e*e12-e1*e2)-(e*e111+e*e122-e1122*e1)*(e*e22-e2*e2))/((e*e11-e1*e1)*(e*e22-e2*e2)-(e*e12-e1*e2)*(e*e12-e1*e2));
	b = ((e*e112+e*e222-e1122*e2)*(e*e11-e1*e1)-(e*e111+e*e122-e1122*e1)*(e*e12-e1*e2))/((e*e12-e1*e2)*(e*e12-e1*e2)-(e*e11-e1*e1)*(e*e22-e2*e2));
	c = -(e1122+a*e1+b*e2)/e;
	double t1,r1,d1;
	t1 = -a/2;
	r1 = -b/2;
	d1 = sqrt(a*a+b*b-4*c);
	
	t1+=t0;
	r1+=r0;
	d1=(d0+d1)/2;
	
	
	cout.setf(ios::fixed);
	cout << "圆心坐标：（" << setprecision(2) << t1 << "," << setprecision(2) << r1 << "）" << endl;
	cout << "半径：" << setprecision(2) << d1/2 << endl;
	
	return 0;
}