SAD立体匹配算法在opencv中的实现

FROM：http://blog.csdn.net/tulun/article/details/6456188

SAD算法具体原理见相关图像处理书籍。

该程序是opencv中文论坛的牛人贡献的，感谢他的工作。

 

（程序所需图片可以在网上找如http://vision.middlebury.edu/stereo/data/scenes2003/）

// Sum of Absolute Difference(SAD)

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <math.h>
#include <ctime>

using namespace std;

template<class T> class Image
{
private:
    IplImage* imgp;

public:
    Image(IplImage* img=0){imgp=img;}
    ~Image(){imgp=0;}
    void operator=(IplImage* img){imgp=img;}
    inline T* operator[](const int rowIndx)
    {
        return((T*)(imgp->imageData+rowIndx*imgp->widthStep));
    }
};

typedef struct
{
    unsigned char b,g,r;
}RgbPixel;

typedef struct
{
    float b,g,r;
}RgbPixelFloat;

typedef Image<RgbPixel> RgbImage;
typedef Image<RgbPixelFloat> RgbImageFloat;
typedef Image<unsigned char> BwImage;
typedef Image<float> BwImageFloat;

//display an image in a new window with title to be given.
void displayImageNewWindow(char* title,CvArr* img)
{
    cvNamedWindow(title, CV_WINDOW_AUTOSIZE );
    cvShowImage(title,img);
}

int getMaxMin(double value[],int valueSize, int maxmin)
{
    int pos=0;
    int i=0;
    double max1=-1;//?-999999;
    double min1=999999;

    if (maxmin==1)
    {
        //find max
        for (i=0;i<valueSize;i++)
        {
            //find the index with the max value;
            if (value[i]>max1)
            {
                pos=i;
                max1=value[i];
            }
        }
    }

    if (maxmin==0)
    {
        //find min
        for (i=0;i<valueSize;i++)
        {
            //find the index with the minimum value;
            if (value[i]<min1)
            {
                pos=i;
                min1=value[i];
            }
        }
    }

    return pos;
}

IplImage* generateDisparityImage(IplImage* greyLeftImg32,IplImage* greyRightImg32,int windowSize,int DSR)
{
    int offset=floor((double)windowSize/2);
    int height=greyLeftImg32->height;
    int width=greyLeftImg32->width;
    double* localSAD=new double[DSR];

    int x=0, y=0,d=0,m=0;
    int N=windowSize;

    IplImage* winImg=cvCreateImage(cvSize(N,N),32,1);//mySubImage(greyLeftImg32,cvRect(0,0,N,N));

    IplImage* disparity=cvCreateImage(cvSize(width,height),8,1);//or IPL_DEPTH_8U
    BwImage imgA(disparity);

    for (y=0;y<height;y++)
    {
      for (x=0;x<width;x++)
      {
         imgA[y][x]=0;
      }
    }

    CvScalar sum;
    //CvScalar s2;
    for (y=0;y<height-N;y++)
    {
        //height-N
        for (x=0;x<width-N;x++)
        {
            //width-N
            cvSetImageROI(greyLeftImg32, cvRect(x,y,N,N));
            d=0;
            //initialise localSAD
            for (m=0;m<DSR;m++)
            {
                localSAD[m]=0;
            }

            //start matching
             do{
                if (x-d>=0)
                {
                    cvSetImageROI(greyRightImg32, cvRect(x-d,y,N,N));
                }
                else
                {
                break;
                }

                cvAbsDiff(greyLeftImg32,greyRightImg32,winImg);//absolute difference
                sum=cvSum(winImg);//sum
                localSAD[d]=sum.val[0];//0 means single channel

                cvResetImageROI(greyRightImg32);
                d++;
            }while(d<=DSR);

            //to find the best d and store
            imgA[y+offset][x+offset]=getMaxMin(localSAD,DSR,0)*16; //0 means return minimum index
            cvResetImageROI(greyLeftImg32);
        }//x
        if (y%10==0)
            cout<<"row="<<y<<" of "<<height<<endl;
    }//y

    cvReleaseImage(&winImg);
    //cvReleaseImage(&rightWinImg);

    return disparity;
}

int main (int argc, char * const argv[])
{
    cout << "Sum of Absolute Difference(SAD) Strereo Vision"<<endl;

    //**********image input*********************//
    char* filename1="L.jpg";//im2_cone.png
    IplImage* greyLeftImg= cvLoadImage(filename1,0);
    char* filename2="R.jpg";
    IplImage* greyRightImg= cvLoadImage(filename2,0);

    if (greyLeftImg==NULL){cout << "No valid image input."<<endl; return 1;}
    if (greyRightImg==NULL){cout << "No valid image input."<<endl; return 1;}

    int width=greyLeftImg->width;
    int height=greyLeftImg->height;

    /****************8U to 32F**********************/
    IplImage* greyLeftImg32=cvCreateImage(cvSize(width,height),32,1);//IPL_DEPTH_32F
    IplImage* greyRightImg32=cvCreateImage(cvSize(width,height),32,1);
    cvConvertScale(greyLeftImg, greyLeftImg32, 1/255.);
    cvConvertScale(greyRightImg, greyRightImg32, 1/255.);//1/255. equals to 1/255.0

    //-------------obtain disparity image----------------
    time_t tstart, tend;
    tstart = time(0);
    int windowSize=13,DSR=20;//Disparity Search Range
    IplImage* disparity32=generateDisparityImage(greyLeftImg32,greyRightImg32,windowSize,DSR);
    tend = time(0);
    cout << "It took "<< difftime(tend, tstart) <<" second(s)."<< endl;

    displayImageNewWindow("Dispairty Image",disparity32);
    displayImageNewWindow("Left Image",greyLeftImg32);
    displayImageNewWindow("Right Image",greyRightImg32);

    //cvSaveImage("D:/OpenCV_stuff/SampleImages/disparitySAD.jpg",disparity32);

    //********destroy window************/
    cvWaitKey(0);
    cvReleaseImage(&greyLeftImg32);
    cvReleaseImage(&greyRightImg32);
    cvReleaseImage(&greyLeftImg);
    cvReleaseImage(&greyRightImg);
    cvReleaseImage(&disparity32);
    cvDestroyWindow("Left Image");
    cvDestroyWindow("Right Image");
    cvDestroyWindow("Dispairty Image");
    return 0;
}