最小二乘拟合圆及椭圆

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;}