過圓弧首尾點擬合圓的MATLAB和C++實現

 

 

自己想實現的功能就是：給了一系列的離散點，用圓或直線的方式進行擬合，大致效果圖如下：

先是找到了兩個人寫的很好的幾篇博客：

最小二乘法擬合圓——MATLAB和Qt-C++實現，這篇有MATLAB版本，但是其C++版本是結合了QT實現的

這個人的三點確定一個圓的計算方法，最小二乘法擬合圓，圓擬合算法（距離之和最小）都是進行圓的計算及擬合的

受限於本人的數據源的格式限制，本人對於上面的兩人的MATLAB及C++代碼都進行了簡單的改變來適應自己的需求

實現如下：

MATLAB實現：

function [xc,yc,R] = circleFitting( x, y )
%版權聲明：本文爲CSDN博主「馮Jungle」的原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接及本聲明。
%原文鏈接：https://blog.csdn.net/sinat_21107433/article/details/80877704
%函數測試：驗證：Verify_circleFittingAnd1.m
%備註：這個函數跟circleFitting1函數的運行結果是一樣的，只是一個矩陣版本，一個是非矩陣版本
%%
N = max(size(x));
sum_X_Raw = 0;
sum_Z_Raw = 0;
sum_XSquare_Raw = 0;
sum_ZSquare_Raw = 0;
sum_XCube_Raw = 0;
sum_ZCube_Raw = 0;
sum_XZZ_Raw = 0;
sum_XZ_Raw = 0;
sum_XXZ_Raw = 0;

for i=1:N
    sum_X_Raw = sum_X_Raw+x(i);
    sum_Z_Raw = sum_Z_Raw+y(i);
    sum_XSquare_Raw = sum_XSquare_Raw+x(i)*x(i);
    sum_ZSquare_Raw = sum_ZSquare_Raw+y(i)*y(i);
    sum_XCube_Raw = sum_XCube_Raw+x(i)*x(i)*x(i);
    sum_ZCube_Raw = sum_ZCube_Raw+y(i)*y(i)*y(i);
    sum_XZ_Raw = sum_XZ_Raw+x(i)*y(i);
    sum_XZZ_Raw = sum_XZZ_Raw+x(i)*y(i)*y(i);
    sum_XXZ_Raw = sum_XXZ_Raw+x(i)*x(i)*y(i);
end
D = N*sum_XZ_Raw-sum_X_Raw*sum_Z_Raw;
C = N*sum_XSquare_Raw-sum_X_Raw*sum_X_Raw;
E = N*sum_XCube_Raw+N*sum_XZZ_Raw-(sum_XSquare_Raw+sum_ZSquare_Raw)*sum_X_Raw;
G = N*sum_ZSquare_Raw-sum_Z_Raw*sum_Z_Raw;
H = N*sum_ZCube_Raw+N*sum_XXZ_Raw-(sum_XSquare_Raw+sum_ZSquare_Raw)*sum_Z_Raw;

a = (H*D-E*G)/(C*G-D*D);
b = (H*C-E*D)/(D*D-G*C);
c = -((sum_XSquare_Raw+sum_ZSquare_Raw)+a*sum_X_Raw+b*sum_Z_Raw)/N;

xc = -0.5*a;
yc = -0.5*b;
R = 0.5*sqrt(a*a+b*b-4*c);
end

下面這個也是MATLAB的實現，這個是用矩陣方法實現的，跟上面的程序的運行結果是一樣的，這個是以前師兄發給我的代碼，也不知是從哪裏來的，就不能標註原文鏈接了，作者勿怪，路人勿噴，謝謝！

