00 双目测距与三维重建的OpenCV实现问题集锦(四)三维重建与OpenGL显示

Post date: 2012/4/10 上午 08:28:02

chenyusiyuan » 2010-10-28 6:50

五、三维重建与OpenGL显示

在获取到视差数据后,利用OpenCV的 reProjectImageTo3D 函数结合 Bouquet校正方法得到的Q矩阵就可以得到环境的三维坐标数据,然后利用OpenGL来实现三维重构。OpenCV与OpenGL的编程范例,我在学习笔记(15)中有详细的讨论,这里就不重复了,下面补充一些细节问题:

1. reProjectImageTo3D是怎样计算出三维坐标数据的?

22.jpg (23.31 KiB) 被浏览 2253 次

图22

相信看过OpenCV第12章的朋友对上图中的Q矩阵不会陌生,根据以上变换公式,按理说OpenCV应该也是通过矩阵运算的方式来计算出三维坐标数据的,但实际上仔细查看源代码,会发现cvReprojectImageTo3D用了比较奇怪的方法来实现,主要代码如下:

代码: 全选

02737     for( y = 0; y < rows; y++ )
02738     {
02739         const float* sptr = (const float*)(src->data.ptr + src->step*y);   // 视差矩阵指针
02740         float* dptr0 = (float*)(dst->data.ptr + dst->step*y), *dptr = dptr0;   // 三维坐标矩阵指针
// 每一行运算开始时,用 当前行号y 乘以Q阵第2列、再加上Q阵第4列,作为初始值
// 记 qq=[qx, qy, qz, qw]’
02741         double qx = q[0][1]*y + q[0][3], qy = q[1][1]*y + q[1][3];    
02742         double qz = q[2][1]*y + q[2][3], qw = q[3][1]*y + q[3][3];  
// 每算完一个像素的三维坐标,向量qq 累加一次q阵第1列
// 即:qq = qq + q(:,1)
02769         for( x = 0; x < cols; x++, qx += q[0][0], qy += q[1][0], qz += q[2][0], qw += q[3][0] )
02770         {02771             double d = sptr[x];// 计算当前像素三维坐标// 将向量qq 加上 Q阵第3列与当前像素视差d的乘积,用所得结果的第4元素除前三位元素即可// [X,Y,Z,W]’ = qq + q(:,3) * d;   iW = 1/W; X=X*iW; Y=Y*iW; Z=Z*iW;02772             double iW = 1./(qw + q[3][2]*d);02773             double X = (qx + q[0][2]*d)*iW;02774             double Y = (qy + q[1][2]*d)*iW;02775             double Z = (qz + q[2][2]*d)*iW;02776             if( fabs(d-minDisparity) <= FLT_EPSILON )02777                 Z = bigZ;   // 02713     const double bigZ = 10000.;02778 02779             dptr[x*3] = (float)X;02780             dptr[x*3+1] = (float)Y;02781             dptr[x*3+2] = (float)Z;02782         }

OpenCV的这种计算方式比较令人费解,我的理解是可能这种方式的计算速度比较快。理论上,直接通过矩阵Q与向量[x,y,d,1]’的乘积就可以得到相同的结果,下面用Matlab来验证一下两种方式是异曲同工的,用Matlab按照OpenCV计算方式得到的结果称为“OpenCV method”,直接按公式计算得到的结果称为“Equation method”,用OpenCV本身算出的三维坐标作为参考,程序代码如下:

代码: 全选

close all;clear all;clc 
im = imread('C:\Stereo IO Data\lfFrame_01.jpg');
data = importdata('C:\Stereo IO Data\disparity_01.txt');
r = data(1);    % 行数
c = data(2);    % 列数
disp = data(3:end); % 视差
vmin = min(disp);
vmax = max(disp);
disp = reshape(disp, [c,r])'; % 将列向量形式的 disp 重构为 矩阵形式
%  OpenCV 是行扫描存储图像,Matlab 是列扫描存储图像
%  故对 disp 的重新排列是首先变成 c 行 r 列的矩阵,然后再转置回 r 行 c 列
img = uint8( 255 * ( disp - vmin ) / ( vmax - vmin ) );q = [1. 0. 0. -1.5690376663208008e+002;...    0. 1. 0. -1.4282237243652344e+002;...        0. 0. 0. 5.2004731331639300e+002;...    0. 0. 1.0945105843175637e-002 0.]; % q(4,3) 原为负值,现修正为正值big_z = 1e5;pos1 = zeros(r,c,3);pos2 = zeros(r,c,3);for i = 1:r    qq = q*[0 i 0 1]';    for j = 1:c        if disp(i,j)>0        % OpenCV method            vec = qq + q(:,3)*disp(i,j);            vec = vec/vec(4);            pos1(i,j,:) = vec(1:3);        % Equation method            tmp = q*[j,i,disp(i,j),1]'; % j 是列数,i 是行数,分别对应公式中的 x 和 y            pos2(i,j,:) = tmp(1:3)/tmp(4);        else            pos1(i,j,3) = big_z;            pos2(i,j,3) = big_z;        end        qq = qq + q(:,1);    endendsubplot(221);imshow(im); title('Left Frame');subplot(222);imshow(img); title('Disparity map');% Matlab按OpenCV计算方式得到的三维坐标x = pos1(:,:,1); y = -pos1(:,:,2);  % 图像坐标系Y轴是向下为正方向,因此需添加负号来修正z = pos1(:,:,3); ind = find(z>10000);  % 以毫米为量纲x(ind)=NaN; y(ind)=NaN; z(ind)=NaN;subplot(234);mesh(x,z,y,double(im),'FaceColor','texturemap');  % Matlab 的 mesh、surf 函数支持纹理映射colormap(gray); axis equal; axis([-1000 1000 0 9000 -500 2000]);xlabel('Horizonal');ylabel('Depth');zlabel('Vertical'); title('OpenCV method');view([0 0]);  % 正视图% view([0 90]);   % 俯视图% view([90 0]);   % 侧视图% Matlab 按公式直接计算得到的三维坐标x = pos2(:,:,1); y = -pos2(:,:,2); z = pos2(:,:,3); ind = find(z>10000);  % 以毫米为量纲x(ind)=NaN; y(ind)=NaN; z(ind)=NaN;subplot(235);mesh(x,z,y,double(im),'FaceColor','texturemap'); colormap(gray); axis equal; axis([-1000 1000 0 9000 -500 2000]);xlabel('Horizonal');ylabel('Depth');zlabel('Vertical'); title('Equation method');view([0 0]);% 读入OpenCV计算保存到本地的三维坐标作为参考data=importdata('C:\Stereo IO Data\xyz.txt');x=data(:,1); y=data(:,2); z=data(:,3);ind=find(z>1000);  % 以厘米为量纲x(ind)=NaN; y(ind)=NaN; z(ind)=NaN;x=reshape(x,[352 288])'; % 数据写入时是逐行进行的,而Matlab是逐列读取y=-reshape(y,[352 288])'; z=reshape(z,[352 288])';subplot(236)mesh(x,z, y,double(im),'FaceColor','texturemap');colormap(gray); axis equal;axis([-100 100 0 900 -50 200]);xlabel('Horizonal');ylabel('Depth');zlabel('Vertical'); title('OpenCV result');view([0 0]);

23.jpg (78.61 KiB) 被浏览 2253 次

图23

2. 为什么利用修正了的Q矩阵所计算得到的三维数据中,Y坐标数据是正负颠倒的?

24.JPG (24.73 KiB) 被浏览 2253 次

图24

这个问题我觉得可以从图像坐标系与摄像机坐标系的关系这一角度来解释。如上图所示,一般图像坐标系和摄像机坐标系都是以从左至右为X轴正方向,从上至下为Y轴正方向,摄像机坐标系的Z轴正方向则是从光心到成像平面的垂线方向。因此,我们得到的三维坐标数据中Y轴数据的正负与实际是相反的,在应用时要添加负号来修正。

3. 如何画出三维重建图像和景深图像?

利用cvReprojectImageTo3D计算出的三维坐标数据矩阵一般是三通道浮点型的,需要注意的是这个矩阵存储的是三维坐标数据,而不是RGB颜色值,所以是不能调用cvShowImage()或者OpenCV2.1版的imshow()等函数来显示这个矩阵,否则就会看到这种图像:

25.JPG (99.79 KiB) 被浏览 2253 次

图25

这里出现的明显的四个色块,其实应该是由三维坐标数据中的X轴和Y轴数据造成,不同象限的数据形成相应的色块。

要画出正确的三维重建图像,可以结合OpenGL(可参考我的学习笔记(15)或者Matlab(例如保存三维数据到本地然后用Matlab的mesh函数画出,例程见本文问题1;也可以考虑在OpenCV中调用Matlab混合编程)来实现。

深度图像的显示相对比较简单,只要从三维坐标数据中分离出来(可用cvSplit() 函数),经过适当的格式转换(例如转换为CV_8U格式),就可用cvShowImage() 或者OpenCV2.1版的imshow()等函数来显示了,伪彩色的深度图也可以参考我的学习笔记(18)问题6给出的例程稍作修改即可实现。

4. 怎样把OpenGL窗口的图像复制到OpenCV中用IplImage格式显示和保存?

学习笔记(15)中详细给出了将OpenCV生成的IplImage图像和三维坐标数据复制到OpenGL中显示的例程,而在应用中,我们有时候也需要把OpenGL实时显示的三维图像复制到OpenCV中,用IplImage格式保存,以便和其它图像组合起来显示或保存为视频文件。这里给出相应的例程以供参考:

首先在创建OpenGL窗口时,显示模式要如下设置:

代码: 全选

   //***OpenGL Window
   glutInit(&argc, argv);
   glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
   glutInitWindowPosition(10,420);
   glutInitWindowSize(glWinWidth, glWinHeight);
   glutCreateWindow("3D disparity image");

在循环中的调用:

代码: 全选

      //////////////////////////////////////////////////////////////////////////
      // OpenGL显示
      img3dIpl = img3d;
      load3dDataToGL(&img3dIpl);      // 载入需要显示的图像(三维坐标数据)
      loadTextureToGL(&img1roi);      // 显示纹理
      glutReshapeFunc (reshape);      // 窗口变化时重绘图像
      glutDisplayFunc(renderScene);      // 显示三维图像
      glutPostRedisplay();               // 刷新画面(不用此语句则不能动态更新图像)
      loadGLPixelToIplImage(imgGL);         // 将 OpenGL 生成的像素值存储到 IplImage 中

loadGLPixelToIplImage 函数定义:

代码: 全选

//////////////////////////////////////////////////////////////////////////
// 将OpenGL窗口像素存储到 IplImage 中
void  loadGLPixelToIplImage(IplImage* img)
{
   const int n = 3*glWinWidth*glWinHeight; 
   float *pixels = (float *)malloc(n * sizeof(GL_FLOAT)); 
   IplImage *tmp = cvCreateImage(cvSize(glWinWidth, glWinHeight), 8, 3);
   tmp->origin = CV_ORIGIN_BL;
/* 后台缓存的图像数据才是我们需要复制的,若复制前台缓存会把可能的叠加在OpenGL窗口上的对象(其它窗口或者鼠标指针)也复制进去*/
   glReadBuffer(GL_BACK);      
   glReadPixels(0, 0, glWinWidth, glWinHeight, GL_RGB, GL_FLOAT, pixels);
   int k = 0;   for(int i = 0 ; i < glWinHeight; i++)   {      for(int j = 0 ; j < glWinWidth; j++,k+=3)      {         CvPoint pt = {j, glWinHeight - i - 1};         uchar* temp_ptr = &((uchar*)(tmp->imageData + tmp->widthStep*pt.y))[pt.x*3];         //OpenGL采用的是BGR格式,所以,读出来以后,还要换一下R<->B,才能得到RGB         temp_ptr[0] = pixels[k+2] * 255; //blue          temp_ptr[1] = pixels[k+1] * 255; //green         temp_ptr[2] = pixels[k] * 255;   //red      }   }      cvResize(tmp, img);   // 释放内存   free(pixels);   cvReleaseImage(&tmp);}

显示效果如下:

26.jpg (87.73 KiB) 被浏览 2253 次

图26

chenyusiyuan

OpenCV本科生

帖子: 144

注册: 2009-09-23 10:40

seaok » 2010-11-02 18:04

非常好的一篇帖子,看了楼主4篇帖子,感觉都写的非常好,个人也在做双目视觉方面的项目,希望能够和楼主多多交流。