基于Opencv的最小二乘法拟合圆及椭圆

Opencv最小二乘拟合圆源代码：

 

Point3f LeastSquareFittingCircle(vector<Point2f> temp_coordinates)//利用opencv solve函数的高斯消元算法（LU分解）解方程组
{
	float x1 = 0;
	float x2 = 0;
	float x3 = 0;
	float y1 = 0;
	float y2 = 0;
	float y3 = 0;
	float x1y1 = 0;
	float x1y2 = 0;
	float x2y1 = 0;
	int num;
	vector<Point2f>::iterator k;
	Point3f tempcircle;
	num = temp_coordinates.size();
	for (k = temp_coordinates.begin(); k != temp_coordinates.end(); k++)
	{

		x1 = x1 + (*k).x;
		x2 = x2 + (*k).x * (*k).x;
		x3 = x3 + (*k).x * (*k).x * (*k).x;
		y1 = y1 + (*k).y;
		y2 = y2 + (*k).y * (*k).y;
		y3 = y3 + (*k).y * (*k).y * (*k).y;
		x1y1 = x1y1 + (*k).x * (*k).y;
		x1y2 = x1y2 + (*k).x * (*k).y * (*k).y;
		x2y1 = x2y1 + (*k).x * (*k).x * (*k).y;
	}
	Mat left_matrix = (Mat_<float>(3,3) << x2, x1y1, x1, x1y1, y2, y1, x1, y1, num);
	cout << "left_matrix=" << left_matrix << endl;
	Mat right_matrix = (Mat_<float>(3, 1) << -(x3 + x1y2), -(x2y1 + y3), -(x2 + y2));
	cout << "right_matrix=" << right_matrix << endl;
	Mat solution(3,1,CV_32F);
	solve(left_matrix, right_matrix, solution,CV_LU);
	cout << "solution=" << solution << endl;
	float a, b, c;
	a = solution.at<float>(0);
	b = solution.at<float>(1);
	c = solution.at<float>(2);
	tempcircle.x = -a / 2; //圆心x坐标
	tempcircle.y = -b / 2;//圆心y坐标
	tempcircle.z = sqrt(a*a + b*b - 4 * c) / 2;//圆心半径
	return tempcircle;
}
Point3f LeastSquareFittingCircle(vector<Point2f> temp_coordinates)//高斯消元法直接求解方程组
{
	float x1 = 0;
	float x2 = 0;
	float x3 = 0;
	float y1 = 0;
	float y2 = 0;
	float y3 = 0;
	float x1y1 = 0;
	float x1y2 = 0;
	float x2y1 = 0;
	int num;
	vector<Point2f>::iterator k;
	Point3f tempcircle;
	for (k = temp_coordinates.begin(); k != temp_coordinates.end(); k++)
	{

		x1 = x1 + (*k).x;
		x2 = x2 + (*k).x * (*k).x;
		x3 = x3 + (*k).x * (*k).x * (*k).x;
		y1 = y1 + (*k).y;
		y2 = y2 + (*k).y * (*k).y;
		y3 = y3 + (*k).y * (*k).y * (*k).y;
		x1y1 = x1y1 + (*k).x * (*k).y;
		x1y2 = x1y2 + (*k).x * (*k).y * (*k).y;
		x2y1 = x2y1 + (*k).x * (*k).x * (*k).y;
	}
	float C, D, E, G, H, a, b, c;
	num = temp_coordinates.size();
	C = num*x2 - x1*x1;
	D = num*x1y1 - x1*y1;
	E = num*x3 + num*x1y2 - x1*(x2 + y2);
	G = num*y2 - y1*y1;
	H = num*x2y1 + num*y3 - y1*(x2 + y2);
	a = (H*D - E*G) / (C*G - D*D);
	b = (H*C - E*D) / (D*D - G*C);
	c = -(x2 + y2 + a*x1 + b*y1) / num;
	tempcircle.x = -a / 2; //圆心x坐标
	tempcircle.y = -b / 2;//圆心y坐标
	tempcircle.z = sqrt(a*a + b*b - 4 * c) / 2;//圆心半径
	return tempcircle;
}

最小二乘法拟合椭圆：

 

