C++:最小二乘法 拟合圆算法,已用在项目使用了

  • wm_chen09
    了解作者
  • C/C++
    开发工具
  • 674B
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 5 积分
    下载积分
  • 0
    下载次数
  • 2022-07-03 12:56
    上传日期
C++:最小二乘法 拟合圆算法,多散点轨迹拟合
C++:最小二乘法 拟合圆.rar
  • C++:最小二乘法 拟合圆.cpp
    1.5KB
内容介绍
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 }
评论
    相关推荐