Opencv学习笔记 - 使用最小二乘法拟合平面-优快云博客

这篇博客介绍了如何使用最小二乘法进行平面拟合，通过实例解释了最小二乘法的思想，并对比了OpenCV与MATLAB中拟合平面的公式。博主验证了两种方法得到的平面参数一致，同时提供了计算点到平面距离的流程和MATLAB代码。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

最小二乘法简述

最小二乘法(Least Squares method)，又称最小平方法，起源于十八世纪的大航海探索时期，发展于天文领域和航海领域。

以一个简单的例子，介绍最小二乘法的思想。

假设下面这20个数据是你用同一把尺子测量20次水杯高度的数据，每次的测量都是有误差的，那么水杯的高度到底是多少呢？

一般情况下我们可以用数据的平均值作为水杯的高度。

如果用最小二乘法来思考刚才的问题。先将上面的数据画在坐标系中，如下图所示，其中红色虚线为水杯高度的估计值。

则这20次测量的残差为：，由于绝对值计算起来比较麻烦，可以用残差的平方替代绝对值，则总（平方）误差为：，当估计值变化时，总误差也会变化。最小二乘法的思路是：让总误差最小的就是最小二乘估计（Least Squares Estimation）。上面的公式当其导数为0时取最小值，，可求得令总误差最小的值是10.0015。

本文主要验证

本文主要验证了博客上的最小二乘法拟合平面的。与用matlab拟合出来的平面计算的点到直线的距离是一样的，而且系数也是一样的。说明了本方法的可行性。

matlab中公式为z = c + ax +by

oepncv中公式为Ax+By+Cz=D 将opencv中公式换算成matlab的公式，系数是一样的。

平面公式为：Ax+By+Cz=D

代码来自于：https://blog.youkuaiyun.com/laobai1015/article/details/73603327?utm_source=blogxgwz4

//对应的方程：Ax+By+Cz=D 其中 A = plane12[0], B = plane12[1], C = plane12[2], D = plane12[3],这是要注意的方程的表示
//float plane12[4] = { 0 };//定义用来储存平面参数的数组，分别对应ABCD

拟合平面

void cvFitPlane(vector<float>dx, vector<float>dy, vector<float>dz, float* plane) {

	//直线方程为：
	//构建点集cvmat
	CvMat* points = cvCreateMat(dx.size(), 3, CV_32FC1);
	int nnum = 96;
	for (int i = 0; i < dx.size(); ++i)
	{
		points->data.fl[i * 3 + 0] = dx[i];//矩阵的值进行初始化   X的坐标值  
		points->data.fl[i * 3 + 1] = dy[i];//  Y的坐标值  
		points->data.fl[i * 3 + 2] = dz[i]; //  Z的坐标值

	}

	 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.  
	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);

	// 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];
	}
	// Release allocated resources.  
	cvReleaseMat(&points);
	cvReleaseMat(&centroid);
	cvReleaseMat(&points2);
	cvReleaseMat(&A);
	cvReleaseMat(&W);
	cvReleaseMat(&V);
}

计算点到平面的距离

//计算点到平面的距离
//Ax+By+Cz=D
//|点（a,b,c） 到平面bai Ax+By+Cz=D的距离du

//= | A * a + B * b + C * c - D| /√(A ^ 2 + B ^ 2 + C ^ 2)
void calculateDist(vector<float>dx, vector<float>dy, vector<float>dz, float* plane, vector<float> &dist)
{
	for (int i = 0; i < dx.size(); i++)
	{
		float ds = fabs(plane[0] * dx[i] + plane[1] * dy[i] + plane[2] * dz[i] - plane[3]);
		float dfen = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);
		if (!(dfen > -0.00001 && dfen < -0.00001))
		{
			float ddist = ds / dfen;
			dist.push_back(ddist);
		}		
	}
}

测试流程

从文件中读取数据，然后计算拟合平面，计算点到平面的距离，并输出到csv文件中
数据格式为：
一行中代表xyz

