标签：vector ncols int OpenCV plane dx 拟合 data 乘法
本文主要验证了博客上的最小二乘法拟合平面的。与 用matlab拟合出来的平面计算的点到直线的距离是一样的，而且系数也是一样的。说明了本方法的可行性。
matlab中公式为z = c + ax +by
oepncv中公式为Ax+By+Cz=D 将opencv中公式换算成matlab的公式，系数是一样的。
平面公式为：Ax+By+Cz=D
//对应的方程：Ax+By+Cz=D 其中 A = plane12[0], B = plane12[1], C = plane12[2], D = plane12[3],这是要注意的方程的表示
//float plane12[4] = { 0 };//定义用来储存平面参数的数组，分别对应ABCD
拟合平面
1 void cvFitPlane(vector<float>dx, vector<float>dy, vector<float>dz, float* plane) {
2
3 //直线方程为：
4 //构建点集cvmat
5 CvMat* points = cvCreateMat(dx.size(), 3, CV_32FC1);
6 int nnum = 96;
7 for (int i = 0; i < dx.size(); ++i)
8 {
9 points->data.fl[i * 3 + 0] = dx[i];//矩阵的值进行初始化 X的坐标值
10 points->data.fl[i * 3 + 1] = dy[i];// Y的坐标值
11 points->data.fl[i * 3 + 2] = dz[i]; // Z的坐标值
12
13 }
14
15 Estimate geometric centroid.
16 int nrows = points->rows;
17 int ncols = points->cols;
18 int type = points->type;
19 CvMat* centroid = cvCreateMat(1, ncols, type);
20 cvSet(centroid, cvScalar(0));
21 for (int c = 0; c < ncols; c++) {
22 for (int r = 0; r < nrows; r++)
23 {
24 centroid->data.fl[c] += points->data.fl[ncols*r + c];
25 }
26 centroid->data.fl[c] /= nrows;
27 }
28 // Subtract geometric centroid from each point.
29 CvMat* points2 = cvCreateMat(nrows, ncols, type);
30 for (int r = 0; r < nrows; r++)
31 for (int c = 0; c < ncols; c++)
32 points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];
33 // Evaluate SVD of covariance matrix.
34 CvMat* A = cvCreateMat(ncols, ncols, type);
35 CvMat* W = cvCreateMat(ncols, ncols, type);
36 CvMat* V = cvCreateMat(ncols, ncols, type);
37
38 cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);
39 cvSVD(A, W, NULL, V, CV_SVD_V_T);
40
41 // Assign plane coefficients by singular vector corresponding to smallest singular value.
42 plane[ncols] = 0;
43 for (int c = 0; c < ncols; c++) {
44 plane[c] = V->data.fl[ncols*(ncols - 1) + c];
45 plane[ncols] += plane[c] * centroid->data.fl[c];
46 }
47 // Release allocated resources.
48 cvReleaseMat(&points);
49 cvReleaseMat(¢roid);
50 cvReleaseMat(&points2);
51 cvReleaseMat(&A);
52 cvReleaseMat(&W);
53 cvReleaseMat(&V);
54 }
计算点到平面的距离
1 //计算点到平面的距离
2 //Ax+By+Cz=D
3 //|点（a,b,c） 到平面bai Ax+By+Cz=D的距离du
4
5 //= | A * a + B * b + C * c - D| /√(A ^ 2 + B ^ 2 + C ^ 2)
6 void calculateDist(vector<float>dx, vector<float>dy, vector<float>dz, float* plane, vector<float> &dist)
7 {
8 for (int i = 0; i < dx.size(); i++)
9 {
10 float ds = fabs(plane[0] * dx[i] + plane[1] * dy[i] + plane[2] * dz[i] - plane[3]);
11 float dfen = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);
12 if (!(dfen > -0.00001 && dfen < -0.00001))
13 {
14 float ddist = ds / dfen;
15 dist.push_back(ddist);
16 }
17 }
18 }
测试流程
从文件中读取数据，然后计算拟合平面，计算点到平面的距离，并输出到csv文件中
数据格式为：
一行中代表xyz
1 -53.883533,55.133049,895.801941 2 -40.928612,32.402653,897.237793 3 -21.391739,50.161041,899.748901 4 2.107507,62.850151,902.479065 5 3.594930,37.490810,902.427490
具体代码为：
1 fstream fs;
2 fs.open("E:\\wokspace\\PROJECT\\ThirdTrailInspection\\matlab\\dResult.txt");
3 if (!fs.is_open())
4 {
5 return;
6 }
7 vector<float> dx;
8 vector<float> dy;
9 vector<float> dz;
10 int i = 0;
11 string buff;
12 while (getline(fs, buff))//是否到文件结bai尾
13 {
14 int nfist = buff.find_first_of(',');
15 int nLast = buff.find_last_of(',');
16 string st1 = buff.substr(0, nfist);
17 string st2 = buff.substr(nfist + 1, nLast - nfist - 1);
18 string st3 =(buff.substr(nLast + 1));
19 dx.push_back(stof(buff.substr(0, nfist)));
20 dy.push_back(stof(buff.substr(nfist + 1, nLast - nfist - 1)));
21 dz.push_back(stof(buff.substr(nLast + 1)));
22 }
23 fs.close();
24
25
26 //代入最小二乘算法中
27 float plane[4] = { 0 };
28 vector<float> dx1;
29 vector<float> dy1;
30 vector<float> dz1;
31 dx1.assign(dx.begin(), dx.begin() + 96);
32 dy1.assign(dy.begin(), dy.begin() + 96);
33 dz1.assign(dz.begin(), dz.begin() + 96);
34 cvFitPlane(dx1, dy1, dz1, plane);
35 vector<float> dist;
36 calculateDist(dx, dy, dz, plane, dist);
37 fstream fws("e://de.csv", fstream::in | fstream::out | fstream::trunc);
38 for (int i = 0; i < dist.size(); i++)
39 {
40 fws << dist[i] <<"\r";
41 }
42 fws.close();
对应的matlab代码
1 clc;
2 close all;
3 clear all;
4 %https://www.ilovematlab.cn/thread-220252-1-1.html
5
6 data = importdata('E:\wokspace\PROJECT\ThirdTrailInspection\matlab\dResult.txt');
7 x = data(1:96, 1);
8 y = data(1:96, 2);
9 z = data(1:96, 3);
10 % x = data(113:192, 1);
11 % y = data(113:192, 2);
12 % z = data(113:192, 3);
13 scatter3(x, y,z, 'r');%画点 散点图
14 hold on;
15 X = [ones(length(x),1) x y];
16 [b,bint,r,rint,stats] = regress(z,X,95);
17
18 % 图形绘制
19 xfit = min(x):0.1:max(x);
20 yfit = min(y):0.1:max(y);
21 [XFIT,YFIT]= meshgrid (xfit,yfit);%用于生成网格采样点
22 ZFIT = b(1) + b(2) * XFIT + b(3) * YFIT;
23 mesh(XFIT,YFIT,ZFIT);
24
25 %%测试结果
26 %data = importdata('C:\Users\apr_z\Desktop\dResult.txt');
27 xx = data(:, 1);
28 yy = data(:, 2);
29 zz = data(:, 3);
30 [row, col] = size(xx);%求矩阵的行数和列数
31 dist = ones(row, 1);
32 for i = 1: row
33 dist(i) = abs(b(2) * xx(i) + b(3) * yy(i) - zz(i) + b(1)) / sqrt(b(2)* b(2) + b(3)*b(3) + 1 );
34 end
35 xlswrite('C:\Users\apr_z\Desktop\AnalyzeResult.xlsx', dist);
小结：
vector 复制某一些数据时： dx1.assign(dx.begin(), dx.begin() + 96);
vector容器 追加其他容器的内容，使用insert
pt3DList.insert(pt3DList.end(), vc.begin(), vc.end());
用此平面拟合的计算 点到直线的距离 与 用matlab计算出来的点到直线的距离是一模一样的。
标签：vector,ncols,int,OpenCV,plane,dx,拟合,data,乘法 来源： https://www.cnblogs.com/ybqjymy/p/16309568.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。