function [xc,yc,R]=circleFitting1(x,y)
% CIRCLEFIT fits a circle in x,y plane
% x^2+y^2+a(1)*x+a(2)*y+a(3)=0
%可將此等式寫成矩陣的形式  [x1^2 + y1^2] = -[x1,y1,1]  [a(1)]
%                         [x2^2 + y2^2] = -[x2,y2,1]  [a(2)]
%                         [x3^2 + y3^2] = -[x3,y3,1]  [a(3)]
%此矩陣左邊是一個D（n*1）的矩陣，右邊是一個C（n*3）的矩陣，不能直接乘過去，所以兩邊都乘以C的轉置
n=length(x);
xx=x.*x;
yy=y.*y;
xy=x.*y;

A=[sum(x) sum(y) n;sum(xy) sum(yy) sum(y);sum(xx) sum(xy) sum(x)];
B=[-sum(xx+yy);-sum(xx.*y+yy.*y);-sum(xx.*x+xy.*y)];
a=A\B;                %A的逆乘以B
xc = -0.5*a(1);
yc = -0.5*a(2);
R = sqrt(-(a(3)-xc^2-yc^2));

%以下代碼也可以實現此功能
% C=[x;y;ones(1,4)]';
% D=[-(xx+yy)]';
% P=(C'*C)\(C'*D);     %主要是爲了構造能夠求逆並轉乘的矩陣
% xcc = -0.5*P(1);
% ycc = -0.5*P(2);
% Rc = sqrt(-(P(3)-xcc^2-ycc^2));
end

C++實現：

void CtestDlg::circleLeastFitting(double x0[], double y0[], int N, double &center_x, double &center_y, double &radius)  //砂輪
{//函數功能：輸入一系列的離散點x,y，對離散點進行最小二乘法圓擬合，輸出圓心的座標（x,y）及半徑radius
//版權聲明：本文爲CSDN博主「liyuanbhu」的原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接及本聲明。
//原文鏈接：https://blog.csdn.net/liyuanbhu/article/details/50889951
     if (N < 3)
     {
         return ;
     }
     double sum_x = 0.0f, sum_y = 0.0f;
     double sum_x2 = 0.0f, sum_y2 = 0.0f;
     double sum_x3 = 0.0f, sum_y3 = 0.0f;
     double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;

     for (int i = 0; i < N; i++)
     {
         double x = x0[i];
         double y = y0[i];
         double x2 = x * x;
         double y2 = y * y;
         sum_x += x;
         sum_y += y;
         sum_x2 += x2;
         sum_y2 += y2;
         sum_x3 += x2 * x;
         sum_y3 += y2 * y;
         sum_xy += x * y;
         sum_x1y2 += x * y2;
         sum_x2y1 += x2 * y;
     }

     double C, D, E, G, H;
     double a, b, c;

     C = N * sum_x2 - sum_x * sum_x;
     D = N * sum_xy - sum_x * sum_y;
     E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
     G = N * sum_y2 - sum_y * sum_y;
     H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
     a = (H * D - E * G) / (C * G - D * D);
     b = (H * C - E * D) / (D * D - G * C);
     c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

     center_x = a / (-2);
     center_y = b / (-2);
     radius = sqrt(a * a + b * b - 4 * c) / 2;
}

但是當我把這些代碼用到自己的數據源中，發現了一個問題，就是像我最開始展示的那張圖中，在不同顏色的連接點處，兩段曲線並不是相連的，具體如下：

因爲自己的目的是要兩段曲線在分界點是首尾相連的，所以就要保證擬合的圓必須過首尾點，因此我做的就是用三點確定一個圓的方法，首先選定首尾點，然後從第2到第n-1個點中選擇一個使擬合誤差（所有離散點到圓心距離與圓半徑差值的累加和）最小的點。

MATLAB實現如下：

