几种典型的立体匹配算法

立体匹配算法最新动态：http://vision.middlebury.edu/stereo/eval/

介绍立体匹配的基本原理: http://vision.deis.unibo.it/~smatt/Seminars/StereoVision.pdf

立体匹配综述性文章 ： http://wenku.baidu.com/view/5b359d7d5acfa1c7aa00cc7b.html

使用左右两张图片，计算深度图。一下几种算法代码参考 
http://www.360doc.com/content/13/0129/11/11533449_263014896.shtml，经验证可行。并得到一下的深度图，貌似DP算法比较快并且效果还蛮好的。

SAD算法

#include   
#include   
#include   

using namespace std;  
int GetHammingWeight(unsigned int value);  
int main(){  
    /*Half of the window size for the census transform*/  
    int hWin = 11;  
    int compareLength = (2*hWin+1)*(2*hWin+1);  

    cout<<"hWin: "<";  "<<"compare length:  "<cout<<"SAD test"<// char stopKey;   
    IplImage * leftImage = cvLoadImage("left.bmp",0);  
    IplImage * rightImage = cvLoadImage("right.bmp",0);  

    int imageWidth = leftImage->width;  
    int imageHeight =leftImage->height;  

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

    int minDBounds = 0;  
    int maxDBounds = 31;  

    cvNamedWindow("Left",1);  
    cvNamedWindow("Right",1);  
    cvNamedWindow("Census",1);  
    cvNamedWindow("MatchLevel",1);  

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



    /*Census Transform */  
    int i,j ,m,n,k;  
    unsigned char centerPixel = 0;   
    unsigned char neighborPixel = 0;  
    int bitCount = 0;  
    unsigned int bigger = 0;  



    int sum = 0;  
    unsigned int *matchLevel = new unsigned int[maxDBounds - minDBounds  + 1];  
    int tempMin = 0;  
    int tempIndex = 0;  

    unsigned char* dst;  
    unsigned char* leftSrc  = NULL;  
    unsigned char* rightSrc = NULL;  

    unsigned char leftPixel = 0;  
    unsigned char rightPixel =0;  
    unsigned char subPixel = 0;  


    for(i = 0 ; i < leftImage->height;i++){  
        for(j = 0; j< leftImage->width;j++){  

            for (k = minDBounds;k <= maxDBounds;k++)  
            {  
                sum = 0;  
                for (m = i-hWin; m <= i + hWin;m++)  
                {  
                    for (n = j - hWin; n <= j + hWin;n++)  
                    {  
                          if (m < 0 || m >= imageHeight || n <0 || n >= imageWidth )  
                          {  
                              subPixel  = 0;  
                          }else if (n + k >= imageWidth)  
                          {  
                              subPixel = 0;  
                          }else  
                          {  
                              leftSrc = (unsigned char*)leftImage->imageData   
                                          + m*leftImage->widthStep + n + k;   
                              rightSrc = (unsigned char*)rightImage->imageData   
                                           + m*rightImage->widthStep + n;  

                              leftPixel = *leftSrc;  
                              rightPixel = *rightSrc;  
                              if (leftPixel > rightPixel)  
                              {  
                                   subPixel = leftPixel - rightPixel;  
                              }else   
                              {  
                                   subPixel = rightPixel -leftPixel;  
                              }  

                          }  

                          sum += subPixel;  
                    }  
                }  
                matchLevel[k] = sum;  
                //cout<
            }  

            /*寻找最佳匹配点*/  
           // matchLevel[0] = 1000000;   

            tempMin = 0;  
            tempIndex = 0;  
            for ( m = 1;m < maxDBounds - minDBounds + 1;m++)  
            {  
                //cout<
                if (matchLevel[m] < matchLevel[tempIndex])  
                {  
                    tempMin = matchLevel[m];  
                    tempIndex = m;  
                }  
            }  
            dst = (unsigned char *)SADImage->imageData + i*SADImage->widthStep + j;  
            //cout<<"index: "<

            *dst = tempIndex*8;  

            dst = (unsigned char *)MatchLevelImage->imageData + i*MatchLevelImage->widthStep + j;  
            *dst = tempMin;  
            //cout<<"min:  "<
            //cout<< tempIndex<<"  " <
        }  
        //cvWaitKey(0);   
    }  

    cvShowImage("Census",SADImage);  
    cvShowImage("MatchLevel",MatchLevelImage);  
    cvSaveImage("depth.jpg",SADImage);  
    cvSaveImage("matchLevel.jpg",MatchLevelImage);  

    cvWaitKey(0);  
    cvDestroyAllWindows();  
    cvReleaseImage(&leftImage);  
    cvReleaseImage(&rightImage);  
    return 0;  
}  
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
   
   
   
   
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
   
   
   
   

SSD算法：

#include   
#include   
#include   

