The least square method is used to fit the plane ., Least multiplication plane

The least square method is used to fit the plane ., Least multiplication plane

This article uses some opencv functions to fit the plane.

//Ax+by+cz=Dvoid 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);}

Call method:

CvMat * points_mat = cvCreateMat (X_vector.size (), 3, CV_32FC1); // defines the matrix used to store the points to fit (int I = 0; I <X_vector.size (); ++ I) {points_mat-> data. fl [I * 3 + 0] = X_vector [I]; // initialize the coordinate value of X for Matrix value points_mat-> data. fl [I * 3 + 1] = Y_vector [I]; // coordinate value of Y points_mat-> data. fl [I * 3 + 2] = Z_vector [I]; <span style = "font-family: Arial, Helvetica, sans-serif; "> // Z coordinate value </span>} float plane12 [4] = {0}; // defines the array cvFitPlane (points_mat, plane12) used to store plane parameters ); // call the equation

The fitting equation is Ax + By + Cz = D.

Where A = plane12 [0], B = plane12 [1], C = plane12 [2], D = plane12 [3],

This is the expression of the equation.