C++：最小二乘法拟合圆

一、推导

二、分享给有需要的人，代码质量勿喷。

 1 void xjLeastSquares::FitCenterByLeastSquares(std::map<int, std::vector<double>> mapPoint, std::vector<double> &centerP, 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 }

posted on 2020-12-28 15:24 一杯清酒邀明月阅读(1596) 评论(0) 编辑收藏举报