【視覺-立體視覺】全域性匹配演算法SGBM實現（含動態規劃DP）詳解

阿新 • • 發佈：2019-01-13

最近一直在學習SGBM演算法，作為一種全域性匹配演算法，立體匹配的效果明顯好於區域性匹配演算法，但是同時複雜度上也要遠遠大於區域性匹配演算法。演算法主要是參考Stereo Processing by Semiglobal Matching and Mutual Information，裡面有講完整的演算法實現。
OpenCV中實際上是提供了SGBM類進行SGBM演算法的實現。
#include <highgui.h>
#include <cv.h>
#include <cxcore.h>
#include <iostream>
using namespace std;
using namespace cv;
int main()
{

IplImage * img1 = cvLoadImage("left.png",0);
IplImage * img2 = cvLoadImage("right.png",0);
cv::StereoSGBM sgbm;
int SADWindowSize = 9;
sgbm.preFilterCap = 63;
sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
int cn = img1->nChannels;
int numberOfDisparities=64;
sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
sgbm.minDisparity = 0;
sgbm.numberOfDisparities = numberOfDisparities;
sgbm.uniquenessRatio = 10;
sgbm.speckleWindowSize = 100;
sgbm.speckleRange = 32;
sgbm.disp12MaxDiff = 1;
Mat disp, disp8;
int64 t = getTickCount();
sgbm((Mat)img1, (Mat)img2, disp);
t = getTickCount() - t;
cout<<"Time elapsed:"<<t*1000/getTickFrequency()<<endl;
disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));

namedWindow("left", 1);
cvShowImage("left", img1);
namedWindow("right", 1);
cvShowImage("right", img2);
namedWindow("disparity", 1);
imshow("disparity", disp8);
waitKey();
imwrite("sgbm_disparity.png", disp8);
cvDestroyAllWindows();
return 0;
}
貼出效果：

//深度圖

但是僅僅用OpenCV自帶的SGBM類來實現並不能滿足，我還是希望能夠自己實現該演算法，然後最關鍵是移植到FPGA上去。

於是我嘗試自已去寫SGBM的程式碼，論文中提高Dynamic programming演算法,實際上SGBM中也用到了多方向的Dynamic programming，但是我目前只是實現了單方向的DP。

//引入概率公式

//引入CBT的插值方法
//加上相鄰匹配點位置之間的限制
#include <cstdio>
#include <cstring>
#include <iostream>
#include<cv.h>
#include<highgui.h>
#include <cmath>

using namespace std;
const int Width =  1024;
const int Height = 1024;
int Ddynamic[Width][Width];

//使用鐘形曲線作為匹配概率，差值越小則匹配的概率越大，最終的要求是使匹配的概率最大，概率曲線使用matlab生成

//均方差30
//int Probability[256] = {
// 255, 255, 254, 252, 250, 247, 244, 240, 235, 230, 225, 219, 213, 206, 200, 192, 185, 178, 170, 162,
// 155, 147, 139, 132, 124, 117, 110, 103, 96, 89, 83, 77, 71, 65, 60, 55, 50, 46, 42, 38, 35, 31, 28,
// 25, 23, 20, 18, 16, 14, 13, 11, 10, 9, 8, 7, 6, 5, 4, 4, 3, 3, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 0, 0,
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
//};

//均方差 5
int Probability[256] = {
255, 250, 235, 213, 185, 155, 124, 96, 71, 50, 35, 23, 14, 9, 5, 3, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};

