modules/legacy/src/stereogc.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 "precomp.hpp"
  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* v1 = orphans[--norphans];
 279             int d, min_dist = INT_MAX;
 280             e0 = 0;
 281             vt = v1->t;
 282
 283             for( ei = v1->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( (v1->parent = e0) > 0 )
 331             {
 332                 v1->ts = curr_ts;
 333                 v1->dist = min_dist;
 334                 continue;
 335             }
 336
 337             /* no parent is found */
 338             v1->ts = 0;
 339             for( ei = v1->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 == v1 )
 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     state = (CvStereoGCState*)cvAlloc( sizeof(*state) );
 373     memset( state, 0, sizeof(*state) );
 374     state->minDisparity = 0;
 375     state->numberOfDisparities = numberOfDisparities;
 376     state->maxIters = maxIters <= 0 ? 3 : maxIters;
 377     state->Ithreshold = 5;
 378     state->interactionRadius = 1;
 379     state->K = state->lambda = state->lambda1 = state->lambda2 = -1.f;
 380     state->occlusionCost = OCCLUSION_PENALTY;
 381
 382     return state;
 383 }
 384
 385 void cvReleaseStereoGCState( CvStereoGCState** _state )
 386 {
 387     CvStereoGCState* state;
 388
 389     if( !_state && !*_state )
 390         return;
 391
 392     state = *_state;
 393     cvReleaseMat( &state->left );
 394     cvReleaseMat( &state->right );
 395     cvReleaseMat( &state->ptrLeft );
 396     cvReleaseMat( &state->ptrRight );
 397     cvReleaseMat( &state->vtxBuf );
 398     cvReleaseMat( &state->edgeBuf );
 399     cvFree( _state );
 400 }
 401
 402 // ||I(x) - J(x')|| =
 403 // min(CUTOFF,
 404 //   min(
 405 //     max(
 406 //       max(minJ(x') - I(x), 0),
 407 //       max(I(x) - maxJ(x'), 0)),
 408 //     max(
 409 //       max(minI(x) - J(x'), 0),
 410 //       max(J(x') - maxI(x), 0)))**2) ==
 411 // min(CUTOFF,
 412 //   min(
 413 //       max(minJ(x') - I(x), 0) +
 414 //       max(I(x) - maxJ(x'), 0),
 415 //
 416 //       max(minI(x) - J(x'), 0) +
 417 //       max(J(x') - maxI(x), 0)))**2)
 418 // where (I, minI, maxI) and
 419 //       (J, minJ, maxJ) are stored as interleaved 3-channel images.
 420 // minI, maxI are computed from I,
 421 // minJ, maxJ are computed from J - see icvInitGraySubPix.
 422 static inline int icvDataCostFuncGraySubpix( const uchar* a, const uchar* b )
 423 {
 424     int va = a[0], vb = b[0];
 425     int da = icvTruncTab[b[1] - va + 255] + icvTruncTab[va - b[2] + 255];
 426     int db = icvTruncTab[a[1] - vb + 255] + icvTruncTab[vb - a[2] + 255];
 427     return icvCutoffSqrTab[MIN(da,db)];
 428 }
 429
 430 static inline int icvSmoothnessCostFunc( int da, int db, int maxR, const int* stabR, int scale )
 431 {
 432     return da == db ? 0 : (da == OCCLUDED || db == OCCLUDED ? maxR : stabR[da - db])*scale;
 433 }
 434
 435 static void icvInitGraySubpix( const CvMat* left, const CvMat* right,
 436                                CvMat* left3, CvMat* right3 )
 437 {
 438     int k, x, y, rows = left->rows, cols = left->cols;
 439
 440     for( k = 0; k < 2; k++ )
 441     {
 442         const CvMat* src = k == 0 ? left : right;
 443         CvMat* dst = k == 0 ? left3 : right3;
 444         int sstep = src->step;
 445
 446         for( y = 0; y < rows; y++ )
 447         {
 448             const uchar* sptr = src->data.ptr + sstep*y;
 449             const uchar* sptr_prev = y > 0 ? sptr - sstep : sptr;
 450             const uchar* sptr_next = y < rows-1 ? sptr + sstep : sptr;
 451             uchar* dptr = dst->data.ptr + dst->step*y;
 452             int v_prev = sptr[0];
 453
 454             for( x = 0; x < cols; x++, dptr += 3 )
 455             {
 456                 int v = sptr[x], v1, minv = v, maxv = v;
 457
 458                 v1 = (v + v_prev)/2;
 459                 minv = MIN(minv, v1); maxv = MAX(maxv, v1);
 460                 v1 = (v + sptr_prev[x])/2;
 461                 minv = MIN(minv, v1); maxv = MAX(maxv, v1);
 462                 v1 = (v + sptr_next[x])/2;
 463                 minv = MIN(minv, v1); maxv = MAX(maxv, v1);
 464                 if( x < cols-1 )
 465                 {
 466                     v1 = (v + sptr[x+1])/2;
 467                     minv = MIN(minv, v1); maxv = MAX(maxv, v1);
 468                 }
 469                 v_prev = v;
 470                 dptr[0] = (uchar)v;
 471                 dptr[1] = (uchar)minv;
 472                 dptr[2] = (uchar)maxv;
 473             }
 474         }
 475     }
 476 }
 477
 478 // Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)),
 479 // where k = number_of_disparities*0.25.
 480 static float
 481 icvComputeK( CvStereoGCState* state )
 482 {
 483     int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0;
 484     int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd;
 485     int k = MIN(MAX((nd + 2)/4, 3), nd), delta, t, sum = 0;
 486     std::vector<int> _arr(k+1);
 487     int *arr = &_arr[0];
 488
 489     for( y = 0; y < rows; y++ )
 490     {
 491         const uchar* lptr = state->left->data.ptr + state->left->step*y;
 492         const uchar* rptr = state->right->data.ptr + state->right->step*y;
 493
 494         for( x = 0; x < cols; x++ )
 495         {
 496             for( d = maxd-1, i = 0; d >= mind; d-- )
 497             {
 498                 x1 = x - d;
 499                 if( (unsigned)x1 >= (unsigned)cols )
 500                     continue;
 501                 delta = icvDataCostFuncGraySubpix( lptr + x*3, rptr + x1*3 );
 502                 if( i < k )
 503                     arr[i++] = delta;
 504                 else
 505                     for( i = 0; i < k; i++ )
 506                         if( delta < arr[i] )
 507                             CV_SWAP( arr[i], delta, t );
 508             }
 509             delta = arr[0];
 510             for( j = 1; j < i; j++ )
 511                 delta = MAX(delta, arr[j]);
 512             sum += delta;
 513             n++;
 514         }
 515     }
 516
 517     return (float)sum/n;
 518 }
 519
 520 static int64 icvComputeEnergy( const CvStereoGCState* state, const CvStereoGCState2* state2,
 521                                bool allOccluded )
 522 {
 523     int x, y, rows = state->left->rows, cols = state->left->cols;
 524     int64 E = 0;
 525     const int* dtab = state2->dataCostFuncTab;
 526     int maxR = state2->interactionRadius;
 527     const int* stabR = state2->smoothnessR + CUTOFF;
 528     const int* stabI = state2->smoothnessGrayDiff + 255;
 529     const uchar* left = state->left->data.ptr;
 530     const uchar* right = state->right->data.ptr;
 531     short* dleft = state->dispLeft->data.s;
 532     short* dright = state->dispRight->data.s;
 533     int step = state->left->step;
 534     int dstep = (int)(state->dispLeft->step/sizeof(short));
 535
 536     assert( state->left->step == state->right->step &&
 537         state->dispLeft->step == state->dispRight->step );
 538
 539     if( allOccluded )
 540         return (int64)OCCLUSION_PENALTY*rows*cols*2;
 541
 542     for( y = 0; y < rows; y++, left += step, right += step, dleft += dstep, dright += dstep )
 543     {
 544         for( x = 0; x < cols; x++ )
 545         {
 546             int d = dleft[x], x1, d1;
 547             if( d == OCCLUDED )
 548                 E += OCCLUSION_PENALTY;
 549             else
 550             {
 551                 x1 = x + d;
 552                 if( (unsigned)x1 >= (unsigned)cols )
 553                     continue;
 554                 d1 = dright[x1];
 555                 if( d == -d1 )
 556                     E += dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )];
 557             }
 558
 559             if( x < cols-1 )
 560             {
 561                 d1 = dleft[x+1];
 562                 E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+3]] );
 563             }
 564             if( y < rows-1 )
 565             {
 566                 d1 = dleft[x+dstep];
 567                 E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+step]] );
 568             }
 569
 570             d = dright[x];
 571             if( d == OCCLUDED )
 572                 E += OCCLUSION_PENALTY;
 573
 574             if( x < cols-1 )
 575             {
 576                 d1 = dright[x+1];
 577                 E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+3]] );
 578             }
 579             if( y < rows-1 )
 580             {
 581                 d1 = dright[x+dstep];
 582                 E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+step]] );
 583             }
 584             assert( E >= 0 );
 585         }
 586     }
 587
 588     return E;
 589 }
 590
 591 static inline void icvAddEdge( GCVtx *x, GCVtx* y, GCEdge* edgeBuf, int nedges, int w, int rw )
 592 {
 593     GCEdge *xy = edgeBuf + nedges, *yx = xy + 1;
 594
 595     assert( x != 0 && y != 0 );
 596     xy->dst = y;
 597     xy->next = x->first;
 598     xy->weight = (short)w;
 599     x->first = nedges;
 600
 601     yx->dst = x;
 602     yx->next = y->first;
 603     yx->weight = (short)rw;
 604     y->first = nedges+1;
 605 }
 606
 607 static inline int icvAddTWeights( GCVtx* vtx, int sourceWeight, int sinkWeight )
 608 {
 609     int w = vtx->weight;
 610     if( w > 0 )
 611         sourceWeight += w;
 612     else
 613         sinkWeight -= w;
 614     vtx->weight = (short)(sourceWeight - sinkWeight);
 615     return MIN(sourceWeight, sinkWeight);
 616 }
 617
 618 static inline int icvAddTerm( GCVtx* x, GCVtx* y, int A, int B, int C, int D,
 619                               GCEdge* edgeBuf, int& nedges )
 620 {
 621     int dE = 0, w;
 622
 623     assert(B - A + C - D >= 0);
 624     if( B < A )
 625     {
 626         dE += icvAddTWeights(x, D, B);
 627         dE += icvAddTWeights(y, 0, A - B);
 628         if( (w = B - A + C - D) != 0 )
 629         {
 630             icvAddEdge( x, y, edgeBuf, nedges, 0, w );
 631             nedges += 2;
 632         }
 633     }
 634     else if( C < D )
 635     {
 636         dE += icvAddTWeights(x, D, A + D - C);
 637         dE += icvAddTWeights(y, 0, C - D);
 638         if( (w = B - A + C - D) != 0 )
 639         {
 640             icvAddEdge( x, y, edgeBuf, nedges, w, 0 );
 641             nedges += 2;
 642         }
 643     }
 644     else
 645     {
 646         dE += icvAddTWeights(x, D, A);
 647         if( B != A || C != D )
 648         {
 649             icvAddEdge( x, y, edgeBuf, nedges, B - A, C - D );
 650             nedges += 2;
 651         }
 652     }
 653     return dE;
 654 }
 655
 656 static int64 icvAlphaExpand( int64 Eprev, int alpha, CvStereoGCState* state, CvStereoGCState2* state2 )
 657 {
 658     GCVtx *var, *var1;
 659     int64 E = 0;
 660     int delta, E00=0, E0a=0, Ea0=0, Eaa=0;
 661     int k, a, d, d1, x, y, x1, y1, rows = state->left->rows, cols = state->left->cols;
 662     int nvtx = 0, nedges = 2;
 663     GCVtx* vbuf = (GCVtx*)state->vtxBuf->data.ptr;
 664     GCEdge* ebuf = (GCEdge*)state->edgeBuf->data.ptr;
 665     int maxR = state2->interactionRadius;
 666     const int* dtab = state2->dataCostFuncTab;
 667     const int* stabR = state2->smoothnessR + CUTOFF;
 668     const int* stabI = state2->smoothnessGrayDiff + 255;
 669     const uchar* left0 = state->left->data.ptr;
 670     const uchar* right0 = state->right->data.ptr;
 671     short* dleft0 = state->dispLeft->data.s;
 672     short* dright0 = state->dispRight->data.s;
 673     GCVtx** pleft0 = (GCVtx**)state->ptrLeft->data.ptr;
 674     GCVtx** pright0 = (GCVtx**)state->ptrRight->data.ptr;
 675     int step = state->left->step;
 676     int dstep = (int)(state->dispLeft->step/sizeof(short));
 677     int pstep = (int)(state->ptrLeft->step/sizeof(GCVtx*));
 678     int aa[] = { alpha, -alpha };
 679
 680     //double t = (double)cvGetTickCount();
 681
 682     assert( state->left->step == state->right->step &&
 683             state->dispLeft->step == state->dispRight->step &&
 684             state->ptrLeft->step == state->ptrRight->step );
 685     for( k = 0; k < 2; k++ )
 686     {
 687         ebuf[k].dst = 0;
 688         ebuf[k].next = 0;
 689         ebuf[k].weight = 0;
 690     }
 691
 692     for( y = 0; y < rows; y++ )
 693     {
 694         const uchar* left = left0 + step*y;
 695         const uchar* right = right0 + step*y;
 696         const short* dleft = dleft0 + dstep*y;
 697         const short* dright = dright0 + dstep*y;
 698         GCVtx** pleft = pleft0 + pstep*y;
 699         GCVtx** pright = pright0 + pstep*y;
 700         const uchar* lr[] = { left, right };
 701         const short* dlr[] = { dleft, dright };
 702         GCVtx** plr[] = { pleft, pright };
 703
 704         for( k = 0; k < 2; k++ )
 705         {
 706             a = aa[k];
 707             for( y1 = y+(y>0); y1 <= y+(y<rows-1); y1++ )
 708             {
 709                 const short* disp = (k == 0 ? dleft0 : dright0) + y1*dstep;
 710                 GCVtx** ptr = (k == 0 ? pleft0 : pright0) + y1*pstep;
 711                 for( x = 0; x < cols; x++ )
 712                 {
 713                     GCVtx* v = ptr[x] = &vbuf[nvtx++];
 714                     v->first = 0;
 715                     v->weight = disp[x] == (short)(OCCLUDED ? -OCCLUSION_PENALTY2 : 0);
 716                 }
 717             }
 718         }
 719
 720         for( x = 0; x < cols; x++ )
 721         {
 722             d = dleft[x];
 723             x1 = x + d;
 724             var = pleft[x];
 725
 726             // (left + x, right + x + d)
 727             if( d != alpha && d != OCCLUDED && (unsigned)x1 < (unsigned)cols )
 728             {
 729                 var1 = pright[x1];
 730                 d1 = dright[x1];
 731                 if( d == -d1 )
 732                 {
 733                     assert( var1 != 0 );
 734                     delta = IS_BLOCKED(alpha, d) ? INFINITY : 0;
 735                     //add inter edge
 736                     E += icvAddTerm( var, var1,
 737                         dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )],
 738                         delta, delta, 0, ebuf, nedges );
 739                 }
 740                 else if( IS_BLOCKED(alpha, d) )
 741                     E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges );
 742             }
 743
 744             // (left + x, right + x + alpha)
 745             x1 = x + alpha;
 746             if( (unsigned)x1 < (unsigned)cols )
 747             {
 748                 var1 = pright[x1];
 749                 d1 = dright[x1];
 750
 751                 E0a = IS_BLOCKED(d, alpha) ? INFINITY : 0;
 752                 Ea0 = IS_BLOCKED(-d1, alpha) ? INFINITY : 0;
 753                 Eaa = dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )];
 754                 E += icvAddTerm( var, var1, 0, E0a, Ea0, Eaa, ebuf, nedges );
 755             }
 756
 757             // smoothness
 758             for( k = 0; k < 2; k++ )
 759             {
 760                 GCVtx** p = plr[k];
 761                 const short* disp = dlr[k];
 762                 const uchar* img = lr[k] + x*3;
 763                 int scale;
 764                 var = p[x];
 765                 d = disp[x];
 766                 a = aa[k];
 767
 768                 if( x < cols - 1 )
 769                 {
 770                     var1 = p[x+1];
 771                     d1 = disp[x+1];
 772                     scale = stabI[img[0] - img[3]];
 773                     E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale );
 774                     Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale );
 775                     E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale );
 776                     E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges );
 777                 }
 778
 779                 if( y < rows - 1 )
 780                 {
 781                     var1 = p[x+pstep];
 782                     d1 = disp[x+dstep];
 783                     scale = stabI[img[0] - img[step]];
 784                     E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale );
 785                     Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale );
 786                     E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale );
 787                     E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges );
 788                 }
 789             }
 790
 791             // visibility term
 792             if( d != OCCLUDED && IS_BLOCKED(alpha, -d))
 793             {
 794                 x1 = x + d;
 795                 if( (unsigned)x1 < (unsigned)cols )
 796                 {
 797                     if( d != -dleft[x1] )
 798                     {
 799                         var1 = pleft[x1];
 800                         E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges );
 801                     }
 802                 }
 803             }
 804         }
 805     }
 806
 807     //t = (double)cvGetTickCount() - t;
 808     ebuf[0].weight = ebuf[1].weight = 0;
 809     E += icvGCMaxFlow( vbuf, nvtx, ebuf, state2->orphans, state2->maxOrphans );
 810
 811     if( E < Eprev )
 812     {
 813         for( y = 0; y < rows; y++ )
 814         {
 815             short* dleft = dleft0 + dstep*y;
 816             short* dright = dright0 + dstep*y;
 817             GCVtx** pleft = pleft0 + pstep*y;
 818             GCVtx** pright = pright0 + pstep*y;
 819             for( x = 0; x < cols; x++ )
 820             {
 821                 GCVtx* var2 = pleft[x];
 822                 if( var2 && var2->parent && var2->t )
 823                     dleft[x] = (short)alpha;
 824
 825                 var2 = pright[x];
 826                 if( var2 && var2->parent && var2->t )
 827                     dright[x] = (short)-alpha;
 828             }
 829         }
 830     }
 831
 832     return MIN(E, Eprev);
 833 }
 834
 835
 836 CV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right,
 837     CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess )
 838 {
 839     CvStereoGCState2 state2;
 840     state2.orphans = 0;
 841     state2.maxOrphans = 0;
 842
 843     CvMat lstub, *left = cvGetMat( _left, &lstub );
 844     CvMat rstub, *right = cvGetMat( _right, &rstub );
 845     CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub );
 846     CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub );
 847     CvSize size;
 848     int iter, i, nZeroExpansions = 0;
 849     CvRNG rng = cvRNG(-1);
 850     int64 E;
 851
 852     CV_Assert( state != 0 );
 853     CV_Assert( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) &&
 854                CV_MAT_TYPE(left->type) == CV_8UC1 );
 855     CV_Assert( !dispLeft ||
 856         (CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) );
 857     CV_Assert( !dispRight ||
 858         (CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) );
 859
 860     size = cvGetSize(left);
 861     if( !state->left || state->left->width != size.width || state->left->height != size.height )
 862     {
 863         int pcn = (int)(sizeof(GCVtx*)/sizeof(int));
 864         int vcn = (int)(sizeof(GCVtx)/sizeof(int));
 865         int ecn = (int)(sizeof(GCEdge)/sizeof(int));
 866         cvReleaseMat( &state->left );
 867         cvReleaseMat( &state->right );
 868         cvReleaseMat( &state->ptrLeft );
 869         cvReleaseMat( &state->ptrRight );
 870         cvReleaseMat( &state->dispLeft );
 871         cvReleaseMat( &state->dispRight );
 872
 873         state->left = cvCreateMat( size.height, size.width, CV_8UC3 );
 874         state->right = cvCreateMat( size.height, size.width, CV_8UC3 );
 875         state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 );
 876         state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 );
 877         state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
 878         state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
 879         state->vtxBuf = cvCreateMat( 1, size.height*size.width*2, CV_32SC(vcn) );
 880         state->edgeBuf = cvCreateMat( 1, size.height*size.width*12 + 16, CV_32SC(ecn) );
 881     }
 882
 883     if( !useDisparityGuess )
 884     {
 885         cvSet( state->dispLeft, cvScalarAll(OCCLUDED));
 886         cvSet( state->dispRight, cvScalarAll(OCCLUDED));
 887     }
 888     else
 889     {
 890         CV_Assert( dispLeft && dispRight );
 891         cvConvert( dispLeft, state->dispLeft );
 892         cvConvert( dispRight, state->dispRight );
 893     }
 894
 895     state2.Ithreshold = state->Ithreshold;
 896     state2.interactionRadius = state->interactionRadius;
 897     state2.lambda = cvRound(state->lambda*DENOMINATOR);
 898     state2.lambda1 = cvRound(state->lambda1*DENOMINATOR);
 899     state2.lambda2 = cvRound(state->lambda2*DENOMINATOR);
 900     state2.K = cvRound(state->K*DENOMINATOR);
 901
 902     icvInitStereoConstTabs();
 903     icvInitGraySubpix( left, right, state->left, state->right );
 904
 905     std::vector<int> disp(state->numberOfDisparities);
 906     CvMat _disp = cvMat( 1, (int)disp.size(), CV_32S, &disp[0] );
 907     cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities );
 908     cvRandShuffle( &_disp, &rng );
 909
 910     if( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) )
 911     {
 912         float L = icvComputeK(state)*0.2f;
 913         state2.lambda = cvRound(L*DENOMINATOR);
 914     }
 915
 916     if( state2.K < 0 )
 917         state2.K = state2.lambda*5;
 918     if( state2.lambda1 < 0 )
 919         state2.lambda1 = state2.lambda*3;
 920     if( state2.lambda2 < 0 )
 921         state2.lambda2 = state2.lambda;
 922
 923     icvInitStereoTabs( &state2 );
 924
 925     E = icvComputeEnergy( state, &state2, !useDisparityGuess );
 926     for( iter = 0; iter < state->maxIters; iter++ )
 927     {
 928         for( i = 0; i < state->numberOfDisparities; i++ )
 929         {
 930             int alpha = disp[i];
 931             int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 );
 932             if( Enew < E )
 933             {
 934                 nZeroExpansions = 0;
 935                 E = Enew;
 936             }
 937             else if( ++nZeroExpansions >= state->numberOfDisparities )
 938                 break;
 939         }
 940     }
 941
 942     if( dispLeft )
 943         cvConvert( state->dispLeft, dispLeft );
 944     if( dispRight )
 945         cvConvert( state->dispRight, dispRight );
 946
 947     cvFree( &state2.orphans );
 948 }