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

1. // Sum of Absolute Difference(SAD)
2. #include <iostream>
3. #include <stdio.h>
4. #include <stdlib.h>
5. #include <cv.h>
6. #include <cxcore.h>
7. #include <highgui.h>
8. #include <math.h>
9. #include <ctime>
10. using namespace std;
11. template<class T> class Image
12. {
13. private:
14. IplImage* imgp;
15. public:
16. Image(IplImage* img=0){imgp=img;}
17. ~Image(){imgp=0;}
18. void operator=(IplImage* img){imgp=img;}
19. inline T* operator[](const int rowIndx)
20. {
21. return((T*)(imgp->imageData+rowIndx*imgp->widthStep));
22. }
23. };
24. typedef struct
25. {
26. unsigned char b,g,r;
27. }RgbPixel;
28. typedef struct
29. {
30. float b,g,r;
31. }RgbPixelFloat;
32. typedef Image<RgbPixel> RgbImage;
33. typedef Image<RgbPixelFloat> RgbImageFloat;
34. typedef Image<unsigned char> BwImage;
35. typedef Image<float> BwImageFloat;
36. //display an image in a new window with title to be given.
37. void displayImageNewWindow(char* title,CvArr* img)
38. {
39. cvNamedWindow(title, CV_WINDOW_AUTOSIZE );
40. cvShowImage(title,img);
41. }
42. int getMaxMin(double value[],int valueSize, int maxmin)
43. {
44. int pos=0;
45. int i=0;
46. double max1=-1;//?-999999;
47. double min1=999999;
48. if (maxmin==1)
49. {
50. //find max
51. for (i=0;i<valueSize;i++)
52. {
53. //find the index with the max value;
54. if (value[i]>max1)
55. {
56. pos=i;
57. max1=value[i];
58. }
59. }
60. }
61. if (maxmin==0)
62. {
63. //find min
64. for (i=0;i<valueSize;i++)
65. {
66. //find the index with the minimum value;
67. if (value[i]<min1)
68. {
69. pos=i;
70. min1=value[i];
71. }
72. }
73. }
74. return pos;
75. }
76. IplImage* generateDisparityImage(IplImage* greyLeftImg32,IplImage* greyRightImg32,int windowSize,int DSR)
77. {
78. int offset=floor((double)windowSize/2);
79. int height=greyLeftImg32->height;
80. int width=greyLeftImg32->width;
81. double* localSAD=new double[DSR];
82. int x=0, y=0,d=0,m=0;
83. int N=windowSize;
84. IplImage* winImg=cvCreateImage(cvSize(N,N),32,1);//mySubImage(greyLeftImg32,cvRect(0,0,N,N));
85. IplImage* disparity=cvCreateImage(cvSize(width,height),8,1);//or IPL_DEPTH_8U
86. BwImage imgA(disparity);
87. for (y=0;y<height;y++)
88. {
89. for (x=0;x<width;x++)
90. {
91. imgA[y][x]=0;
92. }
93. }
94. CvScalar sum;
95. //CvScalar s2;
96. for (y=0;y<height-N;y++)
97. {
98. //height-N
99. for (x=0;x<width-N;x++)
100. {
101. //width-N
102. cvSetImageROI(greyLeftImg32, cvRect(x,y,N,N));
103. d=0;
104. //initialise localSAD
105. for (m=0;m<DSR;m++)
106. {
107. localSAD[m]=0;
108. }
109. //start matching
110. do{
111. if (x-d>=0)
112. {
113. cvSetImageROI(greyRightImg32, cvRect(x-d,y,N,N));
114. }
115. else
116. {
117. break;
118. }
119. cvAbsDiff(greyLeftImg32,greyRightImg32,winImg);//absolute difference
120. sum=cvSum(winImg);//sum
121. localSAD[d]=sum.val[0];//0 means single channel
122. cvResetImageROI(greyRightImg32);
123. d++;
124. }while(d<=DSR);
125. //to find the best d and store
126. imgA[y+offset][x+offset]=getMaxMin(localSAD,DSR,0)*16; //0 means return minimum index
127. cvResetImageROI(greyLeftImg32);
128. }//x
129. if (y%10==0)
130. cout<<"row="<<y<<" of "<<height<<endl;
131. }//y
132. cvReleaseImage(&winImg);
133. //cvReleaseImage(&rightWinImg);
134. return disparity;
135. }
136. int main (int argc, char * const argv[])
137. {
138. cout << "Sum of Absolute Difference(SAD) Strereo Vision"<<endl;
139. //**********image input*********************//
140. char* filename1="L.jpg";//im2_cone.png
141. IplImage* greyLeftImg= cvLoadImage(filename1,0);
142. char* filename2="R.jpg";
143. IplImage* greyRightImg= cvLoadImage(filename2,0);
144. if (greyLeftImg==NULL){cout << "No valid image input."<<endl; return 1;}
145. if (greyRightImg==NULL){cout << "No valid image input."<<endl; return 1;}
146. int width=greyLeftImg->width;
147. int height=greyLeftImg->height;
148. /****************8U to 32F**********************/
149. IplImage* greyLeftImg32=cvCreateImage(cvSize(width,height),32,1);//IPL_DEPTH_32F
150. IplImage* greyRightImg32=cvCreateImage(cvSize(width,height),32,1);
151. cvConvertScale(greyLeftImg, greyLeftImg32, 1/255.);
152. cvConvertScale(greyRightImg, greyRightImg32, 1/255.);//1/255. equals to 1/255.0
153. //-------------obtain disparity image----------------
154. time_t tstart, tend;
155. tstart = time(0);
156. int windowSize=13,DSR=20;//Disparity Search Range
157. IplImage* disparity32=generateDisparityImage(greyLeftImg32,greyRightImg32,windowSize,DSR);
158. tend = time(0);
159. cout << "It took "<< difftime(tend, tstart) <<" second(s)."<< endl;
160. displayImageNewWindow("Dispairty Image",disparity32);
161. displayImageNewWindow("Left Image",greyLeftImg32);
162. displayImageNewWindow("Right Image",greyRightImg32);
163. //cvSaveImage("D:/OpenCV_stuff/SampleImages/disparitySAD.jpg",disparity32);
164. //********destroy window************/
165. cvWaitKey(0);
166. cvReleaseImage(&greyLeftImg32);
167. cvReleaseImage(&greyRightImg32);
168. cvReleaseImage(&greyLeftImg);
169. cvReleaseImage(&greyRightImg);
170. cvReleaseImage(&disparity32);
171. cvDestroyWindow("Left Image");
172. cvDestroyWindow("Right Image");
173. cvDestroyWindow("Dispairty Image");
174. return 0;
175. }