1 //M*//////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42 /****************************************************************************************\
43 * Very fast SAD-based (Sum-of-Absolute-Diffrences) stereo correspondence algorithm. *
44 * Contributed by Kurt Konolige *
45 \****************************************************************************************/
46
47 #include "_cv.h"
48 /*
49 #undef CV_SSE2
50 #define CV_SSE2 1
51 #include "emmintrin.h"
52 */
53
54 CV_IMPL CvStereoBMState*
cvCreateStereoBMState(int,int numberOfDisparities)55 cvCreateStereoBMState( int /*preset*/, int numberOfDisparities )
56 {
57 CvStereoBMState* state = 0;
58
59 //CV_FUNCNAME( "cvCreateStereoBMState" );
60
61 __BEGIN__;
62
63 state = (CvStereoBMState*)cvAlloc( sizeof(*state) );
64 if( !state )
65 EXIT;
66
67 state->preFilterType = CV_STEREO_BM_NORMALIZED_RESPONSE;
68 state->preFilterSize = 9;
69 state->preFilterCap = 31;
70 state->SADWindowSize = 15;
71 state->minDisparity = 0;
72 state->numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : 64;
73 state->textureThreshold = 10;
74 state->uniquenessRatio = 15;
75 state->speckleRange = state->speckleWindowSize = 0;
76
77 state->preFilteredImg0 = state->preFilteredImg1 = state->slidingSumBuf = 0;
78
79 __END__;
80
81 if( cvGetErrStatus() < 0 )
82 cvReleaseStereoBMState( &state );
83 return state;
84 }
85
86
87 CV_IMPL void
cvReleaseStereoBMState(CvStereoBMState ** state)88 cvReleaseStereoBMState( CvStereoBMState** state )
89 {
90 CV_FUNCNAME( "cvReleaseStereoBMState" );
91
92 __BEGIN__;
93
94 if( !state )
95 CV_ERROR( CV_StsNullPtr, "" );
96
97 if( !*state )
98 EXIT;
99
100 cvReleaseMat( &(*state)->preFilteredImg0 );
101 cvReleaseMat( &(*state)->preFilteredImg1 );
102 cvReleaseMat( &(*state)->slidingSumBuf );
103 cvFree( state );
104
105 __END__;
106 }
107
icvPrefilter(const CvMat * src,CvMat * dst,int winsize,int ftzero,uchar * buf)108 static void icvPrefilter( const CvMat* src, CvMat* dst, int winsize, int ftzero, uchar* buf )
109 {
110 int x, y, wsz2 = winsize/2;
111 int* vsum = (int*)cvAlignPtr(buf + (wsz2 + 1)*sizeof(vsum[0]), 32);
112 int scale_g = winsize*winsize/8, scale_s = (1024 + scale_g)/(scale_g*2);
113 const int OFS = 256*5, TABSZ = OFS*2 + 256;
114 uchar tab[TABSZ];
115 const uchar* sptr = src->data.ptr;
116 int srcstep = src->step;
117 CvSize size = cvGetMatSize(src);
118
119 scale_g *= scale_s;
120
121 for( x = 0; x < TABSZ; x++ )
122 tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);
123
124 for( x = 0; x < size.width; x++ )
125 vsum[x] = (ushort)(sptr[x]*(wsz2 + 2));
126
127 for( y = 1; y < wsz2; y++ )
128 {
129 for( x = 0; x < size.width; x++ )
130 vsum[x] = (ushort)(vsum[x] + sptr[srcstep*y + x]);
131 }
132
133 for( y = 0; y < size.height; y++ )
134 {
135 const uchar* top = sptr + srcstep*MAX(y-wsz2-1,0);
136 const uchar* bottom = sptr + srcstep*MIN(y+wsz2,size.height-1);
137 const uchar* prev = sptr + srcstep*MAX(y-1,0);
138 const uchar* curr = sptr + srcstep*y;
139 const uchar* next = sptr + srcstep*MIN(y+1,size.height-1);
140 uchar* dptr = dst->data.ptr + dst->step*y;
141 x = 0;
142
143 for( ; x < size.width; x++ )
144 vsum[x] = (ushort)(vsum[x] + bottom[x] - top[x]);
145
146 for( x = 0; x <= wsz2; x++ )
147 {
148 vsum[-x-1] = vsum[0];
149 vsum[size.width+x] = vsum[size.width-1];
150 }
151
152 int sum = vsum[0]*(wsz2 + 1);
153 for( x = 1; x <= wsz2; x++ )
154 sum += vsum[x];
155
156 int val = ((curr[0]*5 + curr[1] + prev[0] + next[0])*scale_g - sum*scale_s) >> 10;
157 dptr[0] = tab[val + OFS];
158
159 for( x = 1; x < size.width-1; x++ )
160 {
161 sum += vsum[x+wsz2] - vsum[x-wsz2-1];
162 val = ((curr[x]*4 + curr[x-1] + curr[x+1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;
163 dptr[x] = tab[val + OFS];
164 }
165
166 sum += vsum[x+wsz2] - vsum[x-wsz2-1];
167 val = ((curr[x]*5 + curr[x-1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;
168 dptr[x] = tab[val + OFS];
169 }
170 }
171
172
173 static const int DISPARITY_SHIFT = 4;
174
175 #if CV_SSE2
176 static void
icvFindStereoCorrespondenceBM_SSE2(const CvMat * left,const CvMat * right,CvMat * disp,CvStereoBMState * state,uchar * buf,int _dy0,int _dy1)177 icvFindStereoCorrespondenceBM_SSE2( const CvMat* left, const CvMat* right,
178 CvMat* disp, CvStereoBMState* state,
179 uchar* buf, int _dy0, int _dy1 )
180 {
181 int x, y, d;
182 int wsz = state->SADWindowSize, wsz2 = wsz/2;
183 int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);
184 int ndisp = state->numberOfDisparities;
185 int mindisp = state->minDisparity;
186 int lofs = MAX(ndisp - 1 + mindisp, 0);
187 int rofs = -MIN(ndisp - 1 + mindisp, 0);
188 int width = left->cols, height = left->rows;
189 int width1 = width - rofs - ndisp + 1;
190 int ftzero = state->preFilterCap;
191 int textureThreshold = state->textureThreshold;
192 int uniquenessRatio = state->uniquenessRatio;
193 short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);
194
195 ushort *sad, *hsad0, *hsad, *hsad_sub;
196 int *htext;
197 uchar *cbuf0, *cbuf;
198 const uchar* lptr0 = left->data.ptr + lofs;
199 const uchar* rptr0 = right->data.ptr + rofs;
200 const uchar *lptr, *lptr_sub, *rptr;
201 short* dptr = disp->data.s;
202 int sstep = left->step;
203 int dstep = disp->step/sizeof(dptr[0]);
204 int cstep = (height + dy0 + dy1)*ndisp;
205 const int TABSZ = 256;
206 uchar tab[TABSZ];
207 const __m128i d0_8 = _mm_setr_epi16(0,1,2,3,4,5,6,7), dd_8 = _mm_set1_epi16(8);
208
209 sad = (ushort*)cvAlignPtr(buf + sizeof(sad[0]));
210 hsad0 = (ushort*)cvAlignPtr(sad + ndisp + 1 + dy0*ndisp);
211 htext = (int*)cvAlignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2);
212 cbuf0 = (uchar*)cvAlignPtr(htext + height + wsz2 + 2 + dy0*ndisp);
213
214 for( x = 0; x < TABSZ; x++ )
215 tab[x] = (uchar)abs(x - ftzero);
216
217 // initialize buffers
218 memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );
219 memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );
220
221 for( x = -wsz2-1; x < wsz2; x++ )
222 {
223 hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;
224 lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;
225 rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep;
226
227 for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
228 {
229 int lval = lptr[0];
230 for( d = 0; d < ndisp; d++ )
231 {
232 int diff = abs(lval - rptr[d]);
233 cbuf[d] = (uchar)diff;
234 hsad[d] = (ushort)(hsad[d] + diff);
235 }
236 htext[y] += tab[lval];
237 }
238 }
239
240 // initialize the left and right borders of the disparity map
241 for( y = 0; y < height; y++ )
242 {
243 for( x = 0; x < lofs; x++ )
244 dptr[y*dstep + x] = FILTERED;
245 for( x = lofs + width1; x < width; x++ )
246 dptr[y*dstep + x] = FILTERED;
247 }
248 dptr += lofs;
249
250 for( x = 0; x < width1; x++, dptr++ )
251 {
252 int x0 = x - wsz2 - 1, x1 = x + wsz2;
253 const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
254 uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
255 hsad = hsad0 - dy0*ndisp;
256 lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
257 lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
258 rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;
259
260 for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,
261 hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
262 {
263 int lval = lptr[0];
264 __m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128();
265 for( d = 0; d < ndisp; d += 16 )
266 {
267 __m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d));
268 __m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d));
269 __m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8));
270 __m128i cbs = _mm_load_si128((const __m128i*)(cbuf_sub + d));
271 __m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv));
272 __m128i diff_h = _mm_sub_epi16(_mm_unpackhi_epi8(diff, z), _mm_unpackhi_epi8(cbs, z));
273 _mm_store_si128((__m128i*)(cbuf + d), diff);
274 diff = _mm_sub_epi16(_mm_unpacklo_epi8(diff, z), _mm_unpacklo_epi8(cbs, z));
275 hsad_h = _mm_add_epi16(hsad_h, diff_h);
276 hsad_l = _mm_add_epi16(hsad_l, diff);
277 _mm_store_si128((__m128i*)(hsad + d), hsad_l);
278 _mm_store_si128((__m128i*)(hsad + d + 8), hsad_h);
279 }
280 htext[y] += tab[lval] - tab[lptr_sub[0]];
281 }
282
283 // fill borders
284 for( y = dy1; y <= wsz2; y++ )
285 htext[height+y] = htext[height+dy1-1];
286 for( y = -wsz2-1; y < -dy0; y++ )
287 htext[y] = htext[-dy0];
288
289 // initialize sums
290 for( d = 0; d < ndisp; d++ )
291 sad[d] = (ushort)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));
292
293 hsad = hsad0 + (1 - dy0)*ndisp;
294 for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
295 for( d = 0; d < ndisp; d++ )
296 sad[d] = (ushort)(sad[d] + hsad[d]);
297 int tsum = 0;
298 for( y = -wsz2-1; y < wsz2; y++ )
299 tsum += htext[y];
300
301 // finally, start the real processing
302 for( y = 0; y < height; y++ )
303 {
304 int minsad = INT_MAX, mind = -1;
305 hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;
306 hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;
307 __m128i minsad8 = _mm_set1_epi16(SHRT_MAX);
308 __m128i mind8 = _mm_set1_epi16(-1), d8 = d0_8, mask;
309
310 for( d = 0; d < ndisp; d += 8 )
311 {
312 __m128i v0 = _mm_load_si128((__m128i*)(hsad_sub + d));
313 __m128i v1 = _mm_load_si128((__m128i*)(hsad + d));
314 __m128i sad8 = _mm_load_si128((__m128i*)(sad + d));
315 sad8 = _mm_sub_epi16(sad8, v0);
316 sad8 = _mm_add_epi16(sad8, v1);
317
318 mask = _mm_cmpgt_epi16(minsad8, sad8);
319 _mm_store_si128((__m128i*)(sad + d), sad8);
320 minsad8 = _mm_min_epi16(minsad8, sad8);
321 mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(d8,mind8),mask));
322 d8 = _mm_add_epi16(d8, dd_8);
323 }
324
325 __m128i minsad82 = _mm_unpackhi_epi64(minsad8, minsad8);
326 __m128i mind82 = _mm_unpackhi_epi64(mind8, mind8);
327 mask = _mm_cmpgt_epi16(minsad8, minsad82);
328 mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
329 minsad8 = _mm_min_epi16(minsad8, minsad82);
330
331 minsad82 = _mm_shufflelo_epi16(minsad8, _MM_SHUFFLE(3,2,3,2));
332 mind82 = _mm_shufflelo_epi16(mind8, _MM_SHUFFLE(3,2,3,2));
333 mask = _mm_cmpgt_epi16(minsad8, minsad82);
334 mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
335 minsad8 = _mm_min_epi16(minsad8, minsad82);
336
337 minsad82 = _mm_shufflelo_epi16(minsad8, 1);
338 mind82 = _mm_shufflelo_epi16(mind8, 1);
339 mask = _mm_cmpgt_epi16(minsad8, minsad82);
340 mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
341 mind = (short)_mm_cvtsi128_si32(mind8);
342 minsad = sad[mind];
343 tsum += htext[y + wsz2] - htext[y - wsz2 - 1];
344 if( tsum < textureThreshold )
345 {
346 dptr[y*dstep] = FILTERED;
347 continue;
348 }
349
350 if( uniquenessRatio > 0 )
351 {
352 int thresh = minsad + (minsad * uniquenessRatio/100);
353 __m128i thresh8 = _mm_set1_epi16((short)(thresh + 1));
354 __m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+1));
355 __m128i d8 = d0_8;
356
357 for( d = 0; d < ndisp; d += 8 )
358 {
359 __m128i sad8 = _mm_load_si128((__m128i*)(sad + d));
360 __m128i mask = _mm_cmpgt_epi16( thresh8, sad8 );
361 mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi16(d1,d8), _mm_cmpgt_epi16(d8,d2)));
362 if( _mm_movemask_epi8(mask) )
363 break;
364 d8 = _mm_add_epi16(d8, dd_8);
365 }
366 if( d < ndisp )
367 {
368 dptr[y*dstep] = FILTERED;
369 continue;
370 }
371 }
372
373 {
374 sad[-1] = sad[1];
375 sad[ndisp] = sad[ndisp-2];
376 int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind];
377 dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);
378 }
379 }
380 }
381 }
382 #endif
383
384 static void
icvFindStereoCorrespondenceBM(const CvMat * left,const CvMat * right,CvMat * disp,CvStereoBMState * state,uchar * buf,int _dy0,int _dy1)385 icvFindStereoCorrespondenceBM( const CvMat* left, const CvMat* right,
386 CvMat* disp, CvStereoBMState* state,
387 uchar* buf, int _dy0, int _dy1 )
388 {
389 int x, y, d;
390 int wsz = state->SADWindowSize, wsz2 = wsz/2;
391 int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);
392 int ndisp = state->numberOfDisparities;
393 int mindisp = state->minDisparity;
394 int lofs = MAX(ndisp - 1 + mindisp, 0);
395 int rofs = -MIN(ndisp - 1 + mindisp, 0);
396 int width = left->cols, height = left->rows;
397 int width1 = width - rofs - ndisp + 1;
398 int ftzero = state->preFilterCap;
399 int textureThreshold = state->textureThreshold;
400 int uniquenessRatio = state->uniquenessRatio;
401 short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);
402
403 int *sad, *hsad0, *hsad, *hsad_sub, *htext;
404 uchar *cbuf0, *cbuf;
405 const uchar* lptr0 = left->data.ptr + lofs;
406 const uchar* rptr0 = right->data.ptr + rofs;
407 const uchar *lptr, *lptr_sub, *rptr;
408 short* dptr = disp->data.s;
409 int sstep = left->step;
410 int dstep = disp->step/sizeof(dptr[0]);
411 int cstep = (height+dy0+dy1)*ndisp;
412 const int TABSZ = 256;
413 uchar tab[TABSZ];
414
415 sad = (int*)cvAlignPtr(buf + sizeof(sad[0]));
416 hsad0 = (int*)cvAlignPtr(sad + ndisp + 1 + dy0*ndisp);
417 htext = (int*)cvAlignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2);
418 cbuf0 = (uchar*)cvAlignPtr(htext + height + wsz2 + 2 + dy0*ndisp);
419
420 for( x = 0; x < TABSZ; x++ )
421 tab[x] = (uchar)abs(x - ftzero);
422
423 // initialize buffers
424 memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );
425 memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );
426
427 for( x = -wsz2-1; x < wsz2; x++ )
428 {
429 hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;
430 lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;
431 rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep;
432
433 for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
434 {
435 int lval = lptr[0];
436 for( d = 0; d < ndisp; d++ )
437 {
438 int diff = abs(lval - rptr[d]);
439 cbuf[d] = (uchar)diff;
440 hsad[d] = (int)(hsad[d] + diff);
441 }
442 htext[y] += tab[lval];
443 }
444 }
445
446 // initialize the left and right borders of the disparity map
447 for( y = 0; y < height; y++ )
448 {
449 for( x = 0; x < lofs; x++ )
450 dptr[y*dstep + x] = FILTERED;
451 for( x = lofs + width1; x < width; x++ )
452 dptr[y*dstep + x] = FILTERED;
453 }
454 dptr += lofs;
455
456 for( x = 0; x < width1; x++, dptr++ )
457 {
458 int x0 = x - wsz2 - 1, x1 = x + wsz2;
459 const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
460 uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
461 hsad = hsad0 - dy0*ndisp;
462 lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
463 lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
464 rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;
465
466 for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,
467 hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
468 {
469 int lval = lptr[0];
470 for( d = 0; d < ndisp; d++ )
471 {
472 int diff = abs(lval - rptr[d]);
473 cbuf[d] = (uchar)diff;
474 hsad[d] = hsad[d] + diff - cbuf_sub[d];
475 }
476 htext[y] += tab[lval] - tab[lptr_sub[0]];
477 }
478
479 // fill borders
480 for( y = dy1; y <= wsz2; y++ )
481 htext[height+y] = htext[height+dy1-1];
482 for( y = -wsz2-1; y < -dy0; y++ )
483 htext[y] = htext[-dy0];
484
485 // initialize sums
486 for( d = 0; d < ndisp; d++ )
487 sad[d] = (int)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));
488
489 hsad = hsad0 + (1 - dy0)*ndisp;
490 for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
491 for( d = 0; d < ndisp; d++ )
492 sad[d] = (int)(sad[d] + hsad[d]);
493 int tsum = 0;
494 for( y = -wsz2-1; y < wsz2; y++ )
495 tsum += htext[y];
496
497 // finally, start the real processing
498 for( y = 0; y < height; y++ )
499 {
500 int minsad = INT_MAX, mind = -1;
501 hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;
502 hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;
503
504 for( d = 0; d < ndisp; d++ )
505 {
506 int currsad = sad[d] + hsad[d] - hsad_sub[d];
507 sad[d] = currsad;
508 if( currsad < minsad )
509 {
510 minsad = currsad;
511 mind = d;
512 }
513 }
514 tsum += htext[y + wsz2] - htext[y - wsz2 - 1];
515 if( tsum < textureThreshold )
516 {
517 dptr[y*dstep] = FILTERED;
518 continue;
519 }
520
521 if( uniquenessRatio > 0 )
522 {
523 int thresh = minsad + (minsad * uniquenessRatio/100);
524 for( d = 0; d < ndisp; d++ )
525 {
526 if( sad[d] <= thresh && (d < mind-1 || d > mind+1))
527 break;
528 }
529 if( d < ndisp )
530 {
531 dptr[y*dstep] = FILTERED;
532 continue;
533 }
534 }
535
536 {
537 sad[-1] = sad[1];
538 sad[ndisp] = sad[ndisp-2];
539 int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind];
540 dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);
541 }
542 }
543 }
544 }
545
546
547 CV_IMPL void
cvFindStereoCorrespondenceBM(const CvArr * leftarr,const CvArr * rightarr,CvArr * disparr,CvStereoBMState * state)548 cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr,
549 CvArr* disparr, CvStereoBMState* state )
550 {
551 CV_FUNCNAME( "cvFindStereoCorrespondenceBM" );
552
553 __BEGIN__;
554
555 CvMat lstub, *left0 = cvGetMat( leftarr, &lstub );
556 CvMat rstub, *right0 = cvGetMat( rightarr, &rstub );
557 CvMat left, right;
558 CvMat dstub, *disp = cvGetMat( disparr, &dstub );
559 int bufSize0, bufSize1, bufSize, width, width1, height;
560 int wsz, ndisp, mindisp, lofs, rofs;
561 int i, n = cvGetNumThreads();
562
563 if( !CV_ARE_SIZES_EQ(left0, right0) ||
564 !CV_ARE_SIZES_EQ(disp, left0) )
565 CV_ERROR( CV_StsUnmatchedSizes, "All the images must have the same size" );
566
567 if( CV_MAT_TYPE(left0->type) != CV_8UC1 ||
568 !CV_ARE_TYPES_EQ(left0, right0) ||
569 CV_MAT_TYPE(disp->type) != CV_16SC1 )
570 CV_ERROR( CV_StsUnsupportedFormat,
571 "Both input images must have 8uC1 format and the disparity image must have 16sC1 format" );
572
573 if( !state )
574 CV_ERROR( CV_StsNullPtr, "Stereo BM state is NULL." );
575
576 if( state->preFilterType != CV_STEREO_BM_NORMALIZED_RESPONSE )
577 CV_ERROR( CV_StsOutOfRange, "preFilterType must be =CV_STEREO_BM_NORMALIZED_RESPONSE" );
578
579 if( state->preFilterSize < 5 || state->preFilterSize > 255 || state->preFilterSize % 2 == 0 )
580 CV_ERROR( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" );
581
582 if( state->preFilterCap < 1 || state->preFilterCap > 63 )
583 CV_ERROR( CV_StsOutOfRange, "preFilterCap must be within 1..63" );
584
585 if( state->SADWindowSize < 5 || state->SADWindowSize > 255 || state->SADWindowSize % 2 == 0 ||
586 state->SADWindowSize >= MIN(left0->cols, left0->rows) )
587 CV_ERROR( CV_StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and "
588 "be not larger than image width or height" );
589
590 if( state->numberOfDisparities <= 0 || state->numberOfDisparities % 16 != 0 )
591 CV_ERROR( CV_StsOutOfRange, "numberOfDisparities must be positive and divisble by 16" );
592 if( state->textureThreshold < 0 )
593 CV_ERROR( CV_StsOutOfRange, "texture threshold must be non-negative" );
594 if( state->uniquenessRatio < 0 )
595 CV_ERROR( CV_StsOutOfRange, "uniqueness ratio must be non-negative" );
596
597 if( !state->preFilteredImg0 ||
598 state->preFilteredImg0->cols*state->preFilteredImg0->rows < left0->cols*left0->rows )
599 {
600 cvReleaseMat( &state->preFilteredImg0 );
601 cvReleaseMat( &state->preFilteredImg1 );
602
603 state->preFilteredImg0 = cvCreateMat( left0->rows, left0->cols, CV_8U );
604 state->preFilteredImg1 = cvCreateMat( left0->rows, left0->cols, CV_8U );
605 }
606 left = cvMat(left0->rows, left0->cols, CV_8U, state->preFilteredImg0->data.ptr);
607 right = cvMat(right0->rows, right0->cols, CV_8U, state->preFilteredImg1->data.ptr);
608
609 mindisp = state->minDisparity;
610 ndisp = state->numberOfDisparities;
611
612 width = left0->cols;
613 height = left0->rows;
614 lofs = MAX(ndisp - 1 + mindisp, 0);
615 rofs = -MIN(ndisp - 1 + mindisp, 0);
616 width1 = width - rofs - ndisp + 1;
617 if( lofs >= width || rofs >= width || width1 < 1 )
618 {
619 int FILTERED = (short)((state->minDisparity - 1) << DISPARITY_SHIFT);
620 cvSet( disp, cvScalarAll(FILTERED) );
621 EXIT;
622 }
623
624 wsz = state->SADWindowSize;
625 bufSize0 = (ndisp + 2)*sizeof(int) + (height+wsz+2)*ndisp*sizeof(int) +
626 (height + wsz + 2)*sizeof(int) + (height+wsz+2)*ndisp*(wsz+1)*sizeof(uchar) + 256;
627 bufSize1 = (width + state->preFilterSize + 2)*sizeof(int) + 256;
628 bufSize = MAX(bufSize0, bufSize1);
629 n = MAX(MIN(height/wsz, n), 1);
630
631 if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize*n )
632 {
633 cvReleaseMat( &state->slidingSumBuf );
634 state->slidingSumBuf = cvCreateMat( 1, bufSize*n, CV_8U );
635 }
636
637 #ifdef _OPENMP
638 #pragma omp parallel sections num_threads(n)
639 #endif
640 {
641 #ifdef _OPENMP
642 #pragma omp section
643 #endif
644 icvPrefilter( left0, &left, state->preFilterSize,
645 state->preFilterCap, state->slidingSumBuf->data.ptr );
646 #ifdef _OPENMP
647 #pragma omp section
648 #endif
649 icvPrefilter( right0, &right, state->preFilterSize,
650 state->preFilterCap, state->slidingSumBuf->data.ptr + bufSize1*(n>1) );
651 }
652
653 #ifdef _OPENMP
654 #pragma omp parallel for num_threads(n) schedule(static)
655 #endif
656 for( i = 0; i < n; i++ )
657 {
658 int thread_id = cvGetThreadNum();
659 CvMat left_i, right_i, disp_i;
660 int row0 = i*left.rows/n, row1 = (i+1)*left.rows/n;
661 cvGetRows( &left, &left_i, row0, row1 );
662 cvGetRows( &right, &right_i, row0, row1 );
663 cvGetRows( disp, &disp_i, row0, row1 );
664 #if CV_SSE2
665 if( state->preFilterCap <= 31 && state->SADWindowSize <= 21 )
666 {
667 icvFindStereoCorrespondenceBM_SSE2( &left_i, &right_i, &disp_i, state,
668 state->slidingSumBuf->data.ptr + thread_id*bufSize0, row0, left.rows-row1 );
669 }
670 else
671 #endif
672 {
673 icvFindStereoCorrespondenceBM( &left_i, &right_i, &disp_i, state,
674 state->slidingSumBuf->data.ptr + thread_id*bufSize0, row0, left.rows-row1 );
675 }
676 }
677
678 __END__;
679 }
680
681 /* End of file. */
682