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

一、推导

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

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  
    for (std::map<int, std::vector<double>>::iterator it = mapPoint.begin(); it != mapPoint.end(); ++it)  8  {  9         std::vector<double> p = it->second; 10  
        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  
    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  
    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  
    //圆心XY 半径
    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 }