图像算法——特征拟合之平面拟合

最小二乘拟合算法

typedef struct
{
	double r0;
	double r1;
	double r2;
	double distB;    //used in distance caculating
}RATIO_Plane;

typedef struct
{
	float xxx;
	float yyy;
	float zzz;
}roiPointDecimal3D;

int fitPlane3D(const roiPointDecimal3D *point, int pNum, RATIO_Plane *plane3D)
{
	/*平面方程式：z=r0*x+r1*y+r2*/
	double sum_xx = 0;
	double sum_xy = 0;
	double sum_yy = 0;
	double sum_xz = 0;
	double sum_yz = 0;
	double sum_x = 0;
	double sum_y = 0;
	double sum_z = 0;
	double mean_xx, mean_yy, mean_xy, mean_yz, mean_xz, mean_x, mean_y, mean_z;
	double a[4];
	double b[4];
	double c[4];
	double d[4];

	double D1, D2, D3, DD;
	int i;

	double effSize = 0;
	for (i = 0; i < pNum; i++)
	{
		if (!point[i].zzz)
		{
			continue;
		}
		sum_xx += (double)((point[i].xxx)*(point[i].xxx));
		sum_xy += (double)((point[i].xxx)*(point[i].yyy));
		sum_yy += (double)((point[i].yyy)*(point[i].yyy));
		sum_yz += (double)((point[i].yyy)*(point[i].zzz));
		sum_xz += (double)((point[i].xxx)*(point[i].zzz));
		sum_x += (double)((point[i].xxx));
		sum_y += (double)((point[i].yyy));
		sum_z += (double)((point[i].zzz));
		effSize += 1.0;
		//qDebug()<<"new x== "<<point[i].x<<"new y"<<point[i].y<<"new z"<<point[i].z;
	}
	mean_xx = sum_xx / effSize;
	mean_xy = sum_xy / effSize;
	mean_yy = sum_yy / effSize;
	mean_yz = sum_yz / effSize;
	mean_xz = sum_xz / effSize;
	mean_x = sum_x / effSize;
	mean_y = sum_y / effSize;
	mean_z = sum_z / effSize;
	a[1] = sum_xx;
	a[2] = sum_xy;
	a[3] = sum_x;
	b[1] = sum_xy;
	b[2] = sum_yy;
	b[3] = sum_y;
	c[1] = sum_x;
	c[2] = sum_y;
	c[3] = effSize;
	d[1] = sum_xz;
	d[2] = sum_yz;
	d[3] = sum_z;


	D1 = (b[2] * ((d[1] * c[3]) - (c[1] * d[3]))) + (b[1] * ((c[2] * d[3]) - (d[2] * c[3]))) + (b[3] * ((d[2] * c[1]) - (c[2] * d[1])));
	D2 = (d[2] * ((a[1] * c[3]) - (c[1] * a[3]))) + (d[1] * ((c[2] * a[3]) - (a[2] * c[3]))) + (d[3] * ((a[2] * c[1]) - (c[2] * a[1])));
	D3 = (b[2] * ((a[1] * d[3]) - (d[1] * a[3]))) + (b[1] * ((d[2] * a[3]) - (a[2] * d[3]))) + (b[3] * ((a[2] * d[1]) - (d[2] * a[1])));

	DD = (b[2] * ((a[1] * c[3]) - (c[1] * a[3]))) + (b[1] * ((c[2] * a[3]) - (a[2] * c[3]))) + (b[3] * ((a[2] * c[1]) - (c[2] * a[1])));

	plane3D->r0 = D1 / DD;
	plane3D->r1 = D2 / DD;
	plane3D->r2 = D3 / DD;

	plane3D->distB = sqrt(plane3D->r0*plane3D->r0 + plane3D->r1*plane3D->r1 + 1.0);
	return 0;
}

 借鉴一篇https://blog.csdn.net/zhouyelihua/article/details/46122977

//Ax+by+cz=D
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.
	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(¢roid);
	cvReleaseMat(&points2);
	cvReleaseMat(&A);
	cvReleaseMat(&W);
	cvReleaseMat(&V);
}
//引用方法
CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵 
		for (int i=0;i < X_vector.size(); ++i)
		{
			points_mat->data.fl[i*3+0] = X_vector[i];//矩阵的值进行初始化   X的坐标值
			points_mat->data.fl[i * 3 + 1] = Y_vector[i];//  Y的坐标值
			points_mat->data.fl[i * 3 + 2] = Z_vector[i];<span style="font-family: Arial, Helvetica, sans-serif;">//  Z的坐标值</span>
 
		}
		float plane12[4] = { 0 };//定义用来储存平面参数的数组 
		cvFitPlane(points_mat, plane12);//调用方程 

 有机会，再自己写写具有鲁棒性的最小二乘拟合平面算法