int  main()
{

IplImage * leftImage = cvLoadImage("l2.jpg",0);
IplImage * rightImage = cvLoadImage("r2.jpg",0);

//IplImage * leftImage = cvLoadImage("left.bmp",0);
//IplImage * rightImage = cvLoadImage("right.bmp",0);

int imageWidth = leftImage->width;
int imageHeight =leftImage->height;

IplImage * DPImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * effectiveImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * FilterImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

unsigned char * pPixel = NULL;
unsigned char  pixel;
unsigned char * pPixel2 = NULL;
unsigned char  pixel2;
for (int i = 0; i< imageHeight;i++)
{
      for (int j =0; j < imageWidth;j++ )
      {
         pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + j;
         *pPixel = 0;
         pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + j;
         *pPixel = 0;
      }
}

cvNamedWindow("Left",1);
cvNamedWindow("Right",1);
cvNamedWindow("Depth",1);
cvNamedWindow("effectiveImage",1);

cvShowImage("Left",leftImage);
cvShowImage("Right",rightImage);

int minD = 0;
int maxD = 31;
//假設影象是經過矯正的，那麼每次都只是需要搜搜同一行的內容
int max12Diff = 10;

for (int i = 0;i < imageWidth;i++)
{
      Ddynamic[0] = 0;
      Ddynamic[0] = 0;
}

unsigned char * pLeftPixel  = NULL;
unsigned char * pRightPixel = NULL;
unsigned char leftPixel = 0;
unsigned char leftMax = 0;
unsigned char leftMin = 0;
unsigned char tempLeft1 = 0;
unsigned char tempLeft2 = 0;
unsigned char rightPixel =0;
unsigned char difPixel = 0;
int m,n,l;

int t1 = clock();
for (int i = 0 ; i < imageHeight;i++)
{
      for (int j = 0; j<imageWidth;j++)
      {
         for (int k = j + minD; k <= j + maxD;k++)
         {
            if (k <= 0 || k >= imageWidth -1)
            {

            }else {
                  pLeftPixel = (unsigned char*)leftImage->imageData + i*leftImage->widthStep + k;
                  pRightPixel= (unsigned char*)rightImage->imageData+i*rightImage->widthStep + j;
                  leftPixel  = *pLeftPixel;
                  rightPixel = *pRightPixel;
                  leftPixel  = *pLeftPixel;
                  tempLeft1 = (*pLeftPixel +(*(pLeftPixel -1)))/2;
                  tempLeft2 = (*pLeftPixel +(*(pLeftPixel +1)))/2;

                  leftMax = max(max(tempLeft1,tempLeft2),leftPixel);
                  leftMin = min(min(tempLeft1,tempLeft2),leftPixel);
                  difPixel = max(max(rightPixel - leftMax,leftMin - rightPixel),0);

                  if (difPixel <= max12Diff)
                  {
                     Ddynamic[j + 1][k + 1] = Ddynamic[j][k] + Probability[difPixel];
                  }else if (Ddynamic[j][k+1] > Ddynamic[j+1][k])
                  {
                     Ddynamic[j + 1][k + 1] = Ddynamic[j][k+1];
                  }else{
                     Ddynamic[j+1][k+1] = Ddynamic[j+1][k];
                  }

                  //cout<<Ddynamic[j +1][k+1]<<"  ";
            }

         }
         //cout<<"\n";
      }
      //逆向搜尋，找出最佳路徑
      m = imageWidth;
      n = imageWidth;
      int m2 = imageWidth, n2 = imageWidth;
      l = Ddynamic[imageWidth][imageWidth];
      while( m >= 1 && n >= 1)
      {
         if ((m2 == m + 1 && n2 >= n +1) || ( m2 > m +1 && n2 == n + 1))
         {
            pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + m;
            *pPixel = (n-m)*10;
            //標記有效匹配點
            pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + m;
            *pPixel = 255;

            m2 = m;
            n2 = n;

         }
         if (Ddynamic[m-1][n-1] >= Ddynamic[m][n -1] && Ddynamic[m-1][n -1] >= Ddynamic[m-1][n])
         {

            m--;
            n--;
         }else if (Ddynamic[m-1][n] >= Ddynamic[m][n -1] && Ddynamic[m-1][n] >= Ddynamic[m-1][n -1])
         {
            m--;
         }
         else
         {
            n--;
         }

      }

   //cvWaitKey(0);
}
int t2 = clock();
cout<<"dt: "<<t2-t1<<endl;

//顯示Ddynamic最後一行
/* for (int i = 0 ;i<= imageWidth;i++)
{
      for (int j= 0; j<= imageWidth;j++)
      {
         cout<<Ddynamic[j]<<"  ";
      }
      cout<<"\n\n";
}*/

//refine the depth image  7*7中值濾波
//統計未能匹配點的個數
int count = 0;
for (int i = 0 ;i< imageHeight;i++)
{
      for (int j= 0; j< imageWidth;j++)
      {
         pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + j;
         pixel = *pPixel;
         if (pixel == 0)
         {
            count++;
         }
      }
}

cout<<"Count:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;
cvShowImage("Depth",DPImage);
cvShowImage("effectiveImage",effectiveImage);
// cvWaitKey(0);

FilterImage = cvCloneImage(DPImage);

//7*7中值濾波
int halfMedianWindowSize = 3;
int medianWindowSize = 2*halfMedianWindowSize + 1;
int medianArray[100] = {0};
count = 0;
int temp = 0;
int medianVal = 0;

for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++)
{
      for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++)
      {
         pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + j;
         pixel = *pPixel;
         if (pixel == 0)
         {
            count = 0;
            for (int m = i - halfMedianWindowSize ; m <= i + halfMedianWindowSize ;m++)
            {
                  for (int n = j - halfMedianWindowSize; n <= j + halfMedianWindowSize ;n++)
                  {
                     pPixel2 = (unsigned char *)DPImage->imageData + m*DPImage->widthStep + n;
                     pixel2 = *pPixel2;
                     if (pixel2 != 0)
                     {
                           medianArray[count] = pixel2;
                           count++;
                     }

                  }
                  //排序
                  for (int k = 0; k< count;k++)
                  {
                     for (int l = k + 1; l< count;l++)
                     {
                        if (medianArray[l] < medianArray[l-1] )
                        {
                              temp = medianArray[l];
                              medianArray[l] = medianArray[l-1];
                              medianArray[l-1] = temp;
                        }
                     }
                  }
                  medianVal = medianArray[count/2];
                  pPixel = (unsigned char *)FilterImage->imageData + i*DPImage->widthStep + j;
                  *pPixel = medianVal;
            }

         }
      }
}
cvShowImage("Depth",DPImage);
cvShowImage("effectiveImage",effectiveImage);
cvShowImage("Filter",FilterImage);

