一、推导
二、分享给有需要的人,代码质量勿喷。
void xjLeastSquares::FitCenterByLeastSquares(std::map<int, std::vector<double>> mapPoint, std::vector<double> ¢erP, double &radius)
{
double sumX = 0, sumY = 0;
double sumXX = 0, sumYY = 0, sumXY = 0;
double sumXXX = 0, sumXXY = 0, sumXYY = 0, sumYYY = 0;
for (std::map<int, std::vector<double>>::iterator it = mapPoint.begin(); it != mapPoint.end(); ++it)
{
std::vector<double> p = it->second;
sumX += p[0];
sumY += p[1];
sumXX += p[0] * p[0];
sumYY += p[1] * p[1];
sumXY += p[0] * p[1];
sumXXX += p[0] * p[0] * p[0];
sumXXY += p[0] * p[0] * p[1];
sumXYY += p[0] * p[1] * p[1];
sumYYY += p[1] * p[1] * p[1];
}
int pCount = mapPoint.size();
double M1 = pCount * sumXY - sumX * sumY;
double M2 = pCount * sumXX - sumX * sumX;
double M3 = pCount * (sumXXX + sumXYY) - sumX * (sumXX + sumYY);
double M4 = pCount * sumYY - sumY * sumY;
double M5 = pCount * (sumYYY + sumXXY) - sumY * (sumXX + sumYY);
double a = (M1 * M5 - M3 * M4) / (M2*M4 - M1 * M1);
double b = (M1 * M3 - M2 * M5) / (M2*M4 - M1 * M1);
double c = -(a * sumX + b * sumY + sumXX + sumYY) / pCount;
//圆心XY 半径
double xCenter = -0.5*a;
double yCenter = -0.5*b;
radius = 0.5 * sqrt(a * a + b * b - 4 * c);
centerP[0] = xCenter;
centerP[1] = yCenter;
}