輸入一個三維點的陣列 

std::vectorcv::Point3f Points3ds;

找到一個平面

Z=Ax+By+C
根據最小二乘法，使各個點到這個平面的距離最近：

 求使得S最小的ABC的數值
首先取得最小值時，對各引數偏導數為零。

 程式碼如下：

 1 void CaculateLaserPlane(std::vector<cv::Point3f> Points3ds,vector<double> &res)
 2 {
 3     //最小二乘法擬合平面
 4     //獲取cv::Mat的座標系以縱向為x軸，橫向為y軸，而cvPoint等則相反
 
 5     //係數矩陣
 6     cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
 7     //
 8     cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
 9     //結果矩陣
10     cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
11     double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
12     for (int i = 0; i < Points3ds.size(); i++)
 
13     {
14         x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
15         y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
16         xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
17         xi += (double)Points3ds[i].x;
18         yi += (double)Points3ds[i].y;
19         zixi += (double
 
)Points3ds[i].z * (double)Points3ds[i].x;
20         ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
21         zi += (double)Points3ds[i].z;
22     }
23     A.at<double>(0, 0) = x2;
24     A.at<double>(1, 0) = xiyi;
25     A.at<double>(2, 0) = xi;
26     A.at<double>(0, 1) = xiyi;
27     A.at<double>(1, 1) = y2;
28     A.at<double>(2, 1) = yi;
29     A.at<double>(0, 2) = xi;
30     A.at<double>(1, 2) = yi;
31     A.at<double>(2, 2) = Points3ds.size();
32     B.at<double>(0, 0) = zixi;
33     B.at<double>(1, 0) = ziyi;
34     B.at<double>(2, 0) = zi;
35     //計算平面係數
36     X = A.inv() * B;
37     //A
38     res.push_back(X.at<double>(0, 0));
39     //B
40     res.push_back(X.at<double>(1, 0));
41     //Z的係數為1
42     res.push_back(1.0);
43     //C
44     res.push_back(X.at<double>(2, 0));
45     return;
46 }