立体视觉--全局匹配算法SGBM实现

6091820503

最近一直在学习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(&quot;left.png&quot;,0);
    IplImage * img2 = cvLoadImage(&quot;right.png&quot;,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<<&quot;Time elapsed:&quot;<<t*1000/getTickFrequency()<<endl;
        disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));
       
        namedWindow(&quot;left&quot;, 1);
        cvShowImage(&quot;left&quot;, img1);
        namedWindow(&quot;right&quot;, 1);
        cvShowImage(&quot;right&quot;, img2);
        namedWindow(&quot;disparity&quot;, 1);
        imshow(&quot;disparity&quot;, disp8);
        waitKey();
        imwrite(&quot;sgbm_disparity.png&quot;, 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(&quot;l2.jpg&quot;,0);
    IplImage * rightImage = cvLoadImage(&quot;r2.jpg&quot;,0);

    //IplImage * leftImage = cvLoadImage(&quot;left.bmp&quot;,0);                           
    //IplImage * rightImage = cvLoadImage(&quot;right.bmp&quot;,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(&quot;Left&quot;,1);
    cvNamedWindow(&quot;Right&quot;,1);
    cvNamedWindow(&quot;Depth&quot;,1);
    cvNamedWindow(&quot;effectiveImage&quot;,1);

    cvShowImage(&quot;Left&quot;,leftImage);
    cvShowImage(&quot;Right&quot;,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]<<&quot;  &quot;;
                }
               
            }
             //cout<<&quot;\n&quot;;
        }
        //逆向搜索，找出最佳路径
         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<<&quot;dt: &quot;<<t2-t1<<endl;

    //显示Ddynamic最后一行
   /* for (int i = 0 ;i<= imageWidth;i++)
    {
        for (int j= 0; j<= imageWidth;j++)
        {
           cout<<Ddynamic[j]<<&quot;  &quot;;
        }
        cout<<&quot;\n\n&quot;;
    }*/


    //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<<&quot;Count:  &quot;<<count<<&quot;  &quot;<<(double)count/(imageWidth*imageHeight)<<endl;
    cvShowImage(&quot;Depth&quot;,DPImage);
    cvShowImage(&quot;effectiveImage&quot;,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(&quot;Depth&quot;,DPImage);
    cvShowImage(&quot;effectiveImage&quot;,effectiveImage);
    cvShowImage(&quot;Filter&quot;,FilterImage);

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

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



但是论文中多方向的DP却看了好几天都没有头绪该怎么写？因此这里贴出进展，希望各位大侠能够帮帮忙，共同学习。

6091820503

多方向的DP算法，确实不知该怎么写，希望做过的大侠给点建议啊~~~

6091820503

都快沉了，也没人回复，。好蛋疼啊~

yueguangguan

你好，你还在研究sgbm吗

psdto3d33

楼主真厉害，深度信息很准确。
学习了，多谢分享。

nxf1988

楼主你好，最近我也在看半全局的方法，有点云里雾里，搞不懂半全局究竟和全局算法有什么差别，望赐教

zhao20090580237

这段时间这研究半全局匹配，有木有大牛知道基于互信息的，多个方向的DP的算法，求指导啊。

bblr001

custchen 发表于 2013-8-2 11:04
楼主还在做这个算法吗，进行的怎么样了？多方向和水平方向的dp没什么大区别的，下面是我用cuda做的，还有些 ...

钟形曲线应该是高斯分布

world_hope

学习了，mark

y06480509

custchen 发表于 2015-4-29 14:45
今天才看到楼主回的帖子，时光又过去了将近半年，下面是我在13年11月前，用ADCensus实现的效果，ADCensu算 ...

你好，请问你现在还在从事裸眼3D方面的吗？我现在也是做裸眼3D方面的。