版权声明: https://blog.csdn.net/qq_25147107/article/details/83790347
最小二乘拟合算法
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);//调用方程
有机会,再自己写写具有鲁棒性的最小二乘拟合平面算法