一、推导
二、分享给有需要的人，代码质量勿喷。
1 void xjLeastSquares::FitCenterByLeastSquares(std::map<int, std::vector<double>> mapPoint, std::vector<double> ¢erP, double &radius)
2 {
3 double sumX = 0, sumY = 0;
4 double sumXX = 0, sumYY = 0, sumXY = 0;
5 double sumXXX = 0, sumXXY = 0, sumXYY = 0, sumYYY = 0;
6
7 for (std::map<int, std::vector<double>>::iterator it = mapPoint.begin(); it != mapPoint.end(); ++it)
8 {
9 std::vector<double> p = it->second;
10
11 sumX += p[0];
12 sumY += p[1];
13 sumXX += p[0] * p[0];
14 sumYY += p[1] * p[1];
15 sumXY += p[0] * p[1];
16 sumXXX += p[0] * p[0] * p[0];
17 sumXXY += p[0] * p[0] * p[1];
18 sumXYY += p[0] * p[1] * p[1];
19 sumYYY += p[1] * p[1] * p[1];
20 }
21
22 int pCount = mapPoint.size();
23 double M1 = pCount * sumXY - sumX * sumY;
24 double M2 = pCount * sumXX - sumX * sumX;
25 double M3 = pCount * (sumXXX + sumXYY) - sumX * (sumXX + sumYY);
26 double M4 = pCount * sumYY - sumY * sumY;
27 double M5 = pCount * (sumYYY + sumXXY) - sumY * (sumXX + sumYY);
28
29 double a = (M1 * M5 - M3 * M4) / (M2*M4 - M1 * M1);
30 double b = (M1 * M3 - M2 * M5) / (M2*M4 - M1 * M1);
31 double c = -(a * sumX + b * sumY + sumXX + sumYY) / pCount;
32
33 //圆心XY 半径
34 double xCenter = -0.5*a;
35 double yCenter = -0.5*b;
36 radius = 0.5 * sqrt(a * a + b * b - 4 * c);
37 centerP[0] = xCenter;
38 centerP[1] = yCenter;
39 }