在利用OpenCV的 reprojectImageTo3D 函数得到环境的三维(宽度、高度、深度)信息后,通过以下代码保存一帧三维数据:
- const double max_z = 1.0e4;
- FILE* fp = fopen("D://OpenCV2.1//Projects//RobotVision//Imgs//3d_data.txt", "wt");
- for(int y = 0; y < img3d.rows; y++)
- {
- for(int x = 0; x < img3d.cols; x++)
- {
- Vec3f point = img3d.at<Vec3f>(y, x); // Vec3f 是 template 类定义
- if(fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z)
- fprintf(fp, "%d %d %d/n", 0, 0, 0);
- else
- fprintf(fp, "%f %f %f/n", point[0], point[1], point[2]);
- }
- }
- fclose(fp);
然后在Matlab中读入这些数据:
- data = importdata('D:/OpenCV2.1/Projects/RobotVision/Imgs/3d_data.txt');
- xdata=data(:,1);ydata=data(:,2);zdata = data(:,3);
- xdata=-xdata;
- % 显示深度图
- figure;
- depth=reshape(zdata, [352 288]);
- mesh(depth);
- % 显示重构的三维环境
- figure;
- [cx,cy] = meshgrid(min(xdata(:)):max(xdata(:)), ...
- min(ydata(:)):max(ydata(:)));
- cz=griddata(xdata,ydata,zdata,cx,cy,'cubic');
- mesh(cx,cy,cz);
- xlabel('Width'); ylabel('Height'); zlabel('Depth');
由上述代码可以得到如下深度图和重构的三维环境效果,当然这样的重构只是初步的,后期还需要用OpenGL来实现:
2010-10-13:
P.S. 也可以用如下代码,三维效果更好:
- data=importdata('C:/Stereo IO Data/xyz.txt');
- img=imread('C:/Stereo IO Data/lfFrame_01.jpg');
- x=data(:,1); y=data(:,2); z=data(:,3);
- ind=find(z==0);
- x(ind)=NaN; y(ind)=NaN; z(ind)=NaN;
- figure;
- mesh(x,z,-y,double(img'),'FaceColor','texturemap'); //img的像素灰度值用于mesh图的纹理映射
- colormap(gray); // img图是灰度图,故 colormap 为 gray
- axis equal; axis([-100 100 0 900 -50 200]);
- xlabel('Horizonal');ylabel('Depth');zlabel('Vertical');
- view([0 90]); // 视角[0,90]代表正上方俯视
![](http://image71.360doc.com/DownloadImg/2014/03/3117/40396805_4.gif)
|