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 54CV_IMPL CvStereoBMState* 55cvCreateStereoBMState( 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 87CV_IMPL void 88cvReleaseStereoBMState( 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 108static 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 173static const int DISPARITY_SHIFT = 4; 174 175#if CV_SSE2 176static void 177icvFindStereoCorrespondenceBM_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 384static void 385icvFindStereoCorrespondenceBM( 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 547CV_IMPL void 548cvFindStereoCorrespondenceBM( 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