opencv2 Mat 根据线段line进行圆拟合

//对数据line进行拟合  圆拟合

typedef struct Line {
	Point p1; //起点
	Point p2; //终点
	double angle;
	double lenth;
	Point center; //起点
} LINE;

typedef struct cir {
	Point center;
	int radius;
} CIR;
//对数据进行拟合  圆拟合
cir fit_circle(Mat & img, vector<Line> &line, double A,
		double B, double R) {
	//cout << "into fit_circle " << endl;
	cir circ;
	Mat image;
	img.copyTo(image);
	int i;
	double X1, X2, X3, Y1, Y2, Y3, X1Y1, X1Y2, X2Y1;
	double C, D, E, G, H, N;
	double a, b, c;
	//拟合数据数量判断
	if (line.size() < g_config_port.fit_circle_line_size) {
		printf("Error: line  data number is less than 3!\n");
		circ.center = Point(0, 0);
		circ.radius = 0;
		return circ;
	}

	X1 = X2 = X3 = Y1 = Y2 = Y3 = X1Y1 = X1Y2 = X2Y1 = 0;

	for (i = 0; i < line.size(); i++) {
		X1 = X1 + line[i].center.x;
		Y1 = Y1 + line[i].center.y;
		X2 = X2 + line[i].center.x * line[i].center.x;
		Y2 = Y2 + line[i].center.y * line[i].center.y;
		X3 = X3 + line[i].center.x * line[i].center.x * line[i].center.x;
		Y3 = Y3 + line[i].center.y * line[i].center.y * line[i].center.y;
		X1Y1 = X1Y1 + line[i].center.x * line[i].center.y;
		X1Y2 = X1Y2 + line[i].center.x * line[i].center.y * line[i].center.y;
		X2Y1 = X2Y1 + line[i].center.x * line[i].center.x * line[i].center.y;
	}

	N = line.size();
	C = N * X2 - X1 * X1;
	D = N * X1Y1 - X1 * Y1;
	E = N * X3 + N * X1Y2 - (X2 + Y2) * X1;
	G = N * Y2 - Y1 * Y1;
	H = N * X2Y1 + N * Y3 - (X2 + Y2) * Y1;

	a = (H * D - E * G) / (C * G - D * D);
	b = (H * C - E * D) / (D * D - G * C);
	c = -(a * X1 + b * Y1 + X2 + Y2) / N;

	A = a / (-2);
	B = b / (-2);
	R = sqrt(a * a + b * b - 4 * c) / 2;
	Point cen = Point(int(A), int(B));
	int rad;

	circ.center = cen;
	circ.radius = int(R);
#ifdef SHOWIMAGE
	//  	 namedWindow("circle fit");
	// 	 imshow("circle fit",image);
#endif
	//cout << " fit_circle over " << endl;
	return circ;
}