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}