using namespace std;  
int GetHammingWeight(unsigned int value);  
int main(){  
    /*Half of the window size for the census transform*/  
    int hWin = 11;  
    int compareLength = (2*hWin+1)*(2*hWin+1);  

    cout<<"hWin: "<";  "<<"compare length:  "<cout<<"SAD test"<// char stopKey;   
    IplImage * leftImage = cvLoadImage("l2.jpg",0);  
    IplImage * rightImage = cvLoadImage("r2.jpg",0);  

    int imageWidth = leftImage->width;  
    int imageHeight =leftImage->height;  

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

    int minDBounds = 0;  
    int maxDBounds = 31;  

    cvNamedWindow("Left",1);  
    cvNamedWindow("Right",1);  
    cvNamedWindow("Census",1);  
    cvNamedWindow("MatchLevel",1);  

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



    /*Census Transform */  
    int i,j ,m,n,k;  
    unsigned char centerPixel = 0;   
    unsigned char neighborPixel = 0;  
    int bitCount = 0;  
    unsigned int bigger = 0;  



    int sum = 0;  
    unsigned int *matchLevel = new unsigned int[maxDBounds - minDBounds  + 1];  
    int tempMin = 0;  
    int tempIndex = 0;  

    unsigned char* dst;  
    unsigned char* leftSrc  = NULL;  
    unsigned char* rightSrc = NULL;  

    unsigned char leftPixel = 0;  
    unsigned char rightPixel =0;  
    unsigned char subPixel = 0;  


    for(i = 0 ; i < leftImage->height;i++){  
        for(j = 0; j< leftImage->width;j++){  

            for (k = minDBounds;k <= maxDBounds;k++)  
            {  
                sum = 0;  
                for (m = i-hWin; m <= i + hWin;m++)  
                {  
                    for (n = j - hWin; n <= j + hWin;n++)  
                    {  
                          if (m < 0 || m >= imageHeight || n <0 || n >= imageWidth )  
                          {  
                              subPixel  = 0;  
                          }else if (n + k >= imageWidth)  
                          {  
                              subPixel = 0;  
                          }else  
                          {  
                              leftSrc = (unsigned char*)leftImage->imageData   
                                          + m*leftImage->widthStep + n + k;   
                              rightSrc = (unsigned char*)rightImage->imageData   
                                           + m*rightImage->widthStep + n;  

                              leftPixel = *leftSrc;  
                              rightPixel = *rightSrc;  
                              if (leftPixel > rightPixel)  
                              {  
                                   subPixel = leftPixel - rightPixel;  
                              }else   
                              {  
                                   subPixel = rightPixel -leftPixel;  
                              }  

                          }  

                          sum += subPixel*subPixel;  
                    }  
                }  
                matchLevel[k] = sum;  
                //cout<
            }  

            /*寻找最佳匹配点*/  
           // matchLevel[0] = 1000000;   

            tempMin = 0;  
            tempIndex = 0;  
            for ( m = 1;m < maxDBounds - minDBounds + 1;m++)  
            {  
                //cout<
                if (matchLevel[m] < matchLevel[tempIndex])  
                {  
                    tempMin = matchLevel[m];  
                    tempIndex = m;  
                }  
            }  
            dst = (unsigned char *)SADImage->imageData + i*SADImage->widthStep + j;  
            //cout<<"index: "<

            *dst = tempIndex*8;  

            dst = (unsigned char *)MatchLevelImage->imageData + i*MatchLevelImage->widthStep + j;  
            *dst = tempMin;  
            //cout<<"min:  "<
            //cout<< tempIndex<<"  " <
        }  
        //cvWaitKey(0);   
    }  

    cvShowImage("Census",SADImage);  
    cvShowImage("MatchLevel",MatchLevelImage);  
    cvSaveImage("depth.jpg",SADImage);  
    cvSaveImage("matchLevel.jpg",MatchLevelImage);  

    cvWaitKey(0);  
    cvDestroyAllWindows();  
    cvReleaseImage(&leftImage);  
    cvReleaseImage(&rightImage);  
    return 0;  
}  
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
   
   
   
   
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
   
   
   
   

ZSSD算法：

#include   
#include   
#include   