cvSaveImage("depth.jpg",DPImage);
cvSaveImage("Filter.jpg",FilterImage);
cvSaveImage("effective.jpg",effectiveImage);

cvWaitKey(0);
return 0;
}
//效果

但是論文中多方向的DP卻看了好幾天都沒有頭緒該怎麼寫？因此這裡貼出進展，希望各位大俠能夠幫幫忙，共同學習。

opencvSGBM半全域性立體匹配演算法的研究

轉載請說明出處：http://blog.csdn.net/zhubaohua_bupt/article/details/51866700

自己在stereosgbm.cpp的最後加了一個把灰度影象顯示成偽彩色影象的函式，為的是更好的觀察視差圖。

[html] view plain copy

myStereoSGBM::GenerateFalseMap(cv::Mat &src, cv::Mat &disp)

下面分別給出 main.cpp、stereosgbm.h（從calib3d.hpp裡面提取出來，opencv版本2.4.9）、stereosgbm.cpp。配置完opencv環境後即可使用。有興趣的朋友可以研究一下程式碼。

main.cpp

[html] view plain copy

#include"sgbm.h"
int main()
{

Mat img111 = imread("E:/matchpic/testbanch_data/raindeer_left.pgm", 0);
Mat img222 = imread("E:/matchpic/testbanch_data/raindeer_right.pgm", 0);
imshow("left",img111);
waitKey(10);
cv::myStereoSGBM sgbm;
int SADWindowSize = 5;
sgbm.preFilterCap = 63 ;
sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
int cn = img111.channels();
int numberOfDisparities=128;
sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
sgbm.minDisparity = 0;
sgbm.numberOfDisparities = numberOfDisparities;
sgbm.uniquenessRatio = 10;
sgbm.speckleWindowSize =100;
sgbm.speckleRange = 10;
sgbm.disp12MaxDiff = 1;
Mat disp;
sgbm( img111, img222,disp );//disp經過處理輸出的是short型別
Mat disp8,color(disp.size(),CV_8UC3);
disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));//轉化成uchar顯示
sgbm.GenerateFalseMap(disp8,color);
imshow("",color);
imshow("",disp8);
waitKey(10);
}

