vcs.maemo.org Git - opencv/blob - src/cv/cvstereogc.cpp

   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, *nilNode = &stub, *first = nilNode, *last = nilNode;
 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 = nilNode;
 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 = nilNode;
 163     nilNode->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 != nilNode )
 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 = nilNode;
 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 = nilNode;
 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 }