多離散點的圓擬合

最近項目涉及到多個圓盤的旋轉和運動。這個時候繞不開圓盤圓心、半徑的求解。

簡單的來說，三點必能確定一個唯一的圓。圓的標準公式是。這個公式在求解的時候會比較麻煩，一般會用它的展開公式，就是一般式。圓心爲,圓的半徑就是 。這個通過簡單的解方程就可以得到答案了。

下面就是三點求圓的代碼：

LONG CalCenter(stLPOSN lTblPosn[] )
{
	stDPOSN	SqPt1, SqPt2, SqPt3 ;
	double	SqPtOneTwo, SqPtOneThree ;
	double	numerator_x, denominator_x, numerator_y, denominator_y ;

	SqPt1.x = pow(lTblPosn[0].x,2) ;
	SqPt1.y = pow(lTblPosn[0].y,2) ;

	SqPt2.x = pow(lTblPosn[1].x,2) ;
	SqPt2.y = pow(lTblPosn[1].y,2) ;

	SqPt3.x = pow(lTblPosn[2].x,2) ;
	SqPt3.y = pow(lTblPosn[2].y,2) ;

	SqPtOneTwo = SqPt1.x + SqPt1.y - SqPt2.x - SqPt2.y ;

	SqPtOneThree = SqPt1.x + SqPt1.y - SqPt3.x - SqPt3.y ;

	numerator_x=2*(lTblPosn[0].y-lTblPosn[1].y)*SqPtOneThree-2*(lTblPosn[0].y-lTblPosn[2].y)*SqPtOneTwo ;

	denominator_x=4*(lTblPosn[0].y-lTblPosn[1].y)*(lTblPosn[0].x-lTblPosn[2].x )-4*lTblPosn[0].y-lTblPosn[2].y)*(lTblPosn[0].x-lTblPosn[1].x) ;
	
	long x = (LONG) ( numerator_x / denominator_x );


	numerator_y =  SqPtOneTwo-2*m_lRingCenter.x*(lTblPosn[0].x- lTblPosn[1].x ) ;
	
	denominator_y =  2 * ( lTblPosn[0].y - lTblPosn[1].y ) ;

	long y = (LONG) ( numerator_y / denominator_y );
	return OK;
}

但實際上，三點算法對於外部輸入要求比較高，在很多工程應用中是無法適應的。這個時候就需要找其他算法替代了。一種常用的替代方案就是基於最小二乘法的擬合算法了。最小二乘法是用最簡的方法求得一些絕對不可知的真值，而令誤差平方之和爲最小來尋找一組數據的最佳匹配函數的計算方法，最小二乘法通常用於曲線擬合 (least squares fitting)，擬合算法的推倒過程就省略了，直接看代碼吧

int reform_data	(double ax[],
				 double ay[],
				 int	n,
				 double *mx,
				 double *my,
				 double *suu,
				 double *suv,
				 double *svv,
				 double *suuu,
				 double *suvv,
				 double *svvv,
				 double *svuu)
{
	double *au = (double*) malloc(sizeof(double) * n);
	double *av = (double*) malloc(sizeof(double) * n);

	if (au == NULL)
		return -1;		/// not enough memory

	if (av == NULL)
	{
		free(au);
		return -1;
	}
	
	double sx = 0;
	double sy = 0;
	for (int i=0; i<n; i++)
	{
		sx += ax[i];
		sy += ay[i];
	}

	*mx = sx / (double) n;
	*my = sy / (double) n;

	for (int i=0; i<n; i++)
	{
		au[i] = ax[i] - *mx;
		av[i] = ay[i] - *my;
	}


	double uu;
	double uv;
	double vv;

	*suu = 0;
	*suv = 0;
	*svv = 0;
	*suuu = 0;
	*svvv = 0;
	*suvv = 0;
	*svuu = 0;
	for (int i=0; i<n; i++)
	{
		uu = au[i] * au[i];
		vv = av[i] * av[i];
		uv = au[i] * av[i];
		*suu += uu;
		*suv += uv;
		*svv += vv;
		*suuu += au[i] * uu;
		*suvv += uv * av[i];
		*svuu += uv * au[i];
		*svvv += av[i] * vv;
	}

	

	free(au);
	free(av);

	return 0;
}


int fit_circle (double	ax[],
				double	ay[],
				int		n,
				double	*px,
				double	*py,
				double	*pr)
{
	double mx;
	double my;
	double suu;
	double suv;
	double svv;
	double suuu;
	double svuu;
	double suvv;
	double svvv;
	
	int nret = reform_data(ax, ay, n, &mx, &my, &suu, &suv, &svv, &suuu, &suvv, &svvv, &svuu); 
	if (nret != 0)
		return nret;

	double data_a[2][2] = {
		{suu, suv},
		{suv, svv}
	};

	double data_b[] = {
		.5*(suuu+suvv),
		.5*(svvv+svuu)
	};

	double data_x[] = {0, 0};
	
	double det = data_a[0][0]*data_a[1][1] - data_a[1][0] * data_a[0][1];

	data_a[0][0] /= det;
	data_a[0][1] /= det;
	data_a[1][0] /= det;
	data_a[1][1] /= det;

	data_x[0] = data_a[1][1]*data_b[0] - data_a[0][1]*data_b[1];
	data_x[1] = data_a[0][0]*data_b[1] - data_a[1][0]*data_b[0];

	*px = data_x[0]+mx;
	*py = data_x[1]+my;
	*pr = sqrt(data_x[0]*data_x[0]+data_x[1]*data_x[1]+(suu+svv)/n);

	return 0;
}