-53.883533,55.133049,895.801941
-40.928612,32.402653,897.237793
-21.391739,50.161041,899.748901
2.107507,62.850151,902.479065
3.594930,37.490810,902.427490

fstream fs;
	fs.open("E:\\wokspace\\PROJECT\\ThirdTrailInspection\\matlab\\dResult.txt");
	if (!fs.is_open())
	{
		return;
	}
	vector<float> dx;
	vector<float> dy;
	vector<float> dz;
	int i = 0;
	string buff;
	while (getline(fs, buff))//是否到文件结bai尾
	{
		int nfist = buff.find_first_of(',');
		int nLast = buff.find_last_of(',');
		string st1 = buff.substr(0, nfist);
		string st2 = buff.substr(nfist + 1, nLast - nfist - 1);
		string st3 =(buff.substr(nLast + 1));
		dx.push_back(stof(buff.substr(0, nfist)));
		dy.push_back(stof(buff.substr(nfist + 1, nLast - nfist - 1)));
		dz.push_back(stof(buff.substr(nLast + 1)));		
	}
	fs.close();


	//代入最小二乘算法中
	float plane[4] = { 0 };
	vector<float> dx1;
	vector<float> dy1;
	vector<float> dz1;
	dx1.assign(dx.begin(), dx.begin() + 96);
	dy1.assign(dy.begin(), dy.begin() + 96);
	dz1.assign(dz.begin(), dz.begin() + 96);
	cvFitPlane(dx1, dy1, dz1,  plane);
	vector<float> dist;
	calculateDist(dx, dy, dz, plane, dist);
	fstream fws("e://de.csv", fstream::in | fstream::out | fstream::trunc);
	for (int i = 0; i < dist.size(); i++)
	{
		fws << dist[i] <<"\r";
	}
	fws.close();

对应的matlab代码

clc;
close all;
clear all;
%https://www.ilovematlab.cn/thread-220252-1-1.html

data = importdata('E:\wokspace\PROJECT\ThirdTrailInspection\matlab\dResult.txt');
x = data(1:96, 1);
y = data(1:96, 2);
z = data(1:96, 3);
% x = data(113:192, 1);
% y = data(113:192, 2);
% z = data(113:192, 3);
scatter3(x, y,z, 'r');%画点  散点图
hold on;
X = [ones(length(x),1) x y];
[b,bint,r,rint,stats] = regress(z,X,95);

% 图形绘制
xfit = min(x):0.1:max(x);
yfit = min(y):0.1:max(y);
[XFIT,YFIT]= meshgrid (xfit,yfit);%用于生成网格采样点
ZFIT = b(1) + b(2) * XFIT + b(3) * YFIT;
mesh(XFIT,YFIT,ZFIT);

%%测试结果
%data = importdata('C:\Users\apr_z\Desktop\dResult.txt');
xx = data(:, 1);
yy = data(:, 2);
zz = data(:, 3);
[row, col] = size(xx);%求矩阵的行数和列数
dist = ones(row, 1);
for i = 1: row
    dist(i) = abs(b(2) * xx(i) + b(3) * yy(i)  - zz(i) + b(1)) / sqrt(b(2)* b(2) + b(3)*b(3) + 1 );
end
xlswrite('C:\Users\apr_z\Desktop\AnalyzeResult.xlsx', dist);

小结：
vector 复制某一些数据时： dx1.assign(dx.begin(), dx.begin() + 96);

vector容器追加其他容器的内容，使用insert
pt3DList.insert(pt3DList.end(), vc.begin(), vc.end());

用此平面拟合的计算点到直线的距离与用matlab计算出来的点到直线的距离是一模一样的。

移动最小二乘法（MLS）曲线曲面拟合

原文链接：移动最小二乘法（MLS）曲线曲面拟合C++代码实现_残影丶的博客-优快云博客_曲面拟合

