三维空间点进行空间平面拟合原理及MATLAB和C++代码实现

平面拟合原理参考网页：https://blog.csdn.net/duiwangxiaomi/article/details/89246715

MATLAB实现参考网页：https://blog.csdn.net/duiwangxiaomi/article/details/89238969

C++代码实现，包括测试数据，vs2013结合OpenCV2.4.13：

#include <iostream>
#include <highgui.h>

using namespace std;
using namespace cv;

//AX+BY+CZ+D=0
void cvFitPlane(const CvMat* points, float* plane){
	// Estimate geometric centroid.
	int nrows = points->rows;
	int ncols = points->cols;
	int type = points->type;
	CvMat* centroid = cvCreateMat(1, ncols, type);
	cvSet(centroid, cvScalar(0));
	for (int c = 0; c < ncols; c++){
		for (int r = 0; r < nrows; r++)
		{
			centroid->data.fl[c] += points->data.fl[ncols*r + c];
		}
		centroid->data.fl[c] /= nrows;
	}
	// Subtract geometric centroid from each point.  points2存放的是各个点减去几何重心的值
	CvMat* points2 = cvCreateMat(nrows, ncols, type);
	for (int r = 0; r < nrows; r++)
		for (int c = 0; c < ncols; c++)
			points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];
	// Evaluate SVD of covariance matrix.
	CvMat* A = cvCreateMat(ncols, ncols, type);
	CvMat* W = cvCreateMat(ncols, ncols, type);
	CvMat* V = cvCreateMat(ncols, ncols, type);
	cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);
	cvSVD(A, W, NULL, V, CV_SVD_V_T);
	Mat A1(A);
	cout << "A: " << A1<<endl;
	Mat V1(V);
	cout << "V: "<<V1<<endl;
	// Assign plane coefficients by singular vector corresponding to smallest singular value.
	plane[ncols] = 0;
	for (int c = 0; c < ncols; c++){
		plane[c] = V->data.fl[ncols*(ncols - 1) + c];
		plane[ncols] += plane[c] * centroid->data.fl[c];
	}
	plane[ncols] = -plane[ncols];
	// Release allocated resources.
	//cvReleaseMat(?roid);
	cvReleaseMat(&points2);
	cvReleaseMat(&A);
	cvReleaseMat(&W);
	cvReleaseMat(&V);
}

int main()
{
	//float x0 = 1,L1 = 2,y0 = 1,	L2 = 2, x1,y1,z1;
	//vector<float> x, y, z;
	//for (int i = 0; i < 20;++i)
	//{
	//	x1 = x0 + rand()*L1;
	//	y1 = y0 + rand()*L2;
	//	z1 = 1 + 2 * x1 + 3 * y1;
	//	x.push_back(x1);
	//	y.push_back(y1);
	//	z.push_back(z1);
	//}
	Mat point3D = (Mat_<float>(20, 3) << 2.62944737278636, 2.31148139831317, 13.1933389405122, 2.81158387415124, 1.07142335714838, 9.83743781974761,
		1.25397363258701, 2.69825861173755, 11.6027231003867, 2.82675171227804, 2.86798649551510, 15.2574629111014,
		2.26471849245082, 2.35747030971555, 12.6018479140483, 1.19508080999882, 2.51548026115667, 10.9366024034676,
		1.55699643773410, 2.48626493624983, 11.5727876842177, 2.09376303840997, 1.78445403906834, 10.5408881940249,
		2.91501367086860, 2.31095578035511, 13.7628946828025, 2.92977707039855, 1.34237337562312, 10.8866742676665,
		1.31522616335510, 2.41209217603922, 10.8667288548278, 2.94118556352123, 1.06366569275484, 10.0733682053070,
		2.91433389648589, 1.55384596992178, 11.4902057027371, 1.97075129744568, 1.09234278126231, 8.21853093867829,
		2.60056093777760, 1.19426356247170, 9.78391256297029, 1.28377267725443, 2.64691565665459, 11.5082923244726,
		1.84352256525255, 2.38965724595163, 11.8560168683600, 2.83147105037813, 1.63419896012172, 11.5655389811214,
		2.58441465911911, 2.90044409767671, 14.8701616112683, 2.91898485278581, 1.06889216100582, 10.0446461885891);
	vector<Point3f> points;
	points = Mat_<Point3f>(point3D);
	CvMat* points_mat = cvCreateMat(points.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵 
	for (int i = 0; i < points.size(); ++i)
	{
		points_mat->data.fl[i * 3 + 0] = points[i].x;//矩阵的值进行初始化   X的坐标值
		points_mat->data.fl[i * 3 + 1] = points[i].y;//  Y的坐标值
		points_mat->data.fl[i * 3 + 2] = points[i].z;// < span style = "font-family: Arial, Helvetica, sans-serif;" >//  Z的坐标值</span>

	}
	float plane12[4] = { 0 };//定义用来储存平面参数的数组 
	cvFitPlane(points_mat, plane12);//调用方程 
}

对应的MATLAB代码为：

% 随机生成一组（x,y,z),这些点的坐标离一个空间平面比较近
x0=1;
L1=2;
y0=1;
L2=2;
x=x0+rand(20,1)*L1;
y=y0+rand(20,1)*L2;
z=1+2*x+3*y;
scatter3(x,y,z,'filled')
hold on;

planeData=[x,y,z];

% 协方差矩阵的SVD变换中，最小奇异值对应的奇异向量就是平面的方向
xyz0=mean(planeData,1);
centeredPlane=bsxfun(@minus,planeData,xyz0);
A=centeredPlane'*planeData;
[U,S,V]=svd(A);
%[U,S,V]=svd(centeredPlane);

a=V(1,3);
b=V(2,3);
c=V(3,3);
d=-dot([a b c],xyz0);

% 图形绘制
xfit = min(x):0.1:max(x);
yfit = min(y):0.1:max(y);
[XFIT,YFIT]= meshgrid (xfit,yfit);
ZFIT = -(d + a * XFIT + b * YFIT)/c;
mesh(XFIT,YFIT,ZFIT);