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

最小二乘拟合算法

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== "<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; cdata.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; rdata.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; cdata.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];//  Z的坐标值}float plane12[4] = { 0 };//定义用来储存平面参数的数组 cvFitPlane(points_mat, plane12);//调用方程 

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

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！