SAD匹配产生致密的深度图

今晚突然想写一个matlab的，基于SAD的区域法立体匹配方法。
下面以并列出matlab和c++版本的代码及实验结果。
MATLAB版本：
有个地方36~42行应该还能优化，只是我没找到相应的函数,这个地方运行很慢

 1%function SADMatch
 2clear,clc
 3tic
 4%im1=imread('Test_left.jpg');
 5%im2=imread('Test_right.jpg');
 6% im1=imread('im0.jpg');
 7% im2=imread('im1.jpg');
 8im1=imread('left.BMP');
 9im2=imread('right.BMP');
10
11
12if isrgb(im1)
13    im1=rgb2gray(im1);
14end
15%imshow(im1);
16im1=double(im1);
17
18if isrgb(im2)
19    im2=rgb2gray(im2);
20end
21%figure
22%imshow(im2);
23im2=double(im2);
24
25D=20; %最大视差
26N=5; %窗口大小的一半
27[H,W]=size(im1);
28
29%计算右图减去左图，相减产生D个矩阵放到imgDiff中 
30imgDiff=zeros(H,W,D);
31e=zeros(H,W);
32for i=1:D
33    fprintf('%g\n',i)
34    e(:,1:(W-i))=abs(im2(:,1:(W-i))- im1(:,(i+1):W));
35    %e=conv2(e,e,'same');
36    e2=zeros(H,W);%计算窗口内的和
37    for y=(N+1):(H-N)
38        for x=(N+1):(W-N)
39            e2(y,x)=sum(sum(e((y-N):(y+N),(x-N):(x+N))));
40        end
41    end
42    imgDiff(:,:,i)=e2;
43end
44
45%找到最小的视差，到dispMap
46dispMap=zeros(H,W);
47for x=1:W
48    for y=1:H
49        %[val,id]=min(imgDiff(y,x,:));
50        [val,id]=sort(imgDiff(y,x,:));
51        if abs(val(1)-val(2))>10
52            dispMap(y,x)=id(1);
53        end
54    end
55end
56dispMap=dispMap*200/D;
57dispMap=uint8(dispMap);
58toc
59imshow(dispMap)
60%imwrite(dispMap,'dispMap.jpg')

