立体视觉算法SAD+DP

#include <cstdio>
#include <cstring>
#include <iostream>
#include<cv.h>
#include<highgui.h>
#include <cmath>

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

int  main()
{  
    //打开文件
    FILE* pFile;
    if (pFile = fopen("data.txt","a"))
    {
        //cout <<"File Open Success"<<endl; 
    }else{
        //cout <<"File Open Failed"<<end;
        return 0;
    }
    // IplImage * leftImage = cvLoadImage("l1.png",0);
    // IplImage * rightImage = cvLoadImage("r1.png",0);

    //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 * leftDepth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
    IplImage * rightDepth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

    IplImage * leftValid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
    IplImage * rightValid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

    IplImage * leftFilter = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
    IplImage * rightFilter = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

    IplImage * depth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
    IplImage * valid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

    unsigned char * pPixel = NULL;
    unsigned char  pixel;
    unsigned char * pPixel2 = NULL;
    unsigned char  pixel2;
    cvZero(leftDepth);
    cvZero(rightDepth);
    cvZero(leftValid);
    cvZero(rightValid);
    cvZero(leftFilter);
    cvZero(rightFilter);
    cvZero(depth);
    cvZero(valid);



    CvSize SADWindowSize = cvSize(5,5);  //(widh,height)
    int SW2 = SADWindowSize.width/2;
    int SH2 = SADWindowSize.height/2;
    cout<<"SH2:  "<<SH2<<endl;
    cout<<"SW2:  "<<SW2<<endl;

    int minD = 0;
    int maxD = 15;
    //假设图像是经过矫正的，那么每次都只是需要搜搜同一行的内容
    int max12Diff = 8*SADWindowSize.width*SADWindowSize.height;

    for (int i = 0;i < imageWidth;i++)
    {
        Ddynamic[0][i] = 0;
        Ddynamic[i][0] = 0;
    }
    unsigned char * pLeftPixel  = NULL;
    unsigned char * pRightPixel = NULL;
    unsigned char leftPixel = 0;
    unsigned char rightPixel =0;
    int m,n,l;
    int difPixel = 0;
    int t1 = clock();
    for (int i = SH2 ; i < imageHeight -SH2;i++)
    {
        for (int j = SW2; j<imageWidth - SW2;j++)
        {
            for (int k = j + minD; k <= j + maxD;k++)
            {
                difPixel = 0;
                for (m = -SH2;m <= SH2;m++)
                {
                    for (n = -SW2; n <= SW2;n++)
                    {
                        pRightPixel= (unsigned char*)rightImage->imageData+
                            (i+m)*rightImage->widthStep + j+n;
                        rightPixel = *pRightPixel;
                        if (k < SW2  || k  >= imageWidth -SW2 )
                        {
                            difPixel += rightPixel;
                        }else {
                            pLeftPixel = (unsigned char*)leftImage->imageData + 
                                (i+m)*leftImage->widthStep + k+n;  
                            leftPixel  = *pLeftPixel; 
                            difPixel += abs(leftPixel - rightPixel);
                        }

                    }
                }
                if (difPixel <= max12Diff)
                {
                    Ddynamic[j + 1][k + 1] = Ddynamic[j][k] +1; 
                }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 - SW2;
        n = imageWidth - SW2;
        int m2 = m, n2 = n;

        while( m >= 1 && n >= 1)
        {
            if ((m2 == m + 1 && n2 >= n +1) || ( m2 > m +1 && n2 == n + 1))
            {
                pPixel = (unsigned char *)rightDepth->imageData + i*rightDepth->widthStep + m;
                *pPixel = (n-m)*10;
                //标记有效匹配点
                pPixel = (unsigned char *)rightValid->imageData + i*rightValid->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;
    //统计未能匹配点的个数
    int count = 0;
    for (int i = 0 ;i< imageHeight;i++)
    {
        for (int j= 0; j< imageWidth;j++)
        {
            pPixel = (unsigned char *)rightValid->imageData + i*rightValid->widthStep + j;
            pixel = *pPixel;
            if (pixel == 0)
            {
                count++;
            }
        }
    }
    cout<<"rightCount:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;
    rightFilter = cvCloneImage(rightDepth);
    //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 *)rightValid->imageData + i*rightValid->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 *)rightDepth->imageData + m*rightDepth->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 *)rightFilter->imageData + i*rightFilter->widthStep + j;
                    *pPixel = medianVal;
                }

            }
        }
    }

    //两次进行DP计算，分别是左对右和右对左,从右想左扫描
    for (int j = 0; j<= imageWidth;j++)
    {
        for (int k = 0;k <= imageWidth;k++)
        {
            Ddynamic[j][k] = 0;
        }
    }
    minD = -maxD;
    maxD = 0;
  
    for (int i = SH2 ; i < imageHeight -SH2;i++)
    {
        for (int j = imageWidth-SW2-1; j >= SW2;j--)
        {
            for (int k = j + maxD; k >= j + minD;k--)
            {
                difPixel = 0;
                for (m = -SH2;m <= SH2;m++)
                {
                    for (n = -SW2; n <= SW2;n++)
                    {
                        pLeftPixel = (unsigned char*)leftImage->imageData + 
                                     (i+m)*leftImage->widthStep + j+n;
                        leftPixel  = *pLeftPixel;
                        if (k < SW2 || k  >= imageWidth -SW2)
                        {
                             difPixel += leftPixel;
                        }else {
                            
                            pRightPixel= (unsigned char*)rightImage->imageData+
                                         (i+m)*rightImage->widthStep + k+n; 
                            rightPixel = *pRightPixel;
                            difPixel += abs(leftPixel - rightPixel);
                            //cout<<Ddynamic[j +1][k+1]<<"  ";
                        }
                    }
                }

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

            }
            //cout<<"\n";
        }
        //逆向搜索，找出最佳路径

        m = 0;
        n = 0;
        int m2 = m, n2 = n;
        while( m < imageWidth && n < imageWidth)
        {
            if ((m == m2 + 1 && n >= n2 +1) || ( m > m2 +1 && n == n2 + 1))
            {
                pPixel = (unsigned char *)leftDepth->imageData + i*leftDepth->widthStep + m;
                *pPixel = (m-n)*10;
                //标记有效匹配点
                pPixel = (unsigned char *)leftValid->imageData + i*leftValid->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 t3 = clock();
    //refine the depth image  7*7中值滤波
    //统计未能匹配点的个数
    count = 0;
    for (int i = 0 ;i< imageHeight;i++)
    {
        for (int j= 0; j< imageWidth;j++)
        {
            pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + j;
            pixel = *pPixel;
            if (pixel == 0)
            {
                count++;
            }
        }
    }

    cout<<"Left Count:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;

    //将关键信息计入文件
    fprintf(pFile,"\nDP:\nClock:   %dms",t3-t1);
    fprintf(pFile,"\nInvalid Point:   %f",(double)count/(imageWidth*imageHeight));
    // cvWaitKey(0);
    leftFilter = cvCloneImage(leftDepth);
    //7*7中值滤波
    count = 0;
    for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++)
    {
        for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++)
        {
            pPixel = (unsigned char *)leftValid->imageData + i*leftValid->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 *)leftDepth->imageData + m*leftDepth->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 *)leftFilter->imageData + i*leftFilter->widthStep + j;
                    *pPixel = medianVal;
                }

            }
        }
    }
    //假如已知左右图片的深度，进行refinement
    unsigned char dLeft = 0, dRight = 0;
    count = 0;
    for (int i = 0; i< imageHeight;i++)
    {
        for (int j = 0; j< imageWidth;j++)
        {
            pRightPixel= (unsigned char*)rightDepth->imageData + i*rightDepth->widthStep + j;
            dRight = (*pRightPixel)/10;
            if (j + dRight < imageWidth)
            {    
                pPixel= (unsigned char*)depth->imageData + i*depth->widthStep + j;
                pLeftPixel= (unsigned char*)leftDepth->imageData + i*leftDepth->widthStep + j + dRight;
                dLeft = (*pLeftPixel)/10;
                if (abs(int(dRight) -(int)dLeft) <= 1)
                {
                    //深度一致  
                    *pPixel = dRight*10;
                    
                }else{
                    //深度不一致，认为是误匹配、错误匹配
                    *pPixel = 0;
                    count++;
                }
            }
        }
    }
    cout<<"depth Count:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;


    cvNamedWindow("leftImage",1);
    cvNamedWindow("rightImage",1);
    cvNamedWindow("leftDepth",1);
    cvNamedWindow("leftValid",1);
    cvNamedWindow("rightDepth",1);
    cvNamedWindow("rightValid",1);
    cvNamedWindow("leftFilter",1);
    cvNamedWindow("rightFilter",1);
    cvNamedWindow("depth",1);

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

    cvShowImage("leftDepth",leftDepth);
    cvShowImage("rightDepth",rightDepth);

    cvShowImage("leftValid",leftValid); 
    cvShowImage("rightValid",rightValid);

    cvShowImage("leftFilter",leftFilter);
    cvShowImage("rightFilter",rightFilter);
    cvShowImage("depth",depth);

    cvSaveImage("leftDepth.jpg",leftDepth);
    cvSaveImage("rightDepth.jpg",rightDepth);
    cvSaveImage("leftValid.jpg",leftValid);
    cvSaveImage("rightValid.jpg",rightValid);
    cvSaveImage("leftFilter.jpg",leftFilter);
    cvSaveImage("rightFilter.jpg",rightFilter);
    cvSaveImage("depth.jpg",depth);



    fclose(pFile);
    cvWaitKey(0);
    return 0;
}