stereosgbm.h

[html] view plain copy

<pre name="code" class="html"><pre name="code" class="html">#include"opencv2/opencv.hpp"
#include<iostream>
using namespace cv;
using namespace std;
namespace cv
{
class myStereoSGBM
{
public:
myStereoSGBM( int _minDisparity=0, int _numDisparities=0, int _SADWindowSize=0,
int _P1=0, int _P2=0, int _disp12MaxDiff=0, int _preFilterCap=0,
int _uniquenessRatio=0, int _speckleWindowSize=0, int _speckleRange=0,
bool _fullDP=false ) ;
// myStereoSGBM();
virtual void operator()(cv::InputArray _left, InputArray _right, OutputArray _disp ) ;
void GenerateFalseMap(cv::Mat &src, cv::Mat &disp);
~myStereoSGBM() ;
enum{DISP_SHIFT=4,DISP_SCALE=(1<<DISP_SHIFT)};
int minDisparity;
int numberOfDisparities;
int SADWindowSize;
int P1; int P2;
int disp12MaxDiff;
int preFilterCap;
int uniquenessRatio;
int speckleWindowSize;
int speckleRange;
bool fullDP ;
protected :
Mat buffer;
};
void filterSpeckles( InputOutputArray _img, double _newval, int maxSpeckleSize, double _maxDiff, InputOutputArray __buf ) ;
void validateDisparity( InputOutputArray _disp, InputArray _cost, int minDisparity, int numberOfDisparities, int disp12MaxDiff ) ;
CvRect cvGetValidDisparityROI( CvRect roi1, CvRect roi2, int minDisparity, int numberOfDisparities, int SADWindowSize ) ;
//void cvValidateDisparity( CvArr* _disp, const CvArr* _cost, int minDisparity, int numberOfDisparities, int disp12MaxDiff ) ;
}

stereosgbm.cpp

[html] view plain copy

