多离散点的圆拟合

121 阅读 0 评论 80 点赞

我是靠谱客的博主冷静麦片，这篇文章主要介绍多离散点的圆拟合，现在分享给大家，希望可以做个参考。

最近项目涉及到多个圆盘的旋转和运动。这个时候绕不开圆盘圆心、半径的求解。

简单的来说，三点必能确定一个唯一的圆。圆的标准公式是 $（x-x_{0}）^{2}$ $(x-x_0)^2+(y-y_0)^2=r^2$ 。这个公式在求解的时候会比较麻烦，一般会用它的展开公式，就是一般式 $x^2+y^2+Ax+By+C=0$ 。圆心为 $(frac{-A}{2},frac{-B}{2})$ ,圆的半径就是 $sqrt{(frac{-A}{2})^2+(frac{-B}{2})^2-C}$ 。这个通过简单的解方程就可以得到答案了。

下面就是三点求圆的代码：

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
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;
}