c++ 最小2乘法代碼

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
}