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

输入一个三维点的数组 std::vectorcv::Point3f Points3ds;
找到一个平面
Z=Ax+By+C
根据最小二乘法，使各个点到这个平面的距离最近：
S=∑(Ax_i + By_i + C - Z_i) ²
求使得S最小的ABC的数值
首先取得最小值时，对各参数偏导数为零。
$\{\begin{array}{l}\frac{\partial S}{\partial A}=0\\ \\ \frac{\partial S}{\partial B}=0\\ \\ \frac{\partial S}{\partial C}=0\end{array}$

求偏导：

$\{\begin{array}{l}A\sum x_i^2+B\sum y_i{x}_i+C\sum x_i=\sum z_i{x}_i\\ \\ A\sum x_i{y}_i+B\sum y_i{}^2+C\sum y_i=\sum z_i{y}_i\\ \\ A\sum x_i+B\sum y_i+Cn=\sum z_i\end{array}$
整理得

$\{\begin{array}{l}\sum 2\left(A{x}_i+B{y}_i+C-z_i\right)x_i=0\\ \\ \sum 2\left(A{x}_i+B{y}_i+C-z_i\right)y_i=0\\ \\ \sum 2\left(A{x}_i+B{y}_i+C-z_i\right)=0\end{array}$
写为矩阵方程

$\left|\begin{array}{ccc}\sum x_i^2& \sum x_i{y}_i& \sum x_i\\ & & \\ \sum x_i{y}_i& \sum y_i^2& \sum y_i\\ & & \\ \sum x_i& \sum y_i& n\end{array}\right|\cdot \left|\begin{array}{l}A\\ \\ B\\ \\ C\end{array}\right|=\left|\begin{array}{c}\sum z_i{x}_i\\ \\ \sum z_i{y}_i\\ \\ \sum z_i\end{array}\right|$
矩阵方程A$\cdot$x=b
A是参数矩阵，不是上面多项式的参数，b是向量
通解x=A^-1$\cdot$b
得到参数向量
$\left|\begin{array}{l}A\\ B\\ C\end{array}\right|$
代码如下：

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;
}