using namespace std;  
int GetHammingWeight(unsigned int value);  
int main(){  
    /*Half of the window size for the census transform*/  
    int hWin = 11;  
    int compareLength = (2*hWin+1)*(2*hWin+1);  

    cout<<"hWin: "<";  "<<"compare length:  "<cout<<"ZSSD test"<// char stopKey;   
  /*  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 * SADImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
    IplImage * MatchLevelImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  

    int minDBounds = 0;  
    int maxDBounds = 31;  

    cvNamedWindow("Left",1);  
    cvNamedWindow("Right",1);  
    cvNamedWindow("Census",1);  
    cvNamedWindow("MatchLevel",1);  

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



    /*Census Transform */  
    int i,j ,m,n,k;  
    unsigned char centerPixel = 0;   
    unsigned char neighborPixel = 0;  
    int bitCount = 0;  
    unsigned int bigger = 0;  



    int sumLeft = 0;  
    int sumRight = 0;  
    int sum =0;  

    int zSumLeft  = 0;  
    int zSumRight = 0;  

    unsigned int *matchLevel = new unsigned int[maxDBounds - minDBounds  + 1];  
    int tempMin = 0;  
    int tempIndex = 0;  

    unsigned char* dst;  
    unsigned char* leftSrc  = NULL;  
    unsigned char* rightSrc = NULL;  

    unsigned char leftPixel = 0;  
    unsigned char rightPixel =0;  
    unsigned char subPixel = 0;  
    unsigned char meanLeftPixel  = 0;  
    unsigned char meanRightPixel = 0;  


    for(i = 0 ; i < leftImage->height;i++){  
        for(j = 0; j< leftImage->width;j++){  

            /*均值计算 */  
            for (k = minDBounds;k <= maxDBounds;k++)  
            {  
                sumLeft  = 0;  
                sumRight = 0;  
                for (m = i-hWin; m <= i + hWin;m++)  
                {  
                    for (n = j - hWin; n <= j + hWin;n++)  
                    {  
                          if (m < 0 || m >= imageHeight || n <0 || n >= imageWidth )  
                          {  
                              sumLeft += 0;  
                          }else {  
                              leftSrc = (unsigned char*)leftImage->imageData   
                                  + m*leftImage->widthStep + n + k;   
                              leftPixel = *leftSrc;  
                              sumLeft += leftPixel;  
                          }  


                          if (m < 0 || m >= imageHeight || n + k <0 || n +k >= imageWidth)  
                          {  
                               sumRight += 0;  
                          }else  
                          {   
                              rightSrc = (unsigned char*)rightImage->imageData   
                                           + m*rightImage->widthStep + n;  
                              rightPixel = *rightSrc;  
                              sumRight += rightPixel;     
                          }  

                    }  
                }  

                meanLeftPixel  = sumLeft/compareLength;  
                meanRightPixel = sumRight/compareLength;  
                 /*ZSSD*/  
               sum = 0;  
                 for (m = i-hWin; m <= i + hWin;m++)  
                {  
                    for (n = j - hWin; n <= j + hWin;n++)  
                    {  
                          if (m < 0 || m >= imageHeight || n <0 || n >= imageWidth )  
                          {  
                              //zSumLeft += 0;   
                              leftPixel = 0;  
                          }else {  
                              leftSrc = (unsigned char*)leftImage->imageData   
                                  + m*leftImage->widthStep + n + k;   
                              leftPixel = *leftSrc;  
                              //zSumLeft += (leftPixel - meanLeftPixel)*(leftPixel -meanLeftPixel);   
                          }  


                          if (m < 0 || m >= imageHeight || n + k <0 || n +k >= imageWidth)  
                          {  
                               //zSumRight += 0;   
                              rightPixel = 0;  
                          }else  
                          {   
                              rightSrc = (unsigned char*)rightImage->imageData   
                                           + m*rightImage->widthStep + n;  
                              rightPixel = *rightSrc;  
                              // zSumRight += (rightPixel - meanRightPixel)*(rightPixel - meanRightPixel);      
                          }  

                          sum += ((rightPixel - meanRightPixel)-(leftPixel -meanLeftPixel))  
                                 *((rightPixel - meanRightPixel)-(leftPixel -meanLeftPixel));  
                    }  
                }  


                matchLevel[k] = sum;  
                //cout<
            }  

            /*寻找最佳匹配点*/  
           // matchLevel[0] = 1000000;   

            tempMin = 0;  
            tempIndex = 0;  
            for ( m = 1;m < maxDBounds - minDBounds + 1;m++)  
            {  
                //cout<
                if (matchLevel[m] < matchLevel[tempIndex])  
                {  
                    tempMin = matchLevel[m];  
                    tempIndex = m;  
                }  
            }  
            dst = (unsigned char *)SADImage->imageData + i*SADImage->widthStep + j;  
            //cout<<"index: "<

            *dst = tempIndex*8;  
            dst = (unsigned char *)MatchLevelImage->imageData + i*MatchLevelImage->widthStep + j;  
            *dst = tempMin;  
            //cout<<"min:  "<
            //cout<< tempIndex<<"  " <
        }  
       // cvWaitKey(0);   
    }  

    cvShowImage("Census",SADImage);  
    cvShowImage("MatchLevel",MatchLevelImage);  
    cvSaveImage("depth.jpg",SADImage);  
    cvSaveImage("matchLevel.jpg",MatchLevelImage);  


    cout<"Over"<0);  
    cvDestroyAllWindows();  
    cvReleaseImage(&leftImage);  
    cvReleaseImage(&rightImage);  
    return 0;  
}  
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
    
    
    
    
140
    
    
    
    
141
    
    
    
    
142
    
    
    
    
143
    
    
    
    
144
    
    
    
    
145
    
    
    
    
146
    
    
    
    
147
    
    
    
    
148
    
    
    
    
149
    
    
    
    
150
    
    
    
    
151
    
    
    
    
152
    
    
    
    
153
    
    
    
    
154
    
    
    
    
155
    
    
    
    
156
    
    
    
    
157
    
    
    
    
158
    
    
    
    
159
    
    
    
    
160
    
    
    
    
161
    
    
    
    
162
    
    
    
    
163
    
    
    
    
164
    
    
    
    
165
    
    
    
    
166
    
    
    
    
167
    
    
    
    
168
    
    
    
    
169
    
    
    
    
170
    
    
    
    
171
    
    
    
    
172
    
    
    
    
173
    
    
    
    
174
    
    
    
    
175
    
    
    
    
176
    
    
    
    
177
    
    
    
    
178
    
    
    
    
179
    
    
    
    
180
    
    
    
    
181
    
    
    
    
182
    
    
    
    
183
    
    
    
    
184
    
    
    
    
185
    
    
    
    
186
    
    
    
    
187
    
    
    
    
188
   
   
   
   
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
    
    
    
    
140
    
    
    
    
141
    
    
    
    
142
    
    
    
    
143
    
    
    
    
144
    
    
    
    
145
    
    
    
    
146
    
    
    
    
147
    
    
    
    
148
    
    
    
    
149
    
    
    
    
150
    
    
    
    
151
    
    
    
    
152
    
    
    
    
153
    
    
    
    
154
    
    
    
    
155
    
    
    
    
156
    
    
    
    
157
    
    
    
    
158
    
    
    
    
159
    
    
    
    
160
    
    
    
    
161
    
    
    
    
162
    
    
    
    
163
    
    
    
    
164
    
    
    
    
165
    
    
    
    
166
    
    
    
    
167
    
    
    
    
168
    
    
    
    
169
    
    
    
    
170
    
    
    
    
171
    
    
    
    
172
    
    
    
    
173
    
    
    
    
174
    
    
    
    
175
    
    
    
    
176
    
    
    
    
177
    
    
    
    
178
    
    
    
    
179
    
    
    
    
180
    
    
    
    
181
    
    
    
    
182
    
    
    
    
183
    
    
    
    
184
    
    
    
    
185
    
    
    
    
186
    
    
    
    
187
    
    
    
    
188
   
   
   
   

census 算法：

#include   
#include   
#include   