<pre name="code" class="html">
/*M/////////////////////////////////////////////////////////////////////////////////
//////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this
license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote
products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are
disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
/*
This is a variation of
"Stereo Processing by Semiglobal Matching and Mutual Information"
by Heiko Hirschmuller.
We match blocks rather than individual pixels, thus the algorithm is called
SGBM (Semi-global block matching)
*/
//#include "precomp.hpp"
#include <limits.h>
#include"sgbm.h"
#include<fstream>
namespace cv
{
typedef unsigned char PixType;
typedef short CostType;
typedef short DispType;
enum { NR = 16 , NR2 = NR/2 }; //NR=16
//myStereoSGBM::myStereoSGBM()
//{
//minDisparity = numberOfDisparities = 0;
//SADWindowSize = 0;
//P1 = P2 = 0;
//disp12MaxDiff = 0;
//preFilterCap = 0;
//uniquenessRatio = 0;
//speckleWindowSize = 0;
//speckleRange = 0;
//fullDP = false;
//}
myStereoSGBM::myStereoSGBM( int _minDisparity, int _numDisparities, int _SADWindowSize, int _P1, int _P2, int _disp12MaxDiff,
int _preFilterCap, int _uniquenessRatio, int _speckleWindowSize, int _speckleRange, bool _fullDP )
{
minDisparity = _minDisparity;
numberOfDisparities = _numDisparities;
SADWindowSize = _SADWindowSize;
P1 = _P1;
P2 = _P2;
disp12MaxDiff = _disp12MaxDiff;
preFilterCap = _preFilterCap;
uniquenessRatio = _uniquenessRatio;
speckleWindowSize = _speckleWindowSize;
speckleRange = _speckleRange;
fullDP = _fullDP;
}
myStereoSGBM::~myStereoSGBM()
{
}
/*
For each pixel row1[x], max(-maxD, 0) <= minX <= x < maxX <= width - max(0, -minD),
and for each disparity minD<=d<maxD the function
computes the cost (cost[(x-minX)*(maxD - minD) + (d - minD)]), depending on the
difference between row1[x] and row2[x-d]. The subpixel algorithm from
"Depth Discontinuities by Pixel-to-Pixel Stereo" by Stan Birchfield and C. Tomasi //亞畫素插值
is used, hence the suffix BT.
the temporary buffer should contain width2*2 elements
*/
static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y, int minD, int maxD, CostType* cost,
PixType* buffer, const PixType* tab, int tabOfs, int )
{
int x, c, width = img1.cols, cn = img1.channels();
int minX1 = max(-maxD, 0), maxX1 = width + min(minD, 0);
int minX2 = max(minX1 - maxD, 0), maxX2 = min(maxX1 - minD, width);
int D = maxD - minD, width1 = maxX1 - minX1, width2 = maxX2 - minX2;
const PixType *row1 = img1.ptr<PixType>(y), *row2 = img2.ptr<PixType>(y);
PixType *prow1 = buffer + width2*2, *prow2 = prow1 + width*cn*2;
tab += tabOfs;
for( c = 0; c < cn*2; c++ )
{
prow1[width*c] = prow1[width*c + width-1] = prow2[width*c] = prow2[width*c + width-1] = tab[0];
}
int n1 = y > 0 ? -(int)img1.step : 0, s1 = y < img1.rows-1 ? (int)img1.step : 0;
int n2 = y > 0 ? -(int)img2.step : 0, s2 = y < img2.rows-1 ? (int)img2.step : 0;
if( cn == 1 )
{
for( x = 1; x < width-1; x++ )
{
prow1[x] = tab[(row1[x+1] - row1[x-1])*2 + row1[x+n1+1] - row1[x+n1-1] + row1[x+s1+1] - row1[x+s1-1]];
prow2[width-1-x] = tab[(row2[x+1] - row2[x-1])*2 + row2[x+n2+1] - row2[x+n2-1] + row2[x+s2+1] - row2[x+s2-1]];
prow1[x+width] = row1[x];
prow2[width-1-x+width] = row2[x];
}
}
else
{
for( x = 1; x < width-1; x++ )
{
prow1[x] = tab[(row1[x*3+3] - row1[x*3-3])*2 + row1[x*3+n1+3] - row1[x*3+n1-3] + row1[x*3+s1+3] - row1[x*3+s1-3]];
prow1[x+width] = tab[(row1[x*3+4] - row1[x*3-2])*2 + row1[x*3+n1+4] - row1[x*3+n1-2] + row1[x*3+s1+4] - row1[x*3+s1-2]];
prow1[x+width*2] = tab[(row1[x*3+5] - row1[x*3-1])*2 + row1[x*3+n1+5] - row1[x*3+n1-1] + row1[x*3+s1+5] - row1[x*3+s1-1]];
prow2[width-1-x] = tab[(row2[x*3+3] - row2[x*3-3])*2 + row2[x*3+n2+3] - row2[x*3+n2-3] + row2[x*3+s2+3] - row2[x*3+s2-3]];
prow2[width-1-x+width] = tab[(row2[x*3+4] - row2[x*3-2])*2 + row2[x*3+n2+4] - row2[x*3+n2-2] + row2[x*3+s2+4] - row2[x*3+s2-2]];
prow2[width-1-x+width*2] = tab[(row2[x*3+5] - row2[x*3-1])*2 + row2[x*3+n2+5] - row2[x*3+n2-1] + row2[x*3+s2+5] - row2[x*3+s2-1]];
prow1[x+width*3] = row1[x*3];
prow1[x+width*4] = row1[x*3+1];
prow1[x+width*5] = row1[x*3+2];
prow2[width-1-x+width*3] = row2[x*3];
prow2[width-1-x+width*4] = row2[x*3+1];
prow2[width-1-x+width*5] = row2[x*3+2];
}
}
memset( cost, 0, width1*D*sizeof(cost[0]) );
buffer -= minX2;
cost -= minX1*D + minD; // simplify the cost indices inside the loop
/*#if CV_SSE2
volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
#endif*/
#if 1
for( c = 0; c < cn*2; c++, prow1 += width, prow2 += width )
{
int diff_scale = c < cn ? 0 : 2; //對預處理之後的影象和原影象生成的代價加權相加 C=Cpreprocess+1/4*C原圖
// precompute
// v0 = min(row2[x-1/2], row2[x], row2[x+1/2]) and
// v1 = max(row2[x-1/2], row2[x], row2[x+1/2]) and
for( x = minX2; x < maxX2; x++ )
{
int v = prow2[x];
int vl = x > 0 ? (v + prow2[x-1])/2 : v;
int vr = x < width-1 ? (v + prow2[x+1])/2 : v;
int v0 = min(vl, vr); v0 = min(v0, v);
int v1 = max(vl, vr); v1 = max(v1, v);
buffer[x] = (PixType)v0;
buffer[x + width2] = (PixType)v1;
}
for( x = minX1; x < maxX1; x++ )
{
int u = prow1[x];
int ul = x > 0 ? (u + prow1[x-1])/2 : u;
int ur = x < width-1 ? (u + prow1[x+1])/2 : u;
int u0 = min(ul, ur); u0 = min(u0, u);
int u1 = max(ul, ur); u1 = max(u1, u);
//#if CV_SSE2
//if( useSIMD )
//{
//__m128i _u = _mm_set1_epi8((char)u), _u0 = _mm_set1_epi8((char)u0);
//__m128i _u1 = _mm_set1_epi8((char)u1), z = _mm_setzero_si128();
//__m128i ds = _mm_cvtsi32_si128(diff_scale);
//for( int d = minD; d < maxD; d += 16 )
//{
//__m128i _v = _mm_loadu_si128((const __m128i*)(prow2 + width-x-1 + d));
//__m128i _v0 = _mm_loadu_si128((const __m128i*)(buffer + width-x-1 + d));
//__m128i _v1 = _mm_loadu_si128((const __m128i*)(buffer + width-x-1 + d + width2));
//__m128i c0 = _mm_max_epu8(_mm_subs_epu8(_u, _v1), _mm_subs_epu8(_v0, _u));
//__m128i c1 = _mm_max_epu8(_mm_subs_epu8(_v, _u1), _mm_subs_epu8(_u0, _v));
//__m128i diff = _mm_min_epu8(c0, c1);
//c0 = _mm_load_si128((__m128i*)(cost + x*D + d));
//c1 = _mm_load_si128((__m128i*)(cost + x*D + d + 8));
//_mm_store_si128((__m128i*)(cost + x*D + d), _mm_adds_epi16(c0, _mm_srl_epi16
//
//(_mm_unpacklo_epi8(diff,z), ds)));
//_mm_store_si128((__m128i*)(cost + x*D + d + 8), _mm_adds_epi16(c1, _mm_srl_epi16
//
//(_mm_unpackhi_epi8(diff,z), ds)));
//}
//}
//else
//#endif
{
for( int d = minD; d < maxD; d++ )
{
int v = prow2[width-x-1 + d];
int v0 = buffer[width-x-1 + d];
int v1 = buffer[width-x-1 + d + width2];
int c0 = max(0, u - v1); c0 = max(c0, v0 - u);
int c1 = max(0, v - u1); c1 = max(c1, u0 - v);
//cout<<cost[x*D + d]<<endl;
cost[x*D + d] = (CostType)(cost[x*D+d] + (min(c0, c1) >> diff_scale)); //代價由預處理後的影象和原圖兩部分共同組成
//if(c==0) continue;
//else
//cost[x*D + d] = (CostType)( (min(c0, c1) >> diff_scale));
// cout<<" +="<<cost[x*D + d]<<endl;
}
}
}
}
#else
for( c = 0; c < cn*2; c++, prow1 += width, prow2 += width )
{
for( x = minX1; x < maxX1; x++ )
{
int u = prow1[x];
#if CV_SSE2
if( useSIMD )
{
__m128i _u = _mm_set1_epi8(u), z = _mm_setzero_si128();
for( int d = minD; d < maxD; d += 16 )
{
__m128i _v = _mm_loadu_si128((const __m128i*)(prow2 + width-1-x + d));
__m128i diff = _mm_adds_epu8(_mm_subs_epu8(_u,_v), _mm_subs_epu8(_v,_u));
__m128i c0 = _mm_load_si128((__m128i*)(cost + x*D + d));
__m128i c1 = _mm_load_si128((__m128i*)(cost + x*D + d + 8));
_mm_store_si128((__m128i*)(cost + x*D + d), _mm_adds_epi16(c0, _mm_unpacklo_epi8
(diff,z)));
_mm_store_si128((__m128i*)(cost + x*D + d + 8), _mm_adds_epi16(c1,
_mm_unpackhi_epi8(diff,z)));
}
}
else
#endif
{
for( int d = minD; d < maxD; d++ )
{
int v = prow2[width-1-x + d];
cost[x*D + d] = (CostType)(cost[x*D + d] + (CostType)std::abs(u - v));
}
}
}
}
#endif
}
/*
computes disparity for "roi" in img1 w.r.t. img2 and write it to disp1buf.
that is, disp1buf(x, y)=d means that img1(x+roi.x, y+roi.y) ~ img2(x+roi.x-d, y+roi.y).
minD <= d < maxD.
disp2full is the reverse disparity map, that is:
disp2full(x+roi.x,y+roi.y)=d means that img2(x+roi.x, y+roi.y) ~ img1(x+roi.x+d, y+roi.y)
note that disp1buf will have the same size as the roi and
disp2full will have the same size as img1 (or img2).
On exit disp2buf is not the final disparity, it is an intermediate result that
becomes
final after all the tiles are processed.
the disparity in disp1buf is written with sub-pixel accuracy
(4 fractional bits, see CvmyStereoSGBM::DISP_SCALE),
using quadratic interpolation, while the disparity in disp2buf
is written as is, without interpolation.
disp2cost also has the same size as img1 (or img2).
It contains the minimum current cost, used to find the best disparity, corresponding
to the minimal cost.
*/
static void computeDisparitySGBM( const Mat& img1, const Mat& img2, Mat& disp1, const myStereoSGBM& params, Mat& buffer )
{
//#if CV_SSE2/*
// static const uchar LSBTab[] =
// {
// 0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0,
//
// 2, 0, 1, 0,
// 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0,
//
// 2, 0, 1, 0,
// 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0,
//
// 2, 0, 1, 0,
// 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0,
//
// 2, 0, 1, 0,
// 7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0,
//
// 2, 0, 1, 0,
// 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0,
//
// 2, 0, 1, 0,
// 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0,
//
// &

【視覺-立體視覺】全域性匹配演算法SGBM實現（含動態規劃DP）詳解

opencvSGBM半全域性立體匹配演算法的研究

【視覺-立體視覺】全域性匹配演算法SGBM實現（含動態規劃DP）詳解

【Web】阿里icon圖示webpack外掛（webpack-qc-iconfont-plugin）詳解

常見三維重建演算法及其實現（含原始碼下載連結）

【演算法之動態規劃（一）】動態規劃（DP）詳解

【短道速滑八】文字影象背景純淨化（含光照不均勻圖）演算法初探。

【達內課程】音樂播放器4.0（播放詳情頁下）

【DP計劃】11.3——[BZOJ]股票交易（單調佇列優化DP）MEDIUM

【DP】ssl 1010 方格取數（多執行緒DP）

【洛谷P4315】月下“毛景樹”（樹鏈剖分）

【C#專案實戰】學生社團資訊管理系統（MVC三層架構）

原始碼學習【原子類AtomicInteger】Java原子類底層實現（解釋詳細）

HPUoj 題目1019 【C語言訓練】尼科徹斯定理（水題，數學）

【機器學習-斯坦福】學習筆記21——增強學習（Reinforcement Learning and Control）

【真實面試經歷】我和阿里面試官的一次“邂逅”(附問題詳解)

【python實現卷積神經網路】卷積層Conv2D實現（帶stride、padding）

隱馬爾科夫演算法之實現簡易版的拼音輸入法程式碼詳解

Dijkstra演算法 java實現（含測試）

機器學習與資料探勘-K最近鄰(KNN)演算法的實現（java和python版）

藍橋杯 ALGO-3 演算法訓練 K好數（動態規劃 DP）

【視覺-立體視覺】立體匹配幾種演算法效能比較

【視覺-立體視覺】全域性匹配演算法SGBM實現（含動態規劃DP）詳解

opencvSGBM半全域性立體匹配演算法的研究

相關推薦