欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-04-03 23:19:43
...

平面拟合原理参考网页: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);