openCV光平面标定——空间平面拟合

1. //Ax+by+cz=D  
2. void cvFitPlane(const CvMat* points, float* plane){  
3.     // Estimate geometric centroid.  
4.     int nrows = points->rows;  
5.     int ncols = points->cols;  
6.     int type = points->type;  
7.     CvMat* centroid = cvCreateMat(1, ncols, type);  
8.     cvSet(centroid, cvScalar(0));  
9.     for (int c = 0; c<ncols; c++){         //用地址的形式才能实现先列后行的操作，如果用Mat型只能先行后列
10.         for (int r = 0; r < nrows; r++)  
11.         {  
12.             centroid->data.fl[c] += points->data.fl[ncols*r + c];  
13.         }  
14.         centroid->data.fl[c] /= nrows;  
15.     }  
16.     // Subtract geometric centroid from each point.  
17.     CvMat* points2 = cvCreateMat(nrows, ncols, type);  
18.     for (int r = 0; r<nrows; r++)  
19.         for (int c = 0; c<ncols; c++)  
20.             points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];  
21.     // Evaluate SVD of covariance matrix.  
22.     CvMat* A = cvCreateMat(ncols, ncols, type);  
23.     CvMat* W = cvCreateMat(ncols, ncols, type);  
24.     CvMat* V = cvCreateMat(ncols, ncols, type);  
25.     cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);  //points也要为CVMat型
26.     cvSVD(A, W, NULL, V, CV_SVD_V_T);  
27.     // Assign plane coefficients by singular vector corresponding to smallest singular value.  
28.     plane[ncols] = 0;  
29.     for (int c = 0; c<ncols; c++){  
30.         plane[c] = V->data.fl[ncols*(ncols - 1) + c];  
31.         plane[ncols] += plane[c] * centroid->data.fl[c];  
32.     }  
33.     // Release allocated resources.  
34.     cvReleaseMat(¢roid);  
35.     cvReleaseMat(&points2);  
36.     cvReleaseMat(&A);  
37.     cvReleaseMat(&W);  
38.     cvReleaseMat(&V);  
39. }  

调用的方式：

[cpp]  view plain  copy

 

1. CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵   
2.         for (int i=0;i < X_vector.size(); ++i)  
3.         {  
4.             points_mat->data.fl[i*3+0] = X_vector[i];//矩阵的值进行初始化   X的坐标值  
5.             points_mat->data.fl[i * 3 + 1] = Y_vector[i];//  Y的坐标值  
6.             points_mat->data.fl[i * 3 + 2] = Z_vector[i];<span style="font-family: Arial, Helvetica, sans-serif;">//  Z的坐标值</span>  
7.   
8.         }  
9.         float plane12[4] = { 0 };//定义用来储存平面参数的数组   
10.         cvFitPlane(points_mat, plane12);//调用方程   

我们拟合出来的方程：Ax+By+Cz=D

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

这是要注意的方程的表示