最小二乘法拟合平面，附C++结合OpenCV的实现代码

问题：

给定n个三维点的坐标，根据这些点坐标由最小二乘法拟合平面。

分析推导：

平面一般方程为ax+by+cy+d=0；

这里为了便于描述，将平面方程变形为：z=Ax+By+C(相当于令上述平面一般方程中的c=-1，a=A，b=B，d=C)。因为已知了n个三维点的坐标，所以理想情况下将各点坐标代入平面方程中构建一个方程组，求解方程组即可。

但是由于n个三维点的测量误差，点不一定在平面上，所以上述方程组是无解的。因此，采用最小二乘法求解上述线性方程组

下面对方程z=Ax+By+C根据最小二乘法求解其中的系数A,B,C。

构建方程的目标函数

求解A,B,C使得损失函数J最小，采用微积分中的最小二乘法，即对A,B,C分别求偏导，令其偏导均为0，如下所示：

将上述方程组进行展开、变形，得到一个未知量为A,B,C的线性方程组：

构建上述方程组的系数矩阵Coeff，以及方程右边的常数向量v：

由此构建了形如的线性方程组，，由此即可计算得到方程组的解。

关于最小二乘法是否有解，以及最小二乘法的解是否为全局最优解，参考这篇文章的介绍：https://www.zhihu.com/question/427449730

上面完成了最小二乘法求解平面方程的理论推导，下面进行代码实现: C++结合OpenCV实现矩阵运算

cv::Vec4f fitplane(vector<cv::Point3f> ptsvec)
{
	int n = ptsvec.size();
	cv::Mat input_pts(n, 3, CV_32FC1);
	for (int i = 0; i < n; i++)
	{
		input_pts.at<float>(i, 0) = ptsvec[i].x;
		input_pts.at<float>(i, 1) = ptsvec[i].y;
		input_pts.at<float>(i, 2) = ptsvec[i].z;
	}
	double A11 = 0, A12 = 0, A13 = 0;
	double A21 = 0, A22 = 0, A23 = 0;
	double A31 = 0, A32 = 0, A33 = 0;
	double B1 = 0, B2 = 0, B3 = 0;
	for (int i = 0; i < n; i++)
	{
		A11 += input_pts.at<float>(i, 0)*input_pts.at<float>(i, 0);
		A12 += input_pts.at<float>(i, 0)*input_pts.at<float>(i, 1);
		A13 += input_pts.at<float>(i, 0);
		A21 += input_pts.at<float>(i, 0)*input_pts.at<float>(i, 1);
		A22 += input_pts.at<float>(i, 1)*input_pts.at<float>(i, 1);
		A23 += input_pts.at<float>(i, 1);
		A31 += input_pts.at<float>(i, 0);
		A32 += input_pts.at<float>(i, 1);
		B1 += input_pts.at<float>(i, 0)*input_pts.at<float>(i, 2);
		B2 += input_pts.at<float>(i, 1)*input_pts.at<float>(i, 2);
		B3 += input_pts.at<float>(i, 2);
	}
	A33 = n;

	cv::Mat A(3, 3, CV_32FC1);
	A.at<float>(0, 0) = A11; A.at<float>(0, 1) = A12; A.at<float>(0, 2) = A13;
	A.at<float>(1, 0) = A21; A.at<float>(1, 1) = A22; A.at<float>(1, 2) = A23;
	A.at<float>(2, 0) = A31; A.at<float>(2, 1) = A32; A.at<float>(2, 2) = A33;
	cv::Mat B(3, 1, CV_32FC1);
	B.at<float>(0, 0) = B1; B.at<float>(1, 0) = B2; B.at<float>(2, 0) = B3;
	cv::Mat X;
	X = A.inv()*B;//X为3*1解向量，分别对应平面方程z=ax+by+c中的abc
	cv::Vec4f v;
	v[0] = X.at<float>(0, 0);
	v[1] = X.at<float>(1, 0);
	v[2] = X.at<float>(2, 0);
	return v;
}

随机给一组点进行测试：（该点为三维坐标系中Oxy平面附近的一组点，坐标的z值在0附近小范围波动，拟合的平面法向量应该近似等于(0,0,1)）。

int main()
{
	vector<cv::Point3f> pts;
	//pts.resize(5);
	pts.push_back(cv::Point3f(10.1, 20.5, 0.12));
	pts.push_back(cv::Point3f(15.1, 34.5, 0.1));
	pts.push_back(cv::Point3f(13.1, 7.5, -0.05));
	pts.push_back(cv::Point3f(10.1, 25.5, 0.03));
	pts.push_back(cv::Point3f(14.1, 10.5, 0.1));
	pts.push_back(cv::Point3f(16.1, 40.5, 0.2));
	pts.push_back(cv::Point3f(32.1, 10.5, -0.2));
	cv::Vec4f v = fitplane(pts);
    cout << "coeff: "<< v[0] << ", "<< v[1] <<", " << v[2] << endl;
	return 0; 
}

程序运行后的打印结果如下图：