c版本：
加入了crosscheck功能

  1/**//************************************************************************
  2功能：SAD匹配
  3输入：用图像image1的(x1,y12)和image2的（x2,y12）处建立大小为2*windowSize大小的窗口计算SAD误差并返回
  4图像必须为灰度
  5************************************************************************/
  6int SAD(IplImage *image1,IplImage*image2,int x1,int x2,int y12,int WindowSize)
  7{
  8    //H_CHECK(CV_ARE_SIZES_EQ(image1,image2));
  9    int W=image1->width,H=image1->height;
 10//     H_CHECK(x1>=WindowSize && x1<W-WindowSize
 11//         &&x2>=WindowSize && x2<W-WindowSize
 12//         &&y12>=WindowSize && y12<H-WindowSize
 13//         );
 14
 15    int dx,dy;
 16    int sum=0;
 17    for (dy=-WindowSize;dy<=WindowSize;dy++)
 18    {
 19        LPBYTE p1=&CV_IMAGE_ELEM(image1,BYTE,y12+dy,x1-WindowSize);
 20        LPBYTE p2=&CV_IMAGE_ELEM(image2,BYTE,y12+dy,x2-WindowSize);
 21        for (dx=-WindowSize;dx<=WindowSize;dx++)
 22        {
 23            sum+=abs(*p1-*p2);
 24            p1++;
 25            p2++;
 26        }
 27    }
 28
 29    return sum;
 30}
 31int GetDisparitySAD(IplImage *image1,IplImage*image2,int x,int y,int WindowSize,int maxDX)
 32{
 33    //H_CHECK(x-maxDX-WindowSize >=0 && x+WindowSize<image1->width );
 34    int x2,i;
 35    int sadMin=99999,dispBest=0;
 36    for(i=1;i<maxDX;i++)
 37    {
 38        x2=x-i;
 39        int sad=SAD(image1,image2,x,x2,y,WindowSize);
 40        if(sadMin >sad)
 41        {
 42            sadMin=sad;
 43            dispBest=i;
 44        }
 45    }
 46
 47    return dispBest;
 48}
 49//反相匹配
 50int GetDisparitySAD_Reverse(IplImage *image1,IplImage*image2,int xr,int y,int WindowSize,int maxDX)
 51{
 52    //H_CHECK(x-maxDX-WindowSize >=0 && x+WindowSize<image1->width );
 53    int xl,i;
 54    int sadMin=99999,dispBest=0;
 55    for(i=1;i<maxDX;i++)
 56    {
 57        xl=xr+i;
 58        int sad=SAD(image1,image2,xl,xr,y,WindowSize);
 59        if(sadMin >sad)
 60        {
 61            sadMin=sad;
 62            dispBest=i;
 63        }
 64    }
 65
 66    return dispBest;
 67}
 68void CDlgImagePanorama::OnBnClickedBnMatchSad()
 69{
 70    // TODO: 在此添加控件通知处理程序代码
 71//     HImage im1(5,5,1);
 72//     HImage im2(5,5,1);
 73//     cvSet(im2,cvScalar(1));
 74//     int sum=SAD(im1,im2,2,2,2,2);
 75//     TRACE("sum %d ",sum);
 76    AfxMessageBox("选择左图");
 77    CString fileNameL=GetFileName("imageL");
 78    if(fileNameL.GetLength()==0)
 79        return;
 80    AfxMessageBox("选择右图");
 81    CString fileNameR=GetFileName("imageR");
 82    if(fileNameR.GetLength()==0)
 83        return;
 84    HImage im1(fileNameL,0);
 85    HImage im2(fileNameR,0);
 86    H_CHECK(im1.w()==im2.w() && im1.h()==im2.h() && im1.nChannels()==im2.nChannels() && im1.nChannels()==1);
 87    
 88    if(AfxMessageBox("resample to width 320?",MB_YESNO)==IDYES)
 89    {
 90        int h=((double)im1.h())*320.0/im1.w();
 91        HImage imgTmp(320,h,1);
 92        cvResize(im1,imgTmp);
 93        im1=imgTmp;
 94
 95        cvResize(im2,imgTmp);
 96        im2=imgTmp;
 97    }
 98
 99    int W=im1.w(),H=im1.h();
100    H_CHECK(W>0 && H>0);
101    OutputDebugString("SAD Start");
102    DWORD dwTime=GetTickCount();
103    int windowSize=GetDlgItemInt(IDC_EDIT_WINSIZE)/2;
104    ASSERT(windowSize>0);
105    int MaxDispX=W/6;
106    int x,y;
107    int Scale=255/MaxDispX;
108
109    char s[1000];
110    sprintf(s,"图像大小%d*%d，搜索范围%d,窗口大小%d",W,H,MaxDispX,windowSize);
111    SetDlgItemText(IDC_MATCH_INFO,s);
112
113    HImage imageDisp(W,H,1);
114    TRACE("MaxDispX %d",MaxDispX);
115    for (y=windowSize;y<H-windowSize;y++)
116    {
117        for (x=MaxDispX+windowSize;x<W-windowSize;x++)
118        {
119
120            int disp=GetDisparitySAD(im1,im2,x,y,windowSize,MaxDispX);
121            //CV_IMAGE_ELEM(imageDisp.m_IplImage,BYTE,y,x)=disp*Scale;
122            if(disp && abs(disp-GetDisparitySAD_Reverse(im1,im2,x-disp,y,windowSize,MaxDispX))<5)
123                imageDisp(x,y)=disp*Scale;
124        }
125        TRACE("y %d",y);
126    }
127
128
129    TRACE("SAD End(%d ms)",GetTickCount()-dwTime);
130    imageDisp.Show("disp");
131}