分享最小2乘法C++代码，忘记源码再哪里看到了，这里根据我的实际情况分享一下
struct ModelPointXYZFloat { // unit:m double x_; double y_; double z_; }; struct Mini2MatParam { double a = 1; double b = 1; double c = 1; double d = 1; }; bool GetXYZDieListMini2Mat(const QList<QVector3D>& listFocus, Mini2MatParam& Pa) { std::vector<ModelPointXYZFloat> points; for (int i = 0; i < listFocus.size(); ++i) { ModelPointXYZFloat point; point.x_ = listFocus[i].x(); point.y_ = listFocus[i].y(); point.z_ = listFocus[i].z(); points.push_back(point); } double a; double b; double c; double d; LeastSquaresFitPlane3D(points, a, b, c, d); // ax+by+cz+d = 0;(a,b,c为法线) z = -1/c*(ax+by+d) // qDebug()<<a<<b<<c<<d; Pa.a = a; Pa.b = b; Pa.c = c; Pa.d = d; return true; } ////////////////////// C++ 最小2乘法拟合平面 开始 /////////////////////////////// bool LeastSquaresFitPlaneZ(double (*arr)[3], double* val, double& a, double& b, double& c, double& d) { double arr_r = arr[0][0] * arr[1][1] * arr[2][2] + arr[0][1] * arr[1][2] * arr[2][0] + arr[0][2] * arr[1][0] * arr[2][1] - arr[0][2] * arr[1][1] * arr[2][0] - arr[0][1] * arr[1][0] * arr[2][2] - arr[0][0] * arr[1][2] * arr[2][1]; if (fabs(arr_r) <= 1e-5) { // 行列式值等于0，没有逆矩阵 return false; } // 根据伴随矩阵求逆 double arr_inv[3][3] = { 0 }; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { double marr[2][2] = { 0 }; for (int m = 0, k = 0; m < 3; m++, k++) { if (m == i) { k--; continue; } for (int n = 0, l = 0; n < 3; n++, l++) { if (n == j) { l--; continue; } marr[k][l] = arr[m][n]; } } arr_inv[j][i] = pow(-1, (i + j)) * (marr[0][0] * marr[1][1] - marr[0][1] * marr[1][0]) / arr_r; } } double ret[3] = { 0 }; for (int m = 0; m < 3; m++) { ret[m] = 0; for (int j = 0; j < 3; j++) { ret[m] += arr_inv[m][j] * val[j]; } } a = -ret[0]; b = -ret[1]; c = 1; d = -ret[2]; return true; } bool LeastSquaresFitPlaneY(double (*arr)[3], double* val, double& a, double& b, double& c, double& d) { double arr_r = arr[0][0] * arr[1][1] * arr[2][2] + arr[0][1] * arr[1][2] * arr[2][0] + arr[0][2] * arr[1][0] * arr[2][1] - arr[0][2] * arr[1][1] * arr[2][0] - arr[0][1] * arr[1][0] * arr[2][2] - arr[0][0] * arr[1][2] * arr[2][1]; if (fabs(arr_r) <= 1e-5) { // 行列式值等于0，没有逆矩阵 return false; } // 根据伴随矩阵求逆 double arr_inv[3][3] = { 0 }; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { double marr[2][2] = { 0 }; for (int m = 0, k = 0; m < 3; m++, k++) { if (m == i) { k--; continue; } for (int n = 0, l = 0; n < 3; n++, l++) { if (n == j) { l--; continue; } marr[k][l] = arr[m][n]; } } arr_inv[j][i] = pow(-1, (i + j)) * (marr[0][0] * marr[1][1] - marr[0][1] * marr[1][0]) / arr_r; } } double ret[3] = { 0 }; for (int m = 0; m < 3; m++) { ret[m] = 0; for (int j = 0; j < 3; j++) { ret[m] += arr_inv[m][j] * val[j]; } } a = -ret[0]; b = 1; c = -ret[1]; d = -ret[2]; return true; } bool LeastSquaresFitPlaneX(double (*arr)[3], double* val, double& a, double& b, double& c, double& d) { double arr_r = arr[0][0] * arr[1][1] * arr[2][2] + arr[0][1] * arr[1][2] * arr[2][0] + arr[0][2] * arr[1][0] * arr[2][1] - arr[0][2] * arr[1][1] * arr[2][0] - arr[0][1] * arr[1][0] * arr[2][2] - arr[0][0] * arr[1][2] * arr[2][1]; if (fabs(arr_r) <= 1e-5) { // 行列式值等于0，没有逆矩阵 return false; } // 根据伴随矩阵求逆 double arr_inv[3][3] = { 0 }; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { double marr[2][2] = { 0 }; for (int m = 0, k = 0; m < 3; m++, k++) { if (m == i) { k--; continue; } for (int n = 0, l = 0; n < 3; n++, l++) { if (n == j) { l--; continue; } marr[k][l] = arr[m][n]; } } arr_inv[j][i] = pow(-1, (i + j)) * (marr[0][0] * marr[1][1] - marr[0][1] * marr[1][0]) / arr_r; } } double ret[3] = { 0 }; for (int m = 0; m < 3; m++) { ret[m] = 0; for (int j = 0; j < 3; j++) { ret[m] += arr_inv[m][j] * val[j]; } } a = 1; b = -ret[0]; c = -ret[1]; d = -ret[2]; return true; } bool LeastSquaresFitPlane3D(const std::vector<ModelPointXYZFloat>& points, double& a, double& b, double& c, double& d) { double Mxsq = 0, Mysq = 0, Mzsq = 0, Mxy = 0, Mxz = 0, Myz = 0, Mx = 0, My = 0, Mz = 0; for (unsigned int i = 0; i < points.size(); i++) { Mxsq += pow(points[i].x_, 2); Mysq += pow(points[i].y_, 2); Mzsq += pow(points[i].z_, 2); Mxy += points[i].x_ * points[i].y_; Mxz += points[i].x_ * points[i].z_; Myz += points[i].y_ * points[i].z_; Mx += points[i].x_; My += points[i].y_; Mz += points[i].z_; } double n = points.size(); double arr_z[3][3] = { { Mxsq, Mxy, Mx }, { Mxy, Mysq, My }, { Mx, My, n } }; double val_z[3] = { Mxz, Myz, Mz }; if (LeastSquaresFitPlaneZ(arr_z, val_z, a, b, c, d)) { return true; } double arr_y[3][3] = { { Mxsq, Mxz, Mx }, { Mxz, Mzsq, Mz }, { Mx, Mz, n } }; double val_y[3] = { Mxy, Myz, My }; if (LeastSquaresFitPlaneY(arr_y, val_y, a, b, c, d)) { return true; } double arr_x[3][3] = { { Mysq, Myz, My }, { Myz, Mzsq, Mz }, { My, Mz, n } }; double val_x[3] = { Mxy, Mxz, Mx }; if (LeastSquaresFitPlaneX(arr_x, val_x, a, b, c, d)) { return true; } return false; } bool GetMin2Multi(const QList<st_dist>& listFocus, DieData& lPos) { std::vector<ModelPointXYZFloat> points; for (int i = 0; i < listFocus.size(); ++i) { ModelPointXYZFloat point; point.x_ = listFocus[i].x; point.y_ = listFocus[i].y; point.z_ = listFocus[i].z; points.push_back(point); } double a; double b; double c; double d; LeastSquaresFitPlane3D(points, a, b, c, d); // ax+by+cz+d = 0;(a,b,c为法线) z = -1/c*(ax+by+d) // qDebug()<<a<<b<<c<<d; double zt = -1 / c * (a * lPos.x + b * lPos.y + d); if (_finite(zt)) { lPos.z = zt; return true; } else { qDebug() << "GetMin2Multi value isnan"; return false; } } ////////////////////// C++ 最小2乘法拟合平面 结束 /////////////////////////////// ////////////////////// C++ 最小2乘法拟合直线 开始 /////////////////////////////// bool GetMin2MultiLine(const QList<DieData>& listFocus, double& a, double& b) { // 线性拟合ax+b double xsum, ysum, x2sum, xysum; xsum = 0; ysum = 0; x2sum = 0; xysum = 0; int n = listFocus.count(); for (int i = 0; i < n; i++) { xsum += listFocus.at(i).x_Template; ysum += listFocus.at(i).y_Template; x2sum += listFocus.at(i).x_Template * listFocus.at(i).x_Template; xysum += listFocus.at(i).x_Template * listFocus.at(i).y_Template; } a = (n * xysum - xsum * ysum) / (n * x2sum - xsum * xsum); // a b = (ysum - a * xsum) / n; // b return true; // 计算相关系数 // double yavg = ysum / n; // double dy2sum1 = 0, dy2sum2 = 0; // for (int i = 0; i < n; i++) // { // dy2sum1 += ((abr[0] * x[i] + abr[1]) - yavg)*((abr[0] * x[i] + abr[1]) - yavg);//r^2的分子 // dy2sum2 += (y[i] - yavg)*(y[i] - yavg);//r^2的分母 // } // r = dy2sum1 / dy2sum2;//r^2 }