输入一个三维点的数组
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 }