using namespace std;  
int GetHammingWeight(unsigned int value);  
int main(){  
    /*Half of the window size for the census transform*/  
    int hWin = 11;  
    int bitlength = 0;  
    if ((2*hWin+1)*(2*hWin+1)%32 == 0)  
    {  
        bitlength = (2*hWin+1)*(2*hWin+1)/32;  
    }else {  
        bitlength = (2*hWin+1)*(2*hWin+1)/32  + 1;   
    }    
    cout<<"hWin: "<";  "<<"bit length:  "<cout<<"Census test"<// char stopKey;   
    IplImage * leftImage = cvLoadImage("left.bmp",0);  
    IplImage * rightImage = cvLoadImage("right.bmp",0);  

    int imageWidth = leftImage->width;  
    int imageHeight =leftImage->height;  

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

     int minDBounds = 0;  
     int maxDBounds = 31;  
    // int leftCensus[imageHeight][imageWidth][bitlength] = {0};     
     unsigned  int *leftCensus = new unsigned int[imageHeight*imageWidth*bitlength];  
     unsigned  int *rightCensus = new unsigned int[imageHeight*imageWidth*bitlength];   
     for (int i = 0;i < imageHeight*imageWidth*bitlength;i++)  
     {  
         leftCensus[i] = 0;  
         rightCensus[i] = 0;  
     }  
     int pointCnt = 0;  




    cvNamedWindow("Left",1);  
    cvNamedWindow("Right",1);  
    cvNamedWindow("Census",1);  
    cvNamedWindow("MatchLevel",1);  

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



    /*Census Transform */  
   int i,j ,m,n,k,l;  
   unsigned char centerPixel = 0;   
   unsigned char neighborPixel = 0;  
   int bitCount = 0;  
   unsigned int bigger = 0;  
    for(i = 0 ; i < leftImage->height;i++){  
        for(j = 0; j< leftImage->width;j++){  
             centerPixel = *((unsigned char *)leftImage->imageData + i*leftImage->widthStep + j);   
             bitCount = 0;  

             for (m = i - hWin; m <= i + hWin;m++)  
             {  
                 for (n = j - hWin; n<= j+hWin;n++)  
                 {  
                     bitCount++;  
                     if (m < 0 || m >= leftImage->height || n < 0 || n >= leftImage->width)  
                     {  
                         neighborPixel = 0;  
                     }else{  
                          neighborPixel = *((unsigned char *)leftImage->imageData + m*leftImage->widthStep + n);   
                     }  
                     bigger = (neighborPixel > centerPixel)?1:0;  
                     leftCensus[(i*imageWidth + j)*bitlength + bitCount/32] |= (bigger<<(bitCount%32));  
                 }  
             }  

        }  
    }  

    for(i = 0 ; i < rightImage->height;i++){  
        for(j = 0; j< rightImage->width;j++){  
            centerPixel = *((unsigned char *)rightImage->imageData + i*rightImage->widthStep + j);   
            bitCount = 0;  

            for (m = i - hWin; m <= i + hWin;m++)  
            {  
                for (n = j - hWin; n<= j+hWin;n++)  
                {  
                    bitCount++;  
                    if (m < 0 || m >= rightImage->height || n < 0 || n >= rightImage->width)  
                    {  
                        neighborPixel = 0;  
                    }else{  
                        neighborPixel = *((unsigned char *)rightImage->imageData + m*rightImage->widthStep + n);   
                    }  
                    bigger = (neighborPixel > centerPixel)?1:0;  
                    rightCensus[(i*imageWidth + j)*bitlength + bitCount/32] |= (bigger<<(bitCount%32));  
                }  
            }  

        }  
    }  
    int sum = 0;  
    unsigned int *matchLevel = new unsigned int[maxDBounds - minDBounds  + 1];  
    int tempMin = 0;  
    int tempIndex = 0;  
    unsigned  char *dst;  
    unsigned char pixle = 0;  
    for(i = 0 ; i < rightImage->height;i++){  
        for(j = 0; j< rightImage->width;j++){  


            for (k = minDBounds;k <= maxDBounds;k++)  
            {  
                sum = 0;  
                for (l = 0;l< bitlength;l++)  
                {     
                    if (((i*imageWidth+j+k)*bitlength + l) < imageHeight*imageWidth*bitlength)  
                    {  
                        sum += GetHammingWeight(rightCensus[(i*imageWidth+j)*bitlength + l]   
                        ^ leftCensus[(i*imageWidth+j+k)*bitlength + l]);  
                    }else {  
                        //sum += 0;   
                       // cout<<".";   
                    }  

                }  
                matchLevel[k] = sum;  
            }  

            /*寻找最佳匹配点*/  
            tempMin = 0;  
            tempIndex = 0;  
            for ( m = 1;m < maxDBounds - minDBounds + 1;m++)  
            {  
                 if (matchLevel[m] < matchLevel[tempIndex])  
                 {  
                     tempMin = matchLevel[m];  
                     tempIndex = m;  
                 }  
            }  

            if (tempMin > (2*hWin+1)*(2*hWin+1)*0.2)  
            {  
                tempMin = 0;  
                pointCnt++;  
            }else{  
                tempMin = 255;  
            }  
            dst = (unsigned char *)CensusImage->imageData + i*CensusImage->widthStep + j;  
            *dst = tempIndex*8;  

            dst = (unsigned char *)MatchLevelImage->imageData + i*MatchLevelImage->widthStep + j;  
            *dst = tempMin;  

            //cout<< tempIndex<<"  " <

        }  
    }  
    cout<<"pointCnt:  "<"Census",CensusImage);  
    cvShowImage("MatchLevel",MatchLevelImage);  
    cvSaveImage("depth.jpg",CensusImage);  
    cvSaveImage("matchLevel.jpg",MatchLevelImage);  

    cvWaitKey(0);  
    cvDestroyAllWindows();  
    cvReleaseImage(&leftImage);  
    cvReleaseImage(&rightImage);  
    return 0;  
}  


