【計算機視覺】雙目測距（六）--三維重建及UI顯示
原文：
http://blog.csdn.NET/chenyusiyuan/article/details/5970799
在獲取到視差資料後，利用 OpenCV 的 reProjectImageTo3D 函式結合 Bouquet 校正方法得到的 Q 矩陣就可以得到環境的三維座標資料，然後利用 OpenGL 來實現三維重構。 OpenCV 與 OpenGL 的程式設計範例，我在 學習筆記（ 15 ） 中有詳細的討論，這裡就不重複了，下面補充一些細節問題：
.
.
1． reProjectImageTo3D 是怎樣計算出三維座標資料的？
圖 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 本身算出的三維座標作為參考，程式程式碼如下 ：
[c-sharp] view plain copy print?- 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);
- % Textbook 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);
- end
- end
- subplot(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
.
.
2． 為什麼利用修正了的 Q 矩陣所計算得到的三維資料中， Y 座標資料是正負顛倒的？
圖 24
.
這個問題我覺得可以從影象座標系與攝像機座標系的關係這一角度來解釋。如上圖所示，一般影象座標系和攝像機座標系都是以從左至右為 X 軸正方向，從上至下為 Y 軸正方向 ，攝像機座標系的 Z 軸正方向則是從光心到成像平面的垂線方向。因此，我們得到的三維座標資料中 Y 軸資料的正負與實際是相反的，在應用時要新增負號來修正。
.
.
3． 如何畫出三維重建影象和景深影象？
.
利用 cvReprojectImageTo3D 計算出的三維座標資料矩陣一般是三通道浮點型的，需要注意的是這個矩陣儲存的是三維座標資料，而不是 RGB 顏色值，所以是不能呼叫 cvShowImage() 或者 OpenCV2.1 版的 imshow() 等函式來顯示這個矩陣，否則就會看到這種影象：
.
圖 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 視窗時，顯示模式要如下設定：
[c-sharp] view plain copy print?- //***OpenGL Window
- glutInit(&argc, argv);
- glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
- glutInitWindowPosition(10,420);
- glutInitWindowSize(glWinWidth, glWinHeight);
- glutCreateWindow("3D disparity image");
在迴圈中的呼叫：
[c-sharp] view plain copy print?- //////////////////////////////////////////////////////////////////////////
- // OpenGL顯示
- img3dIpl = img3d;
- load3dDataToGL(&img3dIpl); // 載入需要顯示的影象（視差資料）
- loadTextureToGL(&img1roi); // 顯示紋理
- glutReshapeFunc (reshape); // 視窗變化時重繪影象
- glutDisplayFunc(renderScene); // 顯示三維影象
- glutPostRedisplay(); // 重新整理畫面（不用此語句則不能動態更新影象）
- loadPixel2IplImage(imgGL); // 將 OpenGL 生成的畫素值儲存到 IplImage 中
loadGLPixelToIplImage 函式定義：
[c-sharp] view plain copy print?- //////////////////////////////////////////////////////////////////////////
- // 將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