直接上代码(部分理论可以参考：https://blog.csdn.net/u013541523/article/details/80135568)，该链接为了推导介绍方便，只考虑了1种z=ax+by+d的情况，实际代码情况有3种情况，否则某些合理的点会导致逆矩阵为0求不出平面参数。
ModelPointXYZFloat.h
class ModelPointXYZFloat { public: ModelPointXYZFloat(); virtual ~ModelPointXYZFloat(); //unit:m float x_; float y_; float z_; };ModelPointXYZFloat.cpp
#include "ModelPointXYZFloat.h" ModelPointXYZFloat::ModelPointXYZFloat() { // TODO Auto-generated constructor stub x_ = 0; y_ = 0; z_ = 0; } ModelPointXYZFloat::~ModelPointXYZFloat() { // TODO Auto-generated destructor stub }LeastSquaresFitLine.h
#ifdef X86 #include "ModelPointXYZFloat.h" #else #include "src/ModelPointXYZFloat.h" #endif #include <vector> class LeastSquaresFitPlane { public: LeastSquaresFitPlane(); virtual ~LeastSquaresFitPlane(); //ax+by+cz+d = 0;(a,b,c为法线) static bool LeastSquaresFitPlane3D(const std::vector<ModelPointXYZFloat>& points, double& a, double& b, double& c, double& d); private: //z = ax+by+d static bool LeastSquaresFitPlaneZ(double(*arr)[3], double* val ,double& a, double& b, double& c, double& d); //y = ax+bz+d static bool LeastSquaresFitPlaneY(double(*arr)[3], double* val, double& a, double& b, double& c, double& d); //x = ay+bz+d static bool LeastSquaresFitPlaneX(double(*arr)[3], double* val, double& a, double& b, double& c, double& d); };LeastSquaresFitPlane.cpp
/* * LeastSquaresFitPlane.cpp * * Created on: 2020年9月3日 * Author: zzb */ #include "LeastSquaresFitPlane.h" #include <math.h> LeastSquaresFitPlane::LeastSquaresFitPlane() { // TODO Auto-generated constructor stub } LeastSquaresFitPlane::~LeastSquaresFitPlane() { // TODO Auto-generated destructor stub } bool LeastSquaresFitPlane::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_; } int 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 LeastSquaresFitPlane::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 LeastSquaresFitPlane::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 LeastSquaresFitPlane::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; }