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 #include "_cv.h" 43 44 #undef INFINITY 45 #define INFINITY 10000 46 #define OCCLUSION_PENALTY 10000 47 #define OCCLUSION_PENALTY2 1000 48 #define DENOMINATOR 16 49 #undef OCCLUDED 50 #define OCCLUDED CV_STEREO_GC_OCCLUDED 51 #define CUTOFF 1000 52 #define IS_BLOCKED(d1, d2) ((d1) > (d2)) 53 54 typedef struct GCVtx 55 { 56 GCVtx *next; 57 int parent; 58 int first; 59 int ts; 60 int dist; 61 short weight; 62 uchar t; 63 } 64 GCVtx; 65 66 typedef struct GCEdge 67 { 68 GCVtx* dst; 69 int next; 70 int weight; 71 } 72 GCEdge; 73 74 typedef struct CvStereoGCState2 75 { 76 int Ithreshold, interactionRadius; 77 int lambda, lambda1, lambda2, K; 78 int dataCostFuncTab[CUTOFF+1]; 79 int smoothnessR[CUTOFF*2+1]; 80 int smoothnessGrayDiff[512]; 81 GCVtx** orphans; 82 int maxOrphans; 83 } 84 CvStereoGCState2; 85 86 // truncTab[x+255] = MAX(x-255,0) 87 static uchar icvTruncTab[512]; 88 // cutoffSqrTab[x] = MIN(x*x, CUTOFF) 89 static int icvCutoffSqrTab[256]; 90 91 static void icvInitStereoConstTabs() 92 { 93 static volatile int initialized = 0; 94 if( !initialized ) 95 { 96 int i; 97 for( i = 0; i < 512; i++ ) 98 icvTruncTab[i] = (uchar)MIN(MAX(i-255,0),255); 99 for( i = 0; i < 256; i++ ) 100 icvCutoffSqrTab[i] = MIN(i*i, CUTOFF); 101 initialized = 1; 102 } 103 } 104 105 static void icvInitStereoTabs( CvStereoGCState2* state2 ) 106 { 107 int i, K = state2->K; 108 109 for( i = 0; i <= CUTOFF; i++ ) 110 state2->dataCostFuncTab[i] = MIN(i*DENOMINATOR - K, 0); 111 112 for( i = 0; i < CUTOFF*2 + 1; i++ ) 113 state2->smoothnessR[i] = MIN(abs(i-CUTOFF), state2->interactionRadius); 114 115 for( i = 0; i < 512; i++ ) 116 { 117 int diff = abs(i - 255); 118 state2->smoothnessGrayDiff[i] = diff < state2->Ithreshold ? state2->lambda1 : state2->lambda2; 119 } 120 } 121 122 123 static int icvGCResizeOrphansBuf( GCVtx**& orphans, int norphans ) 124 { 125 int i, newNOrphans = MAX(norphans*3/2, 256); 126 GCVtx** newOrphans = (GCVtx**)cvAlloc( newNOrphans*sizeof(orphans[0]) ); 127 for( i = 0; i < norphans; i++ ) 128 newOrphans[i] = orphans[i]; 129 cvFree( &orphans ); 130 orphans = newOrphans; 131 return newNOrphans; 132 } 133 134 static int64 icvGCMaxFlow( GCVtx* vtx, int nvtx, GCEdge* edges, GCVtx**& _orphans, int& _maxOrphans ) 135 { 136 const int TERMINAL = -1, ORPHAN = -2; 137 GCVtx stub, *nil = &stub, *first = nil, *last = nil; 138 int i, k; 139 int curr_ts = 0; 140 int64 flow = 0; 141 int norphans = 0, maxOrphans = _maxOrphans; 142 GCVtx** orphans = _orphans; 143 stub.next = nil; 144 145 // initialize the active queue and the graph vertices 146 for( i = 0; i < nvtx; i++ ) 147 { 148 GCVtx* v = vtx + i; 149 v->ts = 0; 150 if( v->weight != 0 ) 151 { 152 last = last->next = v; 153 v->dist = 1; 154 v->parent = TERMINAL; 155 v->t = v->weight < 0; 156 } 157 else 158 v->parent = 0; 159 } 160 161 first = first->next; 162 last->next = nil; 163 nil->next = 0; 164 165 // run the search-path -> augment-graph -> restore-trees loop 166 for(;;) 167 { 168 GCVtx* v, *u; 169 int e0 = -1, ei = 0, ej = 0, min_weight, weight; 170 uchar vt; 171 172 // grow S & T search trees, find an edge connecting them 173 while( first != nil ) 174 { 175 v = first; 176 if( v->parent ) 177 { 178 vt = v->t; 179 for( ei = v->first; ei != 0; ei = edges[ei].next ) 180 { 181 if( edges[ei^vt].weight == 0 ) 182 continue; 183 u = edges[ei].dst; 184 if( !u->parent ) 185 { 186 u->t = vt; 187 u->parent = ei ^ 1; 188 u->ts = v->ts; 189 u->dist = v->dist + 1; 190 if( !u->next ) 191 { 192 u->next = nil; 193 last = last->next = u; 194 } 195 continue; 196 } 197 198 if( u->t != vt ) 199 { 200 e0 = ei ^ vt; 201 break; 202 } 203 204 if( u->dist > v->dist+1 && u->ts <= v->ts ) 205 { 206 // reassign the parent 207 u->parent = ei ^ 1; 208 u->ts = v->ts; 209 u->dist = v->dist + 1; 210 } 211 } 212 if( e0 > 0 ) 213 break; 214 } 215 // exclude the vertex from the active list 216 first = first->next; 217 v->next = 0; 218 } 219 220 if( e0 <= 0 ) 221 break; 222 223 // find the minimum edge weight along the path 224 min_weight = edges[e0].weight; 225 assert( min_weight > 0 ); 226 // k = 1: source tree, k = 0: destination tree 227 for( k = 1; k >= 0; k-- ) 228 { 229 for( v = edges[e0^k].dst;; v = edges[ei].dst ) 230 { 231 if( (ei = v->parent) < 0 ) 232 break; 233 weight = edges[ei^k].weight; 234 min_weight = MIN(min_weight, weight); 235 assert( min_weight > 0 ); 236 } 237 weight = abs(v->weight); 238 min_weight = MIN(min_weight, weight); 239 assert( min_weight > 0 ); 240 } 241 242 // modify weights of the edges along the path and collect orphans 243 edges[e0].weight -= min_weight; 244 edges[e0^1].weight += min_weight; 245 flow += min_weight; 246 247 // k = 1: source tree, k = 0: destination tree 248 for( k = 1; k >= 0; k-- ) 249 { 250 for( v = edges[e0^k].dst;; v = edges[ei].dst ) 251 { 252 if( (ei = v->parent) < 0 ) 253 break; 254 edges[ei^(k^1)].weight += min_weight; 255 if( (edges[ei^k].weight -= min_weight) == 0 ) 256 { 257 if( norphans >= maxOrphans ) 258 maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); 259 orphans[norphans++] = v; 260 v->parent = ORPHAN; 261 } 262 } 263 264 v->weight = (short)(v->weight + min_weight*(1-k*2)); 265 if( v->weight == 0 ) 266 { 267 if( norphans >= maxOrphans ) 268 maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); 269 orphans[norphans++] = v; 270 v->parent = ORPHAN; 271 } 272 } 273 274 // restore the search trees by finding new parents for the orphans 275 curr_ts++; 276 while( norphans > 0 ) 277 { 278 GCVtx* v = orphans[--norphans]; 279 int d, min_dist = INT_MAX; 280 e0 = 0; 281 vt = v->t; 282 283 for( ei = v->first; ei != 0; ei = edges[ei].next ) 284 { 285 if( edges[ei^(vt^1)].weight == 0 ) 286 continue; 287 u = edges[ei].dst; 288 if( u->t != vt || u->parent == 0 ) 289 continue; 290 // compute the distance to the tree root 291 for( d = 0;; ) 292 { 293 if( u->ts == curr_ts ) 294 { 295 d += u->dist; 296 break; 297 } 298 ej = u->parent; 299 d++; 300 if( ej < 0 ) 301 { 302 if( ej == ORPHAN ) 303 d = INT_MAX-1; 304 else 305 { 306 u->ts = curr_ts; 307 u->dist = 1; 308 } 309 break; 310 } 311 u = edges[ej].dst; 312 } 313 314 // update the distance 315 if( ++d < INT_MAX ) 316 { 317 if( d < min_dist ) 318 { 319 min_dist = d; 320 e0 = ei; 321 } 322 for( u = edges[ei].dst; u->ts != curr_ts; u = edges[u->parent].dst ) 323 { 324 u->ts = curr_ts; 325 u->dist = --d; 326 } 327 } 328 } 329 330 if( (v->parent = e0) > 0 ) 331 { 332 v->ts = curr_ts; 333 v->dist = min_dist; 334 continue; 335 } 336 337 /* no parent is found */ 338 v->ts = 0; 339 for( ei = v->first; ei != 0; ei = edges[ei].next ) 340 { 341 u = edges[ei].dst; 342 ej = u->parent; 343 if( u->t != vt || !ej ) 344 continue; 345 if( edges[ei^(vt^1)].weight && !u->next ) 346 { 347 u->next = nil; 348 last = last->next = u; 349 } 350 if( ej > 0 && edges[ej].dst == v ) 351 { 352 if( norphans >= maxOrphans ) 353 maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); 354 orphans[norphans++] = u; 355 u->parent = ORPHAN; 356 } 357 } 358 } 359 } 360 361 _orphans = orphans; 362 _maxOrphans = maxOrphans; 363 364 return flow; 365 } 366 367 368 CvStereoGCState* cvCreateStereoGCState( int numberOfDisparities, int maxIters ) 369 { 370 CvStereoGCState* state = 0; 371 372 //CV_FUNCNAME("cvCreateStereoGCState"); 373 374 __BEGIN__; 375 376 state = (CvStereoGCState*)cvAlloc( sizeof(*state) ); 377 memset( state, 0, sizeof(*state) ); 378 state->minDisparity = 0; 379 state->numberOfDisparities = numberOfDisparities; 380 state->maxIters = maxIters <= 0 ? 3 : maxIters; 381 state->Ithreshold = 5; 382 state->interactionRadius = 1; 383 state->K = state->lambda = state->lambda1 = state->lambda2 = -1.f; 384 state->occlusionCost = OCCLUSION_PENALTY; 385 386 __END__; 387 388 return state; 389 } 390 391 void cvReleaseStereoGCState( CvStereoGCState** _state ) 392 { 393 CvStereoGCState* state; 394 395 if( !_state && !*_state ) 396 return; 397 398 state = *_state; 399 cvReleaseMat( &state->left ); 400 cvReleaseMat( &state->right ); 401 cvReleaseMat( &state->ptrLeft ); 402 cvReleaseMat( &state->ptrRight ); 403 cvReleaseMat( &state->vtxBuf ); 404 cvReleaseMat( &state->edgeBuf ); 405 cvFree( _state ); 406 } 407 408 // ||I(x) - J(x')|| = 409 // min(CUTOFF, 410 // min( 411 // max( 412 // max(minJ(x') - I(x), 0), 413 // max(I(x) - maxJ(x'), 0)), 414 // max( 415 // max(minI(x) - J(x'), 0), 416 // max(J(x') - maxI(x), 0)))**2) == 417 // min(CUTOFF, 418 // min( 419 // max(minJ(x') - I(x), 0) + 420 // max(I(x) - maxJ(x'), 0), 421 // 422 // max(minI(x) - J(x'), 0) + 423 // max(J(x') - maxI(x), 0)))**2) 424 // where (I, minI, maxI) and 425 // (J, minJ, maxJ) are stored as interleaved 3-channel images. 426 // minI, maxI are computed from I, 427 // minJ, maxJ are computed from J - see icvInitGraySubPix. 428 static inline int icvDataCostFuncGraySubpix( const uchar* a, const uchar* b ) 429 { 430 int va = a[0], vb = b[0]; 431 int da = icvTruncTab[b[1] - va + 255] + icvTruncTab[va - b[2] + 255]; 432 int db = icvTruncTab[a[1] - vb + 255] + icvTruncTab[vb - a[2] + 255]; 433 return icvCutoffSqrTab[MIN(da,db)]; 434 } 435 436 static inline int icvSmoothnessCostFunc( int da, int db, int maxR, const int* stabR, int scale ) 437 { 438 return da == db ? 0 : (da == OCCLUDED || db == OCCLUDED ? maxR : stabR[da - db])*scale; 439 } 440 441 static void icvInitGraySubpix( const CvMat* left, const CvMat* right, 442 CvMat* left3, CvMat* right3 ) 443 { 444 int k, x, y, rows = left->rows, cols = left->cols; 445 446 for( k = 0; k < 2; k++ ) 447 { 448 const CvMat* src = k == 0 ? left : right; 449 CvMat* dst = k == 0 ? left3 : right3; 450 int sstep = src->step; 451 452 for( y = 0; y < rows; y++ ) 453 { 454 const uchar* sptr = src->data.ptr + sstep*y; 455 const uchar* sptr_prev = y > 0 ? sptr - sstep : sptr; 456 const uchar* sptr_next = y < rows-1 ? sptr + sstep : sptr; 457 uchar* dptr = dst->data.ptr + dst->step*y; 458 int v_prev = sptr[0]; 459 460 for( x = 0; x < cols; x++, dptr += 3 ) 461 { 462 int v = sptr[x], v1, minv = v, maxv = v; 463 464 v1 = (v + v_prev)/2; 465 minv = MIN(minv, v1); maxv = MAX(maxv, v1); 466 v1 = (v + sptr_prev[x])/2; 467 minv = MIN(minv, v1); maxv = MAX(maxv, v1); 468 v1 = (v + sptr_next[x])/2; 469 minv = MIN(minv, v1); maxv = MAX(maxv, v1); 470 if( x < cols-1 ) 471 { 472 v1 = (v + sptr[x+1])/2; 473 minv = MIN(minv, v1); maxv = MAX(maxv, v1); 474 } 475 v_prev = v; 476 dptr[0] = (uchar)v; 477 dptr[1] = (uchar)minv; 478 dptr[2] = (uchar)maxv; 479 } 480 } 481 } 482 } 483 484 // Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)), 485 // where k = number_of_disparities*0.25. 486 static float 487 icvComputeK( CvStereoGCState* state ) 488 { 489 int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0; 490 int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd; 491 int k = MIN(MAX((nd + 2)/4, 3), nd); 492 int *arr = (int*)cvStackAlloc(k*sizeof(arr[0])), delta, t, sum = 0; 493 494 for( y = 0; y < rows; y++ ) 495 { 496 const uchar* lptr = state->left->data.ptr + state->left->step*y; 497 const uchar* rptr = state->right->data.ptr + state->right->step*y; 498 499 for( x = 0; x < cols; x++ ) 500 { 501 for( d = maxd-1, i = 0; d >= mind; d-- ) 502 { 503 x1 = x - d; 504 if( (unsigned)x1 >= (unsigned)cols ) 505 continue; 506 delta = icvDataCostFuncGraySubpix( lptr + x*3, rptr + x1*3 ); 507 if( i < k ) 508 arr[i++] = delta; 509 else 510 for( i = 0; i < k; i++ ) 511 if( delta < arr[i] ) 512 CV_SWAP( arr[i], delta, t ); 513 } 514 delta = arr[0]; 515 for( j = 1; j < i; j++ ) 516 delta = MAX(delta, arr[j]); 517 sum += delta; 518 n++; 519 } 520 } 521 522 return (float)sum/n; 523 } 524 525 static int64 icvComputeEnergy( const CvStereoGCState* state, const CvStereoGCState2* state2, 526 bool allOccluded ) 527 { 528 int x, y, rows = state->left->rows, cols = state->left->cols; 529 int64 E = 0; 530 const int* dtab = state2->dataCostFuncTab; 531 int maxR = state2->interactionRadius; 532 const int* stabR = state2->smoothnessR + CUTOFF; 533 const int* stabI = state2->smoothnessGrayDiff + 255; 534 const uchar* left = state->left->data.ptr; 535 const uchar* right = state->right->data.ptr; 536 short* dleft = state->dispLeft->data.s; 537 short* dright = state->dispRight->data.s; 538 int step = state->left->step; 539 int dstep = (int)(state->dispLeft->step/sizeof(short)); 540 541 assert( state->left->step == state->right->step && 542 state->dispLeft->step == state->dispRight->step ); 543 544 if( allOccluded ) 545 return (int64)OCCLUSION_PENALTY*rows*cols*2; 546 547 for( y = 0; y < rows; y++, left += step, right += step, dleft += dstep, dright += dstep ) 548 { 549 for( x = 0; x < cols; x++ ) 550 { 551 int d = dleft[x], x1, d1; 552 if( d == OCCLUDED ) 553 E += OCCLUSION_PENALTY; 554 else 555 { 556 x1 = x + d; 557 if( (unsigned)x1 >= (unsigned)cols ) 558 continue; 559 d1 = dright[x1]; 560 if( d == -d1 ) 561 E += dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )]; 562 } 563 564 if( x < cols-1 ) 565 { 566 d1 = dleft[x+1]; 567 E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+3]] ); 568 } 569 if( y < rows-1 ) 570 { 571 d1 = dleft[x+dstep]; 572 E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+step]] ); 573 } 574 575 d = dright[x]; 576 if( d == OCCLUDED ) 577 E += OCCLUSION_PENALTY; 578 579 if( x < cols-1 ) 580 { 581 d1 = dright[x+1]; 582 E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+3]] ); 583 } 584 if( y < rows-1 ) 585 { 586 d1 = dright[x+dstep]; 587 E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+step]] ); 588 } 589 assert( E >= 0 ); 590 } 591 } 592 593 return E; 594 } 595 596 static inline void icvAddEdge( GCVtx *x, GCVtx* y, GCEdge* edgeBuf, int nedges, int w, int rw ) 597 { 598 GCEdge *xy = edgeBuf + nedges, *yx = xy + 1; 599 600 assert( x != 0 && y != 0 ); 601 xy->dst = y; 602 xy->next = x->first; 603 xy->weight = (short)w; 604 x->first = nedges; 605 606 yx->dst = x; 607 yx->next = y->first; 608 yx->weight = (short)rw; 609 y->first = nedges+1; 610 } 611 612 static inline int icvAddTWeights( GCVtx* vtx, int sourceWeight, int sinkWeight ) 613 { 614 int w = vtx->weight; 615 if( w > 0 ) 616 sourceWeight += w; 617 else 618 sinkWeight -= w; 619 vtx->weight = (short)(sourceWeight - sinkWeight); 620 return MIN(sourceWeight, sinkWeight); 621 } 622 623 static inline int icvAddTerm( GCVtx* x, GCVtx* y, int A, int B, int C, int D, 624 GCEdge* edgeBuf, int& nedges ) 625 { 626 int dE = 0, w; 627 628 assert(B - A + C - D >= 0); 629 if( B < A ) 630 { 631 dE += icvAddTWeights(x, D, B); 632 dE += icvAddTWeights(y, 0, A - B); 633 if( (w = B - A + C - D) != 0 ) 634 { 635 icvAddEdge( x, y, edgeBuf, nedges, 0, w ); 636 nedges += 2; 637 } 638 } 639 else if( C < D ) 640 { 641 dE += icvAddTWeights(x, D, A + D - C); 642 dE += icvAddTWeights(y, 0, C - D); 643 if( (w = B - A + C - D) != 0 ) 644 { 645 icvAddEdge( x, y, edgeBuf, nedges, w, 0 ); 646 nedges += 2; 647 } 648 } 649 else 650 { 651 dE += icvAddTWeights(x, D, A); 652 if( B != A || C != D ) 653 { 654 icvAddEdge( x, y, edgeBuf, nedges, B - A, C - D ); 655 nedges += 2; 656 } 657 } 658 return dE; 659 } 660 661 static int64 icvAlphaExpand( int64 Eprev, int alpha, CvStereoGCState* state, CvStereoGCState2* state2 ) 662 { 663 GCVtx *var, *var1; 664 int64 E = 0; 665 int delta, E00=0, E0a=0, Ea0=0, Eaa=0; 666 int k, a, d, d1, x, y, x1, y1, rows = state->left->rows, cols = state->left->cols; 667 int nvtx = 0, nedges = 2; 668 GCVtx* vbuf = (GCVtx*)state->vtxBuf->data.ptr; 669 GCEdge* ebuf = (GCEdge*)state->edgeBuf->data.ptr; 670 int maxR = state2->interactionRadius; 671 const int* dtab = state2->dataCostFuncTab; 672 const int* stabR = state2->smoothnessR + CUTOFF; 673 const int* stabI = state2->smoothnessGrayDiff + 255; 674 const uchar* left0 = state->left->data.ptr; 675 const uchar* right0 = state->right->data.ptr; 676 short* dleft0 = state->dispLeft->data.s; 677 short* dright0 = state->dispRight->data.s; 678 GCVtx** pleft0 = (GCVtx**)state->ptrLeft->data.ptr; 679 GCVtx** pright0 = (GCVtx**)state->ptrRight->data.ptr; 680 int step = state->left->step; 681 int dstep = (int)(state->dispLeft->step/sizeof(short)); 682 int pstep = (int)(state->ptrLeft->step/sizeof(GCVtx*)); 683 int aa[] = { alpha, -alpha }; 684 685 double t = (double)cvGetTickCount(); 686 687 assert( state->left->step == state->right->step && 688 state->dispLeft->step == state->dispRight->step && 689 state->ptrLeft->step == state->ptrRight->step ); 690 for( k = 0; k < 2; k++ ) 691 { 692 ebuf[k].dst = 0; 693 ebuf[k].next = 0; 694 ebuf[k].weight = 0; 695 } 696 697 for( y = 0; y < rows; y++ ) 698 { 699 const uchar* left = left0 + step*y; 700 const uchar* right = right0 + step*y; 701 const short* dleft = dleft0 + dstep*y; 702 const short* dright = dright0 + dstep*y; 703 GCVtx** pleft = pleft0 + pstep*y; 704 GCVtx** pright = pright0 + pstep*y; 705 const uchar* lr[] = { left, right }; 706 const short* dlr[] = { dleft, dright }; 707 GCVtx** plr[] = { pleft, pright }; 708 709 for( k = 0; k < 2; k++ ) 710 { 711 a = aa[k]; 712 for( y1 = y+(y>0); y1 <= y+(y<rows-1); y1++ ) 713 { 714 const short* disp = (k == 0 ? dleft0 : dright0) + y1*dstep; 715 GCVtx** ptr = (k == 0 ? pleft0 : pright0) + y1*pstep; 716 for( x = 0; x < cols; x++ ) 717 { 718 GCVtx* v = ptr[x] = &vbuf[nvtx++]; 719 v->first = 0; 720 v->weight = disp[x] == (short)(OCCLUDED ? -OCCLUSION_PENALTY2 : 0); 721 } 722 } 723 } 724 725 for( x = 0; x < cols; x++ ) 726 { 727 d = dleft[x]; 728 x1 = x + d; 729 var = pleft[x]; 730 731 // (left + x, right + x + d) 732 if( d != alpha && d != OCCLUDED && (unsigned)x1 < (unsigned)cols ) 733 { 734 var1 = pright[x1]; 735 d1 = dright[x1]; 736 if( d == -d1 ) 737 { 738 assert( var1 != 0 ); 739 delta = IS_BLOCKED(alpha, d) ? INFINITY : 0; 740 //add inter edge 741 E += icvAddTerm( var, var1, 742 dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )], 743 delta, delta, 0, ebuf, nedges ); 744 } 745 else if( IS_BLOCKED(alpha, d) ) 746 E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges ); 747 } 748 749 // (left + x, right + x + alpha) 750 x1 = x + alpha; 751 if( (unsigned)x1 < (unsigned)cols ) 752 { 753 var1 = pright[x1]; 754 d1 = dright[x1]; 755 756 E0a = IS_BLOCKED(d, alpha) ? INFINITY : 0; 757 Ea0 = IS_BLOCKED(-d1, alpha) ? INFINITY : 0; 758 Eaa = dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )]; 759 E += icvAddTerm( var, var1, 0, E0a, Ea0, Eaa, ebuf, nedges ); 760 } 761 762 // smoothness 763 for( k = 0; k < 2; k++ ) 764 { 765 GCVtx** p = plr[k]; 766 const short* disp = dlr[k]; 767 const uchar* img = lr[k] + x*3; 768 int scale; 769 var = p[x]; 770 d = disp[x]; 771 a = aa[k]; 772 773 if( x < cols - 1 ) 774 { 775 var1 = p[x+1]; 776 d1 = disp[x+1]; 777 scale = stabI[img[0] - img[3]]; 778 E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale ); 779 Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale ); 780 E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale ); 781 E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges ); 782 } 783 784 if( y < rows - 1 ) 785 { 786 var1 = p[x+pstep]; 787 d1 = disp[x+dstep]; 788 scale = stabI[img[0] - img[step]]; 789 E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale ); 790 Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale ); 791 E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale ); 792 E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges ); 793 } 794 } 795 796 // visibility term 797 if( d != OCCLUDED && IS_BLOCKED(alpha, -d)) 798 { 799 x1 = x + d; 800 if( (unsigned)x1 < (unsigned)cols ) 801 { 802 if( d != -dleft[x1] ) 803 { 804 var1 = pleft[x1]; 805 E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges ); 806 } 807 } 808 } 809 } 810 } 811 812 t = (double)cvGetTickCount() - t; 813 ebuf[0].weight = ebuf[1].weight = 0; 814 E += icvGCMaxFlow( vbuf, nvtx, ebuf, state2->orphans, state2->maxOrphans ); 815 816 if( E < Eprev ) 817 { 818 for( y = 0; y < rows; y++ ) 819 { 820 short* dleft = dleft0 + dstep*y; 821 short* dright = dright0 + dstep*y; 822 GCVtx** pleft = pleft0 + pstep*y; 823 GCVtx** pright = pright0 + pstep*y; 824 for( x = 0; x < cols; x++ ) 825 { 826 GCVtx* var = pleft[x]; 827 if( var && var->parent && var->t ) 828 dleft[x] = (short)alpha; 829 830 var = pright[x]; 831 if( var && var->parent && var->t ) 832 dright[x] = (short)-alpha; 833 } 834 } 835 } 836 837 return MIN(E, Eprev); 838 } 839 840 841 CV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right, 842 CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess ) 843 { 844 CvStereoGCState2 state2; 845 state2.orphans = 0; 846 state2.maxOrphans = 0; 847 848 CV_FUNCNAME( "cvFindStereoCorrespondenceGC" ); 849 850 __BEGIN__; 851 852 CvMat lstub, *left = cvGetMat( _left, &lstub ); 853 CvMat rstub, *right = cvGetMat( _right, &rstub ); 854 CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub ); 855 CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub ); 856 CvSize size; 857 int iter, i, nZeroExpansions = 0; 858 CvRNG rng = cvRNG(-1); 859 int* disp; 860 CvMat _disp; 861 int64 E; 862 863 CV_ASSERT( state != 0 ); 864 CV_ASSERT( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) && 865 CV_MAT_TYPE(left->type) == CV_8UC1 ); 866 CV_ASSERT( !dispLeft || 867 (CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) ); 868 CV_ASSERT( !dispRight || 869 (CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) ); 870 871 size = cvGetSize(left); 872 if( !state->left || state->left->width != size.width || state->left->height != size.height ) 873 { 874 int pcn = (int)(sizeof(GCVtx*)/sizeof(int)); 875 int vcn = (int)(sizeof(GCVtx)/sizeof(int)); 876 int ecn = (int)(sizeof(GCEdge)/sizeof(int)); 877 cvReleaseMat( &state->left ); 878 cvReleaseMat( &state->right ); 879 cvReleaseMat( &state->ptrLeft ); 880 cvReleaseMat( &state->ptrRight ); 881 cvReleaseMat( &state->dispLeft ); 882 cvReleaseMat( &state->dispRight ); 883 884 state->left = cvCreateMat( size.height, size.width, CV_8UC3 ); 885 state->right = cvCreateMat( size.height, size.width, CV_8UC3 ); 886 state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 ); 887 state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 ); 888 state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) ); 889 state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) ); 890 state->vtxBuf = cvCreateMat( 1, size.height*size.width*2, CV_32SC(vcn) ); 891 state->edgeBuf = cvCreateMat( 1, size.height*size.width*12 + 16, CV_32SC(ecn) ); 892 } 893 894 if( !useDisparityGuess ) 895 { 896 cvSet( state->dispLeft, cvScalarAll(OCCLUDED)); 897 cvSet( state->dispRight, cvScalarAll(OCCLUDED)); 898 } 899 else 900 { 901 CV_ASSERT( dispLeft && dispRight ); 902 cvConvert( dispLeft, state->dispLeft ); 903 cvConvert( dispRight, state->dispRight ); 904 } 905 906 state2.Ithreshold = state->Ithreshold; 907 state2.interactionRadius = state->interactionRadius; 908 state2.lambda = cvRound(state->lambda*DENOMINATOR); 909 state2.lambda1 = cvRound(state->lambda1*DENOMINATOR); 910 state2.lambda2 = cvRound(state->lambda2*DENOMINATOR); 911 state2.K = cvRound(state->K*DENOMINATOR); 912 913 icvInitStereoConstTabs(); 914 icvInitGraySubpix( left, right, state->left, state->right ); 915 disp = (int*)cvStackAlloc( state->numberOfDisparities*sizeof(disp[0]) ); 916 _disp = cvMat( 1, state->numberOfDisparities, CV_32S, disp ); 917 cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities ); 918 cvRandShuffle( &_disp, &rng ); 919 920 if( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) ) 921 { 922 float L = icvComputeK(state)*0.2f; 923 state2.lambda = cvRound(L*DENOMINATOR); 924 } 925 926 if( state2.K < 0 ) 927 state2.K = state2.lambda*5; 928 if( state2.lambda1 < 0 ) 929 state2.lambda1 = state2.lambda*3; 930 if( state2.lambda2 < 0 ) 931 state2.lambda2 = state2.lambda; 932 933 icvInitStereoTabs( &state2 ); 934 935 E = icvComputeEnergy( state, &state2, !useDisparityGuess ); 936 for( iter = 0; iter < state->maxIters; iter++ ) 937 { 938 for( i = 0; i < state->numberOfDisparities; i++ ) 939 { 940 int alpha = disp[i]; 941 int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 ); 942 if( Enew < E ) 943 { 944 nZeroExpansions = 0; 945 E = Enew; 946 } 947 else if( ++nZeroExpansions >= state->numberOfDisparities ) 948 break; 949 } 950 } 951 952 if( dispLeft ) 953 cvConvert( state->dispLeft, dispLeft ); 954 if( dispRight ) 955 cvConvert( state->dispRight, dispRight ); 956 957 __END__; 958 959 cvFree( &state2.orphans ); 960 } 961
