SGM半全局匹配算法求助

SGM半全局匹配算法求助

SGM半全局匹配算法用c++实现的代码如下，仅供参考（vs中编译通过）。
#include "stdafx.h"
#include 
#include 
#include 
#include
#include
#include 
using namespace std;
const int Width = 1024;
const int Height = 1024;
int Lvalue[Width][Width];
uchar C(int x, int y, int d,  IplImage * matchImage, IplImage * baseImage)
{
    uchar * pMatchPixel = NULL;
    uchar * pBasePixel = NULL;
    uchar matchPixel = 0;
    uchar basePixel =0;
    uchar matchMax = 0;
    uchar matchMin = 0;
    uchar tempMatch1 = 0;
    uchar tempMatch2 = 0;
    uchar difPixel = 0;
    if (x+d <= matchImage->width && x+d >= 0)
    {
        pMatchPixel = (uchar *)matchImage->imageData + y*matchImage->widthStep + (x+d);
        matchPixel = *pMatchPixel;
        pBasePixel= (uchar *)baseImage->imageData + y*baseImage->widthStep + x;
        basePixel = *pBasePixel;
         //匹配影像上的像素点与其左、右像素点的平均值,线性插值的方法
         tempMatch1 = (*pMatchPixel +(*(pMatchPixel -1)))/2;
         tempMatch2 = (*pMatchPixel +(*(pMatchPixel +1)))/2;
         matchMax = max(max(tempMatch1,tempMatch2),matchPixel);
         matchMin = min(min(tempMatch1,tempMatch2),matchPixel);
         //赋予视差d时的匹配代价C
         //BT法
         difPixel = max(max(basePixel - matchMax, matchMin - basePixel),0);
        //AD法
        //difPixel = abs(basePixel - matchPixel);
        return difPixel;
    }
    else
        return 255;
}
int main()
{
    IplImage * leftImage = cvLoadImage("headL.png",0);
    IplImage * rightImage = cvLoadImage("headR.png",0);
    int imageWidth = leftImage->width;
    int imageHeight =leftImage->height;
    int minLvalue = 1000;
    int minL1 = 1000;
    int minL2 = 1000;
    int P1 = 2;
    int P2 = 5;
    int disparity= 0;
    int minDis = 0;
    int maxDis = 21;
    int scale = 12;
    unsigned char * pPixel = NULL;
 #pragma  region 实现横向DP（以右影像为准,0度方向）
     IplImage * MyDPImage_0 = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
     cvZero(MyDPImage_0);
     int t3 = clock();
     for (int i = 0; i < imageHeight;i++)
     {
         for (int j = 0; j          {
             disparity = 0;
             minL1 = 1000;
             minL2 = 1000;
             for (int d = minDis; d <= maxDis; d++)
             {
                 //初始化为代价函数的值
                 Lvalue[j][d] = C(j, i, d, leftImage, rightImage); 
                 if (j > 0)
                  {
                      minL1 = min(minL1, Lvalue[j-1][d]);
                  }
             }
             for (int d = minDis; d <= maxDis; d++)
             {
                 if (j > 0)
                 {
                     minL2 = min(minL2,  Lvalue[j-1][d]);
                     minL2 = min(minL2, (Lvalue[j-1][d+1] + P1));
                     minL2 = min(minL2, (Lvalue[j-1][d-1] + P1));
                     minL2 = min(minL2, (minL1 + P2));
                     Lvalue[j][d] =  Lvalue[j][d] + (minL2 - minL1);   
                 }  
             }
             int max=Lvalue[j][0];
             for(int d=minDis;d<=maxDis;++d)
             {
                 if(Lvalue[j][d]                  {
                     disparity=d;
                     max=Lvalue[j][d];
                 }
             }
             disparity=disparity*scale;
             //生成视差图
             pPixel = (uchar *)MyDPImage_0->imageData + i*MyDPImage_0->widthStep + j;
             *pPixel =disparity;
         }        
     }
     int t4 = clock();
     cout<<"横向DP共用时: "<      cvNamedWindow("MyDPImage_0", 1);
     cvShowImage("MyDPImage_0", MyDPImage_0);
     cvSaveImage("MyDPImage_0.jpg", MyDPImage_0);
 #pragma endregion
 #pragma  region 实现横向DP（以左影像为准，0度方向）
     IplImage * MyDPImage_0_L = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
     cvZero(MyDPImage_0_L);
     for (int i = 0; i < imageHeight;i++)
     {
         for (int j = 0; j          {
             disparity = 0;
             minL1 = 1000;
             minL2 = 1000;
             for (int d = minDis; d <= maxDis; d++)
             {
                 //初始化为代价函数的值
                 Lvalue[j][d] = C(j, i, -d, rightImage, leftImage);             
                 if (j > 0)
                 {
                     minL1 = min(minL1, Lvalue[j-1][d]);
                 }
             }
             for (int d = minDis; d <= maxDis; d++)
             {
                 if (j > 0)
                 {
                     minL2 = min(minL2,  Lvalue[j-1][d]);
                     minL2 = min(minL2, (Lvalue[j-1][d+1] + P1));
                     minL2 = min(minL2, (Lvalue[j-1][d-1] + P1));
                     minL2 = min(minL2, (minL1 + P2));
                     Lvalue[j][d] = Lvalue[j][d] + minL2 - minL1;                                                          
                 }  
             }
             int max=Lvalue[j][0];
             for(int d=0;d<=maxDis;++d)
             {
                 if(Lvalue[j][d]                  {
                     disparity=d;
                     max=Lvalue[j][d];
                 }
             }
             disparity = disparity * scale;
             //生成视差图
             pPixel = (uchar *)MyDPImage_0_L->imageData + i*MyDPImage_0_L->widthStep + j;
             *pPixel = disparity;
         }
     }
     cvNamedWindow("MyDPImage_0_L", 1);
     cvShowImage("MyDPImage_0_L", MyDPImage_0_L);
     cvSaveImage("MyDPImage_0_L.jpg", MyDPImage_0_L); 
 #pragma endregion
 #pragma  region 一致性检查
     uchar * pLeftDepthPixel = NULL;
     uchar * pRightDepthPixel = NULL;
     uchar leftDepthPixel = 0;
     uchar rightDepthPixel =0;
     uchar difDepthPixel = 0;
     IplImage * CheckImage_0 = cvCloneImage(MyDPImage_0);
     cvZero(CheckImage_0);
     for (int i = 0; i < imageHeight; i++)
     {
         for(int j = 0; j < imageWidth; j++)
         {
             pRightDepthPixel = (uchar *)MyDPImage_0->imageData + i*MyDPImage_0->widthStep + j;
             rightDepthPixel = *pRightDepthPixel;
             if(j + rightDepthPixel/scale < imageWidth)
             {
                 pLeftDepthPixel = (uchar *)MyDPImage_0_L->imageData + i*MyDPImage_0_L->widthStep + j + rightDepthPixel/scale;
                 leftDepthPixel = *pLeftDepthPixel;
             }
             else
                 leftDepthPixel = 0;
difDepthPixel = abs((leftDepthPixel - rightDepthPixel)/scale);
             pPixel = (uchar *)CheckImage_0->imageData + i * CheckImage_0->widthStep +j;
             if (difDepthPixel <= 1)
             {
                 *pPixel = rightDepthPixel;
             }
             else
             {
                 //否则，当前像素的视差值为零
                 *pPixel = 0;
             }
         }
     }
     int lp,rp; 
     int lx,rx;
     for(int i=0;i      {
         for(int j=0;j          {  
             int tem = *((uchar *)CheckImage_0->imageData+i*CheckImage_0->widthStep + j);  
             if(tem==0)  
             {    
                 lp = rp = 0;   
                 lx = j;  
                 rx = j;  
                 if(lx-1<0)  
                     lp= *((uchar *)CheckImage_0->imageData+i*CheckImage_0->widthStep + lx);  
                 while((lp==0)&&( lx-1 >= 0 ))  
                     lp = *((uchar *)CheckImage_0->imageData+i*CheckImage_0->widthStep + (--lx));  
                 if(rx+1>=imageWidth)  
                     rp = *((uchar *)CheckImage_0->imageData+i*CheckImage_0->widthStep + rx);  
                 while((rp==0)&&(rx+1                      rp = *((uchar *)CheckImage_0->imageData+i*CheckImage_0->widthStep +(++rx));  
                 if (lp > rp)
                 {
                     *((uchar *)CheckImage_0->imageData+i*CheckImage_0->widthStep + j) = rp;
                 }
                 else
                 {
                     *((uchar *)CheckImage_0->imageData+i*CheckImage_0->widthStep + j) = lp;
                 }               
             }  
         }  
     }
     cvSmooth(CheckImage_0,CheckImage_0,CV_MEDIAN,3,0,0);  
     cvNamedWindow("CheckImage_0", 1);
     cvShowImage("CheckImage_0", CheckImage_0);
     cvSaveImage("CheckImage_0.jpg", CheckImage_0); 
 #pragma endregion
 cout << "完成！"<     cvWaitKey(0);
    return 0;
}

处理前先 直方图均匀化 或 自动色阶 或 腐蚀、膨胀、腐蚀膨胀、膨胀腐蚀 或 …… 《学习OpenCV（中文版）》 编译链接调试OpenCV中自带的相关例子代码。