影象演算法——特徵擬合之平面擬合
阿新 • • 發佈：2018-11-23
最小二乘擬合算法
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);//呼叫方程
有機會，再自己寫寫具有魯棒性的最小二乘擬合平面演算法