int GetHammingWeight(unsigned int value)  
{  
    if(value == 0) return 0;  

    int a = value;  
    int b = value -1;  
    int c = 0;  

    int count = 1;  
    while(c = a & b)  
    {  
        count++;  
        a = c;  
        b = c-1;  
    }  
    return count;  
}  
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
    
    
    
    
140
    
    
    
    
141
    
    
    
    
142
    
    
    
    
143
    
    
    
    
144
    
    
    
    
145
    
    
    
    
146
    
    
    
    
147
    
    
    
    
148
    
    
    
    
149
    
    
    
    
150
    
    
    
    
151
    
    
    
    
152
    
    
    
    
153
    
    
    
    
154
    
    
    
    
155
    
    
    
    
156
    
    
    
    
157
    
    
    
    
158
    
    
    
    
159
    
    
    
    
160
    
    
    
    
161
    
    
    
    
162
    
    
    
    
163
    
    
    
    
164
    
    
    
    
165
    
    
    
    
166
    
    
    
    
167
    
    
    
    
168
    
    
    
    
169
    
    
    
    
170
    
    
    
    
171
    
    
    
    
172
    
    
    
    
173
    
    
    
    
174
    
    
    
    
175
    
    
    
    
176
    
    
    
    
177
    
    
    
    
178
    
    
    
    
179
    
    
    
    
180
    
    
    
    
181
    
    
    
    
182
    
    
    
    
183
    
    
    
    
184
    
    
    
    
185
    
    
    
    
186
    
    
    
    
187
    
    
    
    
188
    
    
    
    
189
    
    
    
    
190
    
    
    
    
191
    
    
    
    
192
    
    
    
    
193
    
    
    
    
194
   
   
   
   
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
    
    
    
    
140
    
    
    
    
141
    
    
    
    
142
    
    
    
    
143
    
    
    
    
144
    
    
    
    
145
    
    
    
    
146
    
    
    
    
147
    
    
    
    
148
    
    
    
    
149
    
    
    
    
150
    
    
    
    
151
    
    
    
    
152
    
    
    
    
153
    
    
    
    
154
    
    
    
    
155
    
    
    
    
156
    
    
    
    
157
    
    
    
    
158
    
    
    
    
159
    
    
    
    
160
    
    
    
    
161
    
    
    
    
162
    
    
    
    
163
    
    
    
    
164
    
    
    
    
165
    
    
    
    
166
    
    
    
    
167
    
    
    
    
168
    
    
    
    
169
    
    
    
    
170
    
    
    
    
171
    
    
    
    
172
    
    
    
    
173
    
    
    
    
174
    
    
    
    
175
    
    
    
    
176
    
    
    
    
177
    
    
    
    
178
    
    
    
    
179
    
    
    
    
180
    
    
    
    
181
    
    
    
    
182
    
    
    
    
183
    
    
    
    
184
    
    
    
    
185
    
    
    
    
186
    
    
    
    
187
    
    
    
    
188
    
    
    
    
189
    
    
    
    
190
    
    
    
    
191
    
    
    
    
192
    
    
    
    
193
    
    
    
    
194
   
   
   
   

NCC算法：

#include   
#include   
#include   
#include    
using namespace std;  

int main(){  
    /*Half of the window size for the census transform*/  
    int hWin = 11;  
    int compareLength = (2*hWin+1)*(2*hWin+1);  

    cout<<"hWin: "<";  "<<"compare length:  "<cout<<"NCC test"</*  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 * NCCImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
    IplImage * MatchLevelImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  

    int minDBounds = 0;  
    int maxDBounds = 31;  

    cvNamedWindow("Left",1);  
    cvNamedWindow("Right",1);  
    cvNamedWindow("Census",1);  
    cvNamedWindow("MatchLevel",1);  

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



    /*Census Transform */  
    int i,j ,m,n,k;  
    unsigned char centerPixel = 0;   
    unsigned char neighborPixel = 0;  
    int bitCount = 0;  
    unsigned int bigger = 0;  

    unsigned int sum =0;  
    unsigned int leftSquareSum = 0;  
    unsigned int rightSquareSum = 0;   

    double *matchLevel = new double[maxDBounds - minDBounds  + 1];  
    double tempMax = 0;  
    int tempIndex = 0;  

    unsigned char* dst;  
    unsigned char* leftSrc  = NULL;  
    unsigned char* rightSrc = NULL;  

    unsigned char leftPixel = 0;  
    unsigned char rightPixel =0;  
    unsigned char subPixel = 0;  
    unsigned char meanLeftPixel  = 0;  
    unsigned char meanRightPixel = 0;  


    for(i = 0 ; i < leftImage->height;i++){  
        for(j = 0; j< leftImage->width;j++){  

            /*均值计算 */  
            for (k = minDBounds;k <= maxDBounds;k++)  
            {  
                sum = 0;  
                leftSquareSum  = 0;  
                rightSquareSum = 0;  

                 for (m = i-hWin; m <= i + hWin;m++)  
                {  
                    for (n = j - hWin; n <= j + hWin;n++)  
                    {  
                          if (m < 0 || m >= imageHeight || n <0 || n >= imageWidth )  
                          {  

                              leftPixel = 0;  
                          }else {  
                              leftSrc = (unsigned char*)leftImage->imageData   
                                  + m*leftImage->widthStep + n + k;   
                              leftPixel = *leftSrc;  

                          }  


                          if (m < 0 || m >= imageHeight || n + k <0 || n +k >= imageWidth)  
                          {  

                              rightPixel = 0;  
                          }else  
                          {   
                              rightSrc = (unsigned char*)rightImage->imageData   
                                           + m*rightImage->widthStep + n;  
                              rightPixel = *rightSrc;  

                          }  

                          sum +=  leftPixel*rightPixel;  
                          leftSquareSum  += leftPixel*leftPixel;  
                          rightSquareSum += rightPixel*rightPixel;  

                    }  
                }  
                matchLevel[k] = (double)sum/(sqrt(double(leftSquareSum))*sqrt((double)rightSquareSum));  

            }  


            tempMax = 0;  
            tempIndex = 0;  
            for ( m = 1;m < maxDBounds - minDBounds + 1;m++)  
            {  

                if (matchLevel[m] > matchLevel[tempIndex])  
                {  
                    tempMax = matchLevel[m];  
                    tempIndex = m;  
                }  
            }  
            dst = (unsigned char *)NCCImage->imageData + i*NCCImage->widthStep + j;  

            *dst = tempIndex*8;  
            dst = (unsigned char *)MatchLevelImage->imageData + i*MatchLevelImage->widthStep + j;  
            *dst = (unsigned char)(tempMax*255);  

        }  

    }  

    cvShowImage("Census",NCCImage);  
    cvShowImage("MatchLevel",MatchLevelImage);  
    cvSaveImage("depth.jpg",NCCImage);  
    cvSaveImage("matchLevel.jpg",MatchLevelImage);  


    cout<"Over"<0);  
    cvDestroyAllWindows();  
    cvReleaseImage(&leftImage);  
    cvReleaseImage(&rightImage);  
    return 0;  
}  
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
    
    
    
    
140
    
    
    
    
141
    
    
    
    
142
    
    
    
    
143
    
    
    
    
144
    
    
    
    
145
    
    
    
    
146
    
    
    
    
147
   
   
   
   
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
    
    
    
    
140
    
    
    
    
141
    
    
    
    
142
    
    
    
    
143
    
    
    
    
144
    
    
    
    
145
    
    
    
    
146
    
    
    
    
147
   
   
   
   