function [center_x,center_y,radius] = circleFitting_throughHandT( x0, y0 )
%函數功能：輸入一系列的離散點的座標x,y，進行圓心計算，要求經過首尾點，並使擬合誤差最小
%函數測試：驗證：Verify_circleFitting_throughHandT.m
%備註：這個函數就是首選選取離散點的首尾點，然後依次從第2到倒數第2個點進行計算圓心，並計算擬合誤差，選取誤差最小的圓心
%%
x_head = x0(1);  y_head = y0(1);
x_tail = x0(end); y_tail = y0(end);
N = max(size(x0));
ERROR = zeros(1,N-2);
CENTER_X = zeros(1,N-2);
CENTER_Y = zeros(1,N-2);
R = zeros(1,N-2);
for i = 2:N-1
    x_add = x0(i);   y_add = y0(i);
    a = 2 * (x_add - x_head);
    b = 2 * (y_add - y_head);
    c = x_add * x_add + y_add * y_add - x_head * x_head - y_head * y_head;
    d = 2 * (x_tail - x_add);
    e = 2 * (y_tail - y_add);
    f = x_tail * x_tail + y_tail * y_tail - x_add * x_add - y_add * y_add;
    xc = (b * f - e * c) / (b * d - e * a);
    yc = (d * c - a * f) / (b * d - e * a);
    r = sqrt(((xc - x_head) * (xc - x_head) + (yc - y_head) * (yc - y_head)));
    
    CENTER_X(i-1) = xc;   CENTER_Y(i-1) = yc;   R(i-1) = r;
    error = 0;
    for j = 1:N
        error = error + abs(hypot(x0(i)-xc,y0(i)-yc) - r);        
    end
    ERROR(i-1) = error;
end

[min_error,index] = min(ERROR);

center_x = CENTER_X(index);
center_y = CENTER_Y(index);
radius = R(index);
end

C++實現：

void CtestDlg::circleFitting_throughHandT(double x0[], double y0[], int N, double &center_x, double &center_y, double &radius)  
{//函數功能：輸入一系列的離散點x,y，對離散點進行最小二乘法圓擬合，並且必過首尾點，輸出圓心的座標（x,y）及半徑radius
	double x_head = x0[0], y_head = y0[0];
	double x_mid = 0, y_mid = 0;
    double x_tail = x0[N-1], y_tail = y0[N-1];
	double a,b,c,d,e,f,xc,yc,r,error;	
	double *CENTER_X = new double[N-2];  //需要減去首尾兩個點
	double *CENTER_Y = new double[N-2];
	double *R = new double[N-2];
	double *ERR = new double[N-2];
	for (int i=1;i<N-1;i++)
	{
		x_mid = x0[i];   y_mid = y0[i];
		a = 2 * (x_mid - x_head);
		b = 2 * (y_mid - y_head);
		c = x_mid * x_mid + y_mid * y_mid - x_head * x_head - y_head * y_head;
		d = 2 * (x_tail - x_mid);
		e = 2 * (y_tail - y_mid);
		f = x_tail * x_tail + y_tail * y_tail - x_mid * x_mid - y_mid * y_mid;

		xc = (b * f - e * c) / (b * d - e * a);
		yc = (d * c - a * f) / (b * d - e * a);
		r = sqrt(((xc - x_head) * (xc - x_head) + (yc - y_head) * (yc - y_head)));
    
		CENTER_X[i-1] = xc;   CENTER_Y[i-1] = yc;   R[i-1] = r;
		error = 0;
		for (int j=1;j<N-1;j++)
			error = error + abs(hypot(x0[j]-xc,y0[j]-yc) - r);     
		ERR[i-1] = error;
	}

	double min_err = ERR[0];
	center_x = CENTER_X[0];
	center_y = CENTER_Y[0];
	radius = R[0];
	for (int k=1;k<N-2;k++)  //注意：此處ERR數組只有N-2個元素
	{
		if (ERR[k] < min_err)
		{
			center_x = CENTER_X[k];
			center_y = CENTER_Y[k];
			radius = R[k];
			min_err = ERR[k];
		}
	}
	
	delete []CENTER_X;
	delete []CENTER_Y;
	delete []R;
	delete []ERR;
}

具體效果如下：