OpenCV 最小二乘法拟合空间平面

输入一个三维点的数组 

std::vectorcv::Point3f Points3ds;

找到一个平面

Z=Ax+By+C
根据最小二乘法，使各个点到这个平面的距离最近：

 求使得S最小的ABC的数值
首先取得最小值时，对各参数偏导数为零。

 代码如下：

void CaculateLaserPlane(std::vector<cv::Point3f> Points3ds,vector<double> &res)
{
    //最小二乘法拟合平面
    //获取cv::Mat的坐标系以纵向为x轴，横向为y轴，而cvPoint等则相反
    //系数矩阵
    cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
    //
    cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
    //结果矩阵
    cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
    double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
    for (int i = 0; i < Points3ds.size(); i++)
    {
        x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
        y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
        xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
        xi += (double)Points3ds[i].x;
        yi += (double)Points3ds[i].y;
        zixi += (double)Points3ds[i].z * (double)Points3ds[i].x;
        ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
        zi += (double)Points3ds[i].z;
    }
    A.at<double>(0, 0) = x2;
    A.at<double>(1, 0) = xiyi;
    A.at<double>(2, 0) = xi;
    A.at<double>(0, 1) = xiyi;
    A.at<double>(1, 1) = y2;
    A.at<double>(2, 1) = yi;
    A.at<double>(0, 2) = xi;
    A.at<double>(1, 2) = yi;
    A.at<double>(2, 2) = Points3ds.size();
    B.at<double>(0, 0) = zixi;
    B.at<double>(1, 0) = ziyi;
    B.at<double>(2, 0) = zi;
    //计算平面系数
    X = A.inv() * B;
    //A
    res.push_back(X.at<double>(0, 0));
    //B
    res.push_back(X.at<double>(1, 0));
    //Z的系数为1
    res.push_back(1.0);
    //C
    res.push_back(X.at<double>(2, 0));
    return;
}