DP算法：

#include    
#include    
#include    
#include   
#include   
#include    

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

int  main()  
{    
    /*Half of the window size for the census transform*/  
    int hWin = 11;  
    int compareLength = (2*hWin+1)*(2*hWin+1);  

    cout<<"hWin: "<";  "<<"compare length:  "<cout<<"belief propagation test"<"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 * DPImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
    //IplImage * MatchLevelImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);   

    unsigned char * pPixel = NULL;  
    unsigned char  pixel;  
    for (int i = 0; i< imageHeight;i++)  
    {  
        for (int j =0; j < imageWidth;j++ )  
        {  
            pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + j;  
            *pPixel = 0;  
        }  
    }  

    int minDBounds = 0;  
    int maxDBounds = 31;  

    cvNamedWindow("Left",1);  
    cvNamedWindow("Right",1);  
    cvNamedWindow("Depth",1);  
    cvNamedWindow("MatchLevel",1);  

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

    int minD = 0;  
    int maxD = 31;  
    //假设图像是经过矫正的，那么每次都只是需要搜搜同一行的内容   
    int max12Diff = 10;  

    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;  

    for (int i = 0 ; i < imageHeight;i++)  
    {  
        for (int j = 0; jfor (int k = j + minD; k <= j + maxD;k++)  
            {  
                if (k <0 || k >= imageWidth)  
                {  

                }else {  
                    pLeftPixel = (unsigned char*)leftImage->imageData + i*leftImage->widthStep + k;  
                    pRightPixel= (unsigned char*)rightImage->imageData+i*rightImage->widthStep + j;  
                    leftPixel  = *pLeftPixel;  
                    rightPixel = *pRightPixel;  

                    if (abs(leftPixel - rightPixel) <= 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<
                }  

            }  
             //cout<<"\n";   
        }  
        //逆向搜索，找出最佳路径   
         m = imageWidth;  
         n = imageWidth;  
         l = Ddynamic[imageWidth][imageWidth];  
        while( l>0 )  
        {  
            if (Ddynamic[m][n] == Ddynamic[m-1][n])    
                m--;  
            else if (Ddynamic[m][n] == Ddynamic[m][n-1])    
                n--;  
            else  
            {   
                //s[--l]=a[i-1];   
                pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + m;  
                *pPixel = (n-m)*8;  
                l--;  
                m--;   
                n--;  
            }  
        }  

       //cvWaitKey(0);   

    }  

    cvShowImage("Depth",DPImage);  
    cvSaveImage("depth.jpg",DPImage);  
    cvWaitKey(0);  
    return 0;  
}  
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
   
   
   
   
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
   
   
   
   

DP_5算法：

//引入概率公式   
//   
#include    
#include    
#include    
#include   
#include   
#include    

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

 //使用钟形曲线作为匹配概率，差值越小则匹配的概率越大，最终的要求是使匹配的概率最大，概率曲线使用matlab生成   
 int Probability[256] = {  
    255, 255, 254, 252, 250, 247, 244, 240, 235, 230, 225, 219, 213, 206, 200, 192, 185, 178, 170, 162,   
    155, 147, 139, 132, 124, 117, 110, 103, 96, 89, 83, 77, 71, 65, 60, 55, 50, 46, 42, 38, 35, 31, 28,   
    25, 23, 20, 18, 16, 14, 13, 11, 10, 9, 8, 7, 6, 5, 4, 4, 3, 3, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 0, 0,   
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,   
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,   
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,   
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,   
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,   
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0  
 };  

int  main()  
{    

    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 * DPImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
    IplImage * effectiveImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
    IplImage * FilterImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  

    unsigned char * pPixel = NULL;  
    unsigned char  pixel;  
    unsigned char * pPixel2 = NULL;  
    unsigned char  pixel2;  
    for (int i = 0; i< imageHeight;i++)  
    {  
        for (int j =0; j < imageWidth;j++ )  
        {  
            pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + j;  
            *pPixel = 0;  
            pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + j;  
            *pPixel = 0;  
        }  
    }  



    cvNamedWindow("Left",1);  
    cvNamedWindow("Right",1);  
    cvNamedWindow("Depth",1);  
    cvNamedWindow("effectiveImage",1);  

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

    int minD = 0;  
    int maxD = 31;  
    //假设图像是经过矫正的，那么每次都只是需要搜搜同一行的内容   
    int max12Diff = 5;  

    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 t1 = clock();  
    for (int i = 0 ; i < imageHeight;i++)  
    {  
        for (int j = 0; jfor (int k = j + minD; k <= j + maxD;k++)  
            {  
                if (k <0 || k >= imageWidth)  
                {  

                }else {  
                    pLeftPixel = (unsigned char*)leftImage->imageData + i*leftImage->widthStep + k;  
                    pRightPixel= (unsigned char*)rightImage->imageData+i*rightImage->widthStep + j;  
                    leftPixel  = *pLeftPixel;  
                    rightPixel = *pRightPixel;  
                    //之前概率最大的点加上当前的概率   

                    Ddynamic[j + 1][k + 1] = max(Ddynamic[j][k],max(Ddynamic[j][k+1],Ddynamic[j+1][k]))  
                                             + Probability[abs(leftPixel - rightPixel)];  
                    /* if (abs(leftPixel - rightPixel) <= 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<
                }  

            }  
             //cout<<"\n";   
        }  
        //逆向搜索，找出最佳路径   
         m = imageWidth;  
         n = imageWidth;  
         l = Ddynamic[imageWidth][imageWidth];  
        while( m >= 1 && n >= 1)  
        {  
            pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + m;  
            *pPixel = (n-m)*8;  
            //标记有效匹配点   
            pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + m;  
            *pPixel = 255;  
            if (Ddynamic[m-1][n] >= Ddynamic[m][n -1] && Ddynamic[m-1][n] >= Ddynamic[m-1][n -1])    
                m--;  
            else if (Ddynamic[m][n-1] >= Ddynamic[m-1][n] && Ddynamic[m][n -1] >= Ddynamic[m-1][n -1])    
                n--;  
            else  
            {   
                //s[--l]=a[i-1];          
               // l -= Ddynamic[m][n];   
                m--;   
                n--;  
            }  
        }  

       //cvWaitKey(0);   
    }  

    //refine the depth image  7*7中值滤波   
    //统计未能匹配点的个数   
    int count = 0;  
    for (int i = 0 ;i< imageHeight;i++)  
    {  
        for (int j= 0; j< imageWidth;j++)  
        {  
            pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + j;  
            pixel = *pPixel;  
            if (pixel == 0)  
            {  
                count++;  
            }  
        }  
    }  
    int t2 = clock();  
    cout<<"dt: "<cout<<"Count:  "<"  "<<(double)count/(imageWidth*imageHeight)<"Depth",DPImage);  
    cvShowImage("effectiveImage",effectiveImage);  
   // cvWaitKey(0);   


    FilterImage = cvCloneImage(DPImage);  

    //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 *)effectiveImage->imageData + i*effectiveImage->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 *)DPImage->imageData + m*DPImage->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 *)FilterImage->imageData + i*DPImage->widthStep + j;  
                    *pPixel = medianVal;  
                }  

            }  
        }  
    }  
    cvShowImage("Depth",DPImage);  
    cvShowImage("effectiveImage",effectiveImage);  
    cvShowImage("Filter",FilterImage);  
    cvSaveImage("depth.jpg",DPImage);  
    cvSaveImage("effective.jpg",effectiveImage);  

    cvWaitKey(0);  
    return 0;  
}  

   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
    
    
    
    
140
    
    
    
    
141
    
    
    
    
142
    
    
    
    
143
    
    
    
    
144
    
    
    
    
145
    
    
    
    
146
    
    
    
    
147
    
    
    
    
148
    
    
    
    
149
    
    
    
    
150
    
    
    
    
151
    
    
    
    
152
    
    
    
    
153
    
    
    
    
154
    
    
    
    
155
    
    
    
    
156
    
    
    
    
157
    
    
    
    
158
    
    
    
    
159
    
    
    
    
160
    
    
    
    
161
    
    
    
    
162
    
    
    
    
163
    
    
    
    
164
    
    
    
    
165
    
    
    
    
166
    
    
    
    
167
    
    
    
    
168
    
    
    
    
169
    
    
    
    
170
    
    
    
    
171
    
    
    
    
172
    
    
    
    
173
    
    
    
    
174
    
    
    
    
175
    
    
    
    
176
    
    
    
    
177
    
    
    
    
178
    
    
    
    
179
    
    
    
    
180
    
    
    
    
181
    
    
    
    
182
    
    
    
    
183
    
    
    
    
184
    
    
    
    
185
    
    
    
    
186
    
    
    
    
187
    
    
    
    
188
    
    
    
    
189
    
    
    
    
190
    
    
    
    
191
    
    
    
    
192
    
    
    
    
193
    
    
    
    
194
    
    
    
    
195
    
    
    
    
196
    
    
    
    
197
    
    
    
    
198
    
    
    
    
199
    
    
    
    
200
    
    
    
    
201
    
    
    
    
202
    
    
    
    
203
    
    
    
    
204
    
    
    
    
205
    
    
    
    
206
    
    
    
    
207
    
    
    
    
208
    
    
    
    
209
    
    
    
    
210
    
    
    
    
211
    
    
    
    
212
    
    
    
    
213
    
    
    
    
214
    
    
    
    
215
    
    
    
    
216
    
    
    
    
217
    
    
    
    
218
    
    
    
    
219
    
    
    
    
220
    
    
    
    
221
    
    
    
    
222
    
    
    
    
223
    
    
    
    
224
    
    
    
    
225
    
    
    
    
226
    
    
    
    
227
    
    
    
    
228
    
    
    
    
229
    
    
    
    
230
    
    
    
    
231
    
    
    
    
232
    
    
    
    
233
   
   
   
   
   
   
   
   

    
    
    
    
1
    
    
    
    
2
    
    
    
    
3
    
    
    
    
4
    
    
    
    
5
    
    
    
    
6
    
    
    
    
7
    
    
    
    
8
    
    
    
    
9
    
    
    
    
10
    
    
    
    
11
    
    
    
    
12
    
    
    
    
13
    
    
    
    
14
    
    
    
    
15
    
    
    
    
16
    
    
    
    
17
    
    
    
    
18
    
    
    
    
19
    
    
    
    
20
    
    
    
    
21
    
    
    
    
22
    
    
    
    
23
    
    
    
    
24
    
    
    
    
25
    
    
    
    
26
    
    
    
    
27
    
    
    
    
28
    
    
    
    
29
    
    
    
    
30
    
    
    
    
31
    
    
    
    
32
    
    
    
    
33
    
    
    
    
34
    
    
    
    
35
    
    
    
    
36
    
    
    
    
37
    
    
    
    
38
    
    
    
    
39
    
    
    
    
40
    
    
    
    
41
    
    
    
    
42
    
    
    
    
43
    
    
    
    
44
    
    
    
    
45
    
    
    
    
46
    
    
    
    
47
    
    
    
    
48
    
    
    
    
49
    
    
    
    
50
    
    
    
    
51
    
    
    
    
52
    
    
    
    
53
    
    
    
    
54
    
    
    
    
55
    
    
    
    
56
    
    
    
    
57
    
    
    
    
58
    
    
    
    
59
    
    
    
    
60
    
    
    
    
61
    
    
    
    
62
    
    
    
    
63
    
    
    
    
64
    
    
    
    
65
    
    
    
    
66
    
    
    
    
67
    
    
    
    
68
    
    
    
    
69
    
    
    
    
70
    
    
    
    
71
    
    
    
    
72
    
    
    
    
73
    
    
    
    
74
    
    
    
    
75
    
    
    
    
76
    
    
    
    
77
    
    
    
    
78
    
    
    
    
79
    
    
    
    
80
    
    
    
    
81
    
    
    
    
82
    
    
    
    
83
    
    
    
    
84
    
    
    
    
85
    
    
    
    
86
    
    
    
    
87
    
    
    
    
88
    
    
    
    
89
    
    
    
    
90
    
    
    
    
91
    
    
    
    
92
    
    
    
    
93
    
    
    
    
94
    
    
    
    
95
    
    
    
    
96
    
    
    
    
97
    
    
    
    
98
    
    
    
    
99
    
    
    
    
100
    
    
    
    
101
    
    
    
    
102
    
    
    
    
103
    
    
    
    
104
    
    
    
    
105
    
    
    
    
106
    
    
    
    
107
    
    
    
    
108
    
    
    
    
109
    
    
    
    
110
    
    
    
    
111
    
    
    
    
112
    
    
    
    
113
    
    
    
    
114
    
    
    
    
115
    
    
    
    
116
    
    
    
    
117
    
    
    
    
118
    
    
    
    
119
    
    
    
    
120
    
    
    
    
121
    
    
    
    
122
    
    
    
    
123
    
    
    
    
124
    
    
    
    
125
    
    
    
    
126
    
    
    
    
127
    
    
    
    
128
    
    
    
    
129
    
    
    
    
130
    
    
    
    
131
    
    
    
    
132
    
    
    
    
133
    
    
    
    
134
    
    
    
    
135
    
    
    
    
136
    
    
    
    
137
    
    
    
    
138
    
    
    
    
139
    
    
    
    
140
    
    
    
    
141
    
    
    
    
142
    
    
    
    
143
    
    
    
    
144
    
    
    
    
145
    
    
    
    
146
    
    
    
    
147
    
    
    
    
148
    
    
    
    
149
    
    
    
    
150
    
    
    
    
151
    
    
    
    
152
    
    
    
    
153
    
    
    
    
154
    
    
    
    
155
    
    
    
    
156
    
    
    
    
157
    
    
    
    
158
    
    
    
    
159
    
    
    
    
160
    
    
    
    
161
    
    
    
    
162
    
    
    
    
163
    
    
    
    
164
    
    
    
    
165
    
    
    
    
166
    
    
    
    
167
    
    
    
    
168
    
    
    
    
169
    
    
    
    
170
    
    
    
    
171
    
    
    
    
172
    
    
    
    
173
    
    
    
    
174
    
    
    
    
175
    
    
    
    
176
    
    
    
    
177
    
    
    
    
178
    
    
    
    
179
    
    
    
    
180
    
    
    
    
181
    
    
    
    
182
    
    
    
    
183
    
    
    
    
184
    
    
    
    
185
    
    
    
    
186
    
    
    
    
187
    
    
    
    
188
    
    
    
    
189
    
    
    
    
190
    
    
    
    
191
    
    
    
    
192
    
    
    
    
193
    
    
    
    
194
    
    
    
    
195
    
    
    
    
196
    
    
    
    
197
    
    
    
    
198
    
    
    
    
199
    
    
    
    
200
    
    
    
    
201
    
    
    
    
202
    
    
    
    
203
    
    
    
    
204
    
    
    
    
205
    
    
    
    
206
    
    
    
    
207
    
    
    
    
208
    
    
    
    
209
    
    
    
    
210
    
    
    
    
211
    
    
    
    
212
    
    
    
    
213
    
    
    
    
214
    
    
    
    
215
    
    
    
    
216
    
    
    
    
217
    
    
    
    
218
    
    
    
    
219
    
    
    
    
220
    
    
    
    
221
    
    
    
    
222
    
    
    
    
223
    
    
    
    
224
    
    
    
    
225
    
    
    
    
226
    
    
    
    
227
    
    
    
    
228
    
    
    
    
229
    
    
    
    
230
    
    
    
    
231
    
    
    
    
232
    
    
    
    
233