SGBM算法獲取視差圖
立體校正后的左右兩幅圖像得到后素征，匹配點是在同一行上的昙啄，可以使用OpenCV中的BM算法或者SGBM算法計算視差圖。由于SGBM算法的表現(xiàn)要遠遠優(yōu)于BM算法狭吼，因此采用SGBM算法獲取視差圖赠幕。SGBM中的參數(shù)設置如下：
int numberOfDisparities = ((imgSize.width / 8) + 15) & -16;
??? cv::Ptr sgbm =cv::StereoSGBM::create(0, 16, 3);
??? sgbm->setPreFilterCap(32);
??? int SADWindowSize = 9;
??? int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
??? sgbm->setBlockSize(sgbmWinSize);
??? intcn = imgL.channels();
??? sgbm->setP1(8* cn*sgbmWinSize*sgbmWinSize);
??? sgbm->setP2(32* cn*sgbmWinSize*sgbmWinSize);
??? sgbm->setMinDisparity(0);
???sgbm->setNumDisparities(numberOfDisparities);
??? sgbm->setUniquenessRatio(10);
??? sgbm->setSpeckleWindowSize(100);
??? sgbm->setSpeckleRange(32);
??? sgbm->setDisp12MaxDiff(1);
??? intalg = STEREO_SGBM;
??? if(alg == STEREO_HH)
???????sgbm->setMode(cv::StereoSGBM::MODE_HH);
??? else if(alg == STEREO_SGBM)
???????sgbm->setMode(cv::StereoSGBM::MODE_SGBM);
??? else if(alg == STEREO_3WAY)
???????sgbm->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);
??? sgbm->compute(imgL, imgR, disp);
默認計算出的是左視差圖，如果需要計算右視差圖，則將上面加粗的三條語句替換為下面前三條語句膨蛮。由于視差值計算出來為負值叠纹，disp類型為16SC1，因此需要取絕對值敞葛，然后保存：
sgbm->setMinDisparity(-numberOfDisparities);
???sgbm->setNumDisparities(numberOfDisparities);
??? sgbm->compute(imgR, imgL, disp);
??? disp = abs(disp);
SGBM算法得到的左誉察、右視差圖如下，左視差圖的數(shù)據(jù)類型為CV_16UC1制肮，右視差圖的數(shù)據(jù)類型為CV_16SC1?（SGBM中視差圖中不可靠的視差值設置為最小視差（mindisp-1）*16冒窍。因此在此例中，左視差圖中不可靠視差值設置為-16豺鼻，截斷值為0综液；右視差圖中不可靠視差值設置為（-numberOfDisparities-1）*16，取絕對值后為（numberOfDisparities+1）*16儒飒，所以兩幅圖會有較大差別）：
左視差圖（不可靠視差值為0）
右視差圖（不可靠視差值為 （numberOfDisparities+1）*16?）
如果將右視差圖不可靠視差值也設置為0谬莹，則如下
至此，左視差圖和右視差圖遙相呼應桩了。
2. 視差圖空洞填充
視差圖中視差值不可靠的視差大多數(shù)是由于遮擋引起附帽，或者光照不均勻引起。既然牛逼如SGBM也覺得不可靠井誉，那與其留著做個空洞蕉扮，倒不如用附近可靠的視差值填充一下。
空洞填充也有很多方法颗圣，在這里我檢測出空洞區(qū)域喳钟，然后用附近可靠視差值的均值進行填充。填充后的視差圖
填充后左視差圖
填充后右視差圖
3. 視差圖轉換為深度圖
視差的單位是像素（pixel）在岂，深度的單位往往是毫米（mm）表示奔则。而根據(jù)平行雙目視覺的幾何關系（此處不再畫圖推導，很簡單）蔽午，可以得到下面的視差與深度的轉換公式：
?depth = ( f * baseline) / disp
?上式中易茬，depth表示深度圖；f表示歸一化的焦距及老，也就是內(nèi)參中的fx抽莱； baseline是兩個相機光心之間的距離，稱作基線距離骄恶；disp是視差值岸蜗。等式后面的均已知，深度值即可算出叠蝇。
在上面我們用SGBM算法獲取了視差圖，接下來轉換為深度圖，函數(shù)代碼如下：
/*
函數(shù)作用：視差圖轉深度圖輸入：dispMap ----視差圖悔捶，8位單通道铃慷，CV_8UC1
K?????? ----內(nèi)參矩陣，float類型輸出：depthMap ----深度圖蜕该，16位無符號單通道犁柜，CV_16UC1
*/
voiddisp2Depth(cv::Mat dispMap, cv::Mat&depthMap, cv::Mat K)
{
??? inttype = dispMap.type();
??? float fx = K.at<float>(0, 0);
??? float fy = K.at<float>(1, 1);
??? float cx = K.at<float>(0, 2);
??? float cy = K.at<float>(1, 2);
??? float baseline = 65; //基線距離65mm
??? if(type == CV_8U)
??? {
??????? const float PI = 3.14159265358;
??????? intheight = dispMap.rows;
??????? intwidth = dispMap.cols;
??????? uchar* dispData = (uchar*)dispMap.data;
??????? ushort* depthData = (ushort*)depthMap.data;
??????? for (int i = 0; i < height; i++)
??????? {
??????????? for (int j = 0; j < width; j++)
??????????? {
??????????????? intid = i*width + j;
??????????????? if(!dispData[id])? continue;? //防止0除
??????????????? depthData[id] =ushort( (float)fx *baseline / ((float)dispData[id]) );
??????????? }
??????? }
??? }
??? else
??? {
??????? cout <<"please confirm dispImg's type!"<< endl;
??????? cv::waitKey(0);
??? }
}
注：png的圖像格式可以保存16位無符號精度，即保存范圍為0-65535堂淡，如果是mm為單位馋缅，則最大能表示約65米的深度，足夠了绢淀。
上面代碼中我設置深度圖的精度為CV_16UC1萤悴，也就是ushort類型，將baseline設置為65mm皆的，轉換后保存為png格式即可覆履。如果保存為jpg或者bmp等圖像格式，會將數(shù)據(jù)截斷為0-255费薄。所以保存深度圖硝全，png格式是理想的選擇。（如果不是為了獲取精確的深度圖楞抡，可以將baseline設置為1伟众，這樣獲取的是相對深度圖，深度值也是相對的深度值）
轉換后的深度圖如下;
?左深度圖
? 右深度圖
空洞填充后的深度圖召廷，如下：
左深度圖（空洞填充后）
?右深度圖（空洞填充后）
視差圖到深度圖完成凳厢。
注：視差圖和深度圖中均有計算不正確的點，此文意在介紹整個流程柱恤，不特別注重算法的優(yōu)化数初，如有大神望不吝賜教。
附：視差圖和深度圖的空洞填充
?步驟如下：
① 以視差圖dispImg為例梗顺。計算圖像的積分圖integral泡孩，并保存對應積分圖中每個積分值處所有累加的像素點個數(shù)n（空洞處的像素點不計入n中，因為空洞處像素值為0寺谤，對積分值沒有任何作用仑鸥，反而會平滑圖像）。
② 采用多層次均值濾波变屁。首先以一個較大的初始窗口去做均值濾波（積分圖實現(xiàn)均值濾波就不多做介紹了眼俊，可以參考我之前的一篇博客），將大區(qū)域的空洞賦值粟关。然后下次濾波時疮胖，將窗口尺寸縮小為原來的一半，利用原來的積分圖再次濾波，給較小的空洞賦值（覆蓋原來的值）澎灸；依次類推院塞，直至窗口大小變?yōu)?x3，此時停止濾波性昭，得到最終結果拦止。
③ 多層次濾波考慮的是對于初始較大的空洞區(qū)域，需要參考更多的鄰域值糜颠，如果采用較小的濾波窗口汹族，不能夠完全填充，而如果全部采用較大的窗口其兴，則圖像會被嚴重平滑顶瞒。因此根據(jù)空洞的大小，不斷調(diào)整濾波窗口忌警。先用大窗口給所有空洞賦值搁拙，然后利用逐漸變成小窗口濾波覆蓋原來的值，這樣既能保證空洞能被填充上法绵，也能保證圖像不會被過度平滑箕速。
?空洞填充的函數(shù)代碼如下，僅供參考：
??voidinsertDepth32f(cv::Mat& depth)
??{
???? const intwidth = depth.cols;
???? const intheight = depth.rows;
???? float* data = (float*)depth.data;
????cv::Mat integralMap = cv::Mat::zeros(height, width, CV_64F);
????cv::Mat ptsMap = cv::Mat::zeros(height, width, CV_32S);
?? ??double* integral = (double*)integralMap.data;
?? ??int* ptsIntegral = (int*)ptsMap.data;
???? memset(integral,0, sizeof(double) * width * height);
???? memset(ptsIntegral,0, sizeof(int) * width * height);
???? for (int i = 0; i < height; ++i)
???? {
???????? intid1 = i * width;
???????? for (int j = 0; j < width; ++j)
???????? {
???????? ????intid2 = id1 + j;
???????????? if (data[id2] > 1e-3)
???????????? {
???????????????? integral[id2] = data[id2];
???????????????? ptsIntegral[id2] =1;
???????????? }
???????? }
???? }
???? // 積分區(qū)間
???? for (int i = 0; i < height; ++i)
???? {
???????? intid1 = i * width;
???????? for (int j = 1; j < width; ++j)
???????? {
???????????? intid2 = id1 + j;
???????????? integral[id2] += integral[id2 -1];
???????????? ptsIntegral[id2] +=ptsIntegral[id2 -1];
???????? }
???? }
???? for (int i = 1; i < height; ++i)
???? {
???????? intid1 = i * width;
???????? for (int j = 0; j < width; ++j)
???????? {
???????????? intid2 = id1 + j;
???????????? integral[id2] += integral[id2 -width];
???????????? ptsIntegral[id2] +=ptsIntegral[id2 - width];
???????? }
???? }
???? intwnd;
???? double dWnd = 2;
???? while (dWnd > 1)
???? {
???????? wnd =int(dWnd);
???????? dWnd /=2;
???????? for (int i = 0; i < height; ++i)
???????? {
????? ???????intid1 = i * width;
???????????? for (int j = 0; j < width; ++j)
???????????? {
???????????????? intid2 = id1 + j;
???????????????? int left = j - wnd - 1;
???????????????? intright = j + wnd;
???????????????? int top = i - wnd - 1;
???????????????? intbot = i + wnd;
???????????????? left = max(0, left);
???????????????? right = min(right,width -1);
???????????????? top = max(0, top);
???????????????? bot = min(bot, height -1);
???????????????? intdx = right - left;
???????????????? intdy = (bot - top) * width;
???????????????? intidLeftTop = top * width + left;
???????????????? intidRightTop = idLeftTop + dx;
???????????????? intidLeftBot = idLeftTop + dy;
???????????????? intidRightBot = idLeftBot + dx;
???????????????? intptsCnt = ptsIntegral[idRightBot] + ptsIntegral[idLeftTop] -(ptsIntegral[idLeftBot] + ptsIntegral[idRightTop]);
???????????????? doublesumGray = integral[idRightBot] +integral[idLeftTop] - (integral[idLeftBot] + integral[idRightTop]);
???????????????? if (ptsCnt <= 0)
???????????????? {
???????????????????? continue;
???????????????? }
???????????????? data[id2] =float(sumGray / ptsCnt);
? ????????????}
???????? }
???????? int s = wnd / 2 * 2 + 1;
???????? if (s > 201)
???????? {
???????????? s =201;
???????? }
???????? cv::GaussianBlur(depth, depth,cv::Size(s, s), s, s);
???? }
?}