Point2f LeastSquareFittingEllipse(vector<Point2f> temp_coordinates)//QR分解
{
	float x1 = 0;
	float x2 = 0;
	float x3 = 0;
	float x4 = 0;
	float y1 = 0;
	float y2 = 0;
	float y3 = 0;
	float y4 = 0;
	float x1y1 = 0;
	float x1y2 = 0;
	float x1y3 = 0;
	float x2y1 = 0;
	float x2y2 = 0;
	float x3y1 = 0;
	int num;
	vector<Point2f>::iterator k;
	Point2f tempcircle;
	num = temp_coordinates.size();
	for (k = temp_coordinates.begin(); k != temp_coordinates.end(); k++)
	{
		x1 = x1 + (*k).x;
		x2 = x2 + pow((*k).x, 2);
		x3 = x3 + pow((*k).x, 3);
		x4 = x4 + pow((*k).x, 4);
		y1 = y1 + (*k).y;
		y2 = y2 + pow((*k).y, 2);
		y3 = y3 + pow((*k).y, 3);
		y4 = y4 + pow((*k).y, 4);
		x1y1 = x1y1 + (*k).x * (*k).y;
		x1y2 = x1y2 + (*k).x * pow((*k).y, 2);
		x1y3 = x1y3 + (*k).x * pow((*k).y, 3);
		x2y1 = x2y1 + pow((*k).x, 2) * (*k).y;
		x2y2 = x2y2 + pow((*k).x, 2) * pow((*k).y, 2);
		x3y1 = x3y1 + pow((*k).x, 3) * (*k).y;
	}
	Mat left_matrix = (Mat_<float>(6, 5) << x3y1, x2y2-x4,   x3,    x2y1,  x2,
						x2y2, x1y3-x3y1, x2y1,  x1y2,  x1y1,
						x1y3, y4-x2y2,   x1y2,  y3,    y2,
						x2y1, x1y2-x3,   x2,    x1y1,  x1,
						x1y2, y3-x2y1,   x1y1,  y2,    y1,
						x1y1, y2-x2,     x1,    y1,    num);
	cout << "left_matrix[6,5]=" << left_matrix << endl;
	Mat right_matrix = (Mat_<float>(6, 1) << -x4, -x3y1, -x2y2, -x3, -x2y1, -x2 );
	cout << "right_matrix[6,1]=" << right_matrix << endl;
	Mat ellipse_solution(5, 1, CV_32F);
	double t = getTickCount();
	solve(left_matrix, right_matrix, ellipse_solution, DECOMP_QR);
	t = getTickCount() - t;
	double ConsumeTime = t / (getTickFrequency());
	cout << "time =" << ConsumeTime << endl;
	cout << "ellipse_solution[5,1]=" << ellipse_solution << endl;
	float A, B, C, D, E, F;
	A = 1-ellipse_solution.at<float>(1);
	B = ellipse_solution.at<float>(0);
	C = ellipse_solution.at<float>(1);
	D = ellipse_solution.at<float>(2);
	E = ellipse_solution.at<float>(3);
	F = ellipse_solution.at<float>(4);
	tempcircle.x = (B*E - 2*C*D) / (4*A*C - B*B);
	tempcircle.y = (B*D - 2*A*E) / (4*A*C - B*B);
	return tempcircle;
}
Point2f LeastSquareFittingEllipse_LU(vector<Point2f> temp_coordinates)//LU分解
{
	float x1 = 0;
	float x2 = 0;
	float x3 = 0;
	//float x4 = 0;
	float y1 = 0;
	float y2 = 0;
	float y3 = 0;
	float y4 = 0;
	float x1y1 = 0;
	float x1y2 = 0;
	float x1y3 = 0;
	float x2y1 = 0;
	float x2y2 = 0;
	float x3y1 = 0;
	int num;
	vector<Point2f>::iterator k;
	Point2f tempcircle;
	num = temp_coordinates.size();
	for (k = temp_coordinates.begin(); k != temp_coordinates.end(); k++)
	{
		x1 = x1 + (*k).x;
		x2 = x2 + pow((*k).x, 2);
		x3 = x3 + pow((*k).x, 3);
		//x4 = x4 + pow((*k).x, 4);
		y1 = y1 + (*k).y;
		y2 = y2 + pow((*k).y, 2);
		y3 = y3 + pow((*k).y, 3);
		y4 = y4 + pow((*k).y, 4);
		x1y1 = x1y1 + (*k).x * (*k).y;
		x1y2 = x1y2 + (*k).x * pow((*k).y, 2);
		x1y3 = x1y3 + (*k).x * pow((*k).y, 3);
		x2y1 = x2y1 + pow((*k).x, 2) * (*k).y;
		x2y2 = x2y2 + pow((*k).x, 2) * pow((*k).y, 2);
		x3y1 = x3y1 + pow((*k).x, 3) * (*k).y;
	}
	Mat left_matrix = (Mat_<float>(5, 5) << x2y2, x1y3, x2y1, x1y2, x1y1,
						x1y3, y4, x1y2, y3, y2,
						x2y1, x1y2, x2, x1y1, x1,
						x1y2, y3, x1y1, y2, y1,
						x1y1, y2, x1, y1, num );
	cout << "left_matrix[5,5]=" << left_matrix << endl;
	Mat right_matrix = (Mat_<float>(5, 1) << -x3y1, -x2y2, -x3, -x2y1, -x2);
	cout << "right_matrix[5,1]=" << right_matrix << endl;
	Mat ellipse_solution(5, 1, CV_32F);
	solve(left_matrix, right_matrix, ellipse_solution, DECOMP_LU);
	cout << "ellipse_solution[5,1]=" << ellipse_solution << endl;
	float A, B, C, D, E, F;
	A = ellipse_solution.at<float>(0);
	B = ellipse_solution.at<float>(1);
	C = ellipse_solution.at<float>(2);
	D = ellipse_solution.at<float>(3);
	E = ellipse_solution.at<float>(4);
	tempcircle.x = (2*B*C - A*D) / (A*A - 4*B);
	tempcircle.y = (2*D - A*D) / (A*A - 4*B);
	return tempcircle;
}