//移动最小二乘法的具体计算过程，参照论文“基于移动最小二乘法的曲线曲面拟合”，AB矩阵参照论文“移动最小二乘法的研究”
int MLS_Calc(int x_val,int y_val,float x[],float y[],float z[])
========================================2019-03-05============================
这里有些错误。三维曲面拟合是以二维（x，y）为基函数的，因此这里只是（x），错了。可以去搜代码去看，一维二维基函数都有。↓↓
{
	int max_delta=max_x-min_x;//区域半径
	float p[M][N]={0};
	float sumf[N][N]={0};
	float w[M]={0};
    for(int j=0;j<M;j++)//求w
	{
        float s=fabs((x[j]-x_val))/max_delta;
        if(s<=0.5)
            w[j]=2/3.0-4*s*s+4*s*s*s;
        else
		{
            if(s<=1)
                w[j]=4/3.0-4*s+4*s*s-4*s*s*s/3.0;
            else
                w[j]=0;
		}
		p[j][0]=1;//每个采样点计算基函数
		p[j][1]=x[j];
		p[j][2]=y[j];
		p[j][3]=x[j]*x[j];
		p[j][4]=x[j]*y[j];
		p[j][5]=y[j]*y[j];
	}
 	f(w,x,y,sumf,p);//计算得出A矩阵
	
	float p1[N];
	Matrix A=Trans_Matrix(sumf,N);
	Matrix A_1=m_c.Matrix_copy(&A);
	m_c.Matrix_inv(&A_1);//求A矩阵的逆A_1

	Matrix B(N,1);//求矩阵B，N行M列
	B.init_Matrix();
	for(int j=0;j<M;j++)//求得B矩阵的每列
	{
		p1[0]=1*w[j];
		p1[1]=x[j]*w[j];
		p1[2]=y[j]*w[j];
		p1[3]=x[j]*x[j]*w[j];
		p1[4]=x[j]*y[j]*w[j];
		p1[5]=y[j]*y[j]*w[j];
		Matrix P=Trans_Matrix_One(p1,N);//数组P1转成1行N列的P矩阵
		if(j==0)//第一列直接赋值
		{
			for(int i=0;i<N;i++)
				B.write(i,0,p1[i]);
		}
		else
		{
			m_c.Matrix_trans(&P);//矩阵转置，P转为N行1列矩阵
			m_c.Matrix_addCols(&B,&P);//矩阵B列附加，形成N行M列矩阵
		}
		P.free_Matrix();
	}

	float D[N]={1,x_val,y_val,x_val*x_val,x_val*y_val,y_val*y_val};
	Matrix D1=Trans_Matrix_One(D,N);//转成1行N列矩阵

	Matrix D_A1_mul(1,N);//定义矩阵并初始化相乘的结果矩阵，1行N列
	D_A1_mul.init_Matrix();
	if(m_c.Matrix_mul(&D1,&A_1,&D_A1_mul)==-1)
		cout<<"矩阵有误1！";//1行N列矩阵乘以N行N列矩阵得到结果为1行N列
	
	Matrix D_A1_B_mul(1,M);//定义矩阵并初始化相乘的结果矩阵，1行M列
	D_A1_B_mul.init_Matrix();
	if(m_c.Matrix_mul(&D_A1_mul,&B,&D_A1_B_mul)==-1)
		cout<<"矩阵有误2";//1行N列矩阵乘以N行M列矩阵得到记过矩阵为1行M列

	Matrix z1=Trans_Matrix_One(z,M);//将数组z转换成1行M列矩阵
	m_c.Matrix_trans(&z1);//转置得到M行1列矩阵
	Matrix Z(1,1);//得到矩阵结果，1行1列
	Z.init_Matrix();
	if(m_c.Matrix_mul(&D_A1_B_mul,&z1,&Z)==-1)
		cout<<"矩阵有误3！";//1行M列矩阵乘以M行1列矩阵得到1行1列矩阵，即值Z
	
	float z_val=Z.read(0,0);
	if(z_val>255)
		z_val=255;
	if(z_val<0)
		z_val=0;

	A.free_Matrix();
	A_1.free_Matrix();
	B.free_Matrix();
	
	D1.free_Matrix();
	D_A1_mul.free_Matrix();
	D_A1_B_mul.free_Matrix();
	z1.free_Matrix();
	Z.free_Matrix();

	return (int)z_val;
}