diff options
Diffstat (limited to 'cv/src/cvstereogc.cpp')
| -rw-r--r-- | cv/src/cvstereogc.cpp | 960 |
1 files changed, 960 insertions, 0 deletions
diff --git a/cv/src/cvstereogc.cpp b/cv/src/cvstereogc.cpp new file mode 100644 index 0000000..0f6f806 --- /dev/null +++ b/cv/src/cvstereogc.cpp @@ -0,0 +1,960 @@ +//M*////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// Intel License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000, Intel Corporation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of Intel Corporation may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include "_cv.h" + +#undef INFINITY +#define INFINITY 10000 +#define OCCLUSION_PENALTY 10000 +#define OCCLUSION_PENALTY2 1000 +#define DENOMINATOR 16 +#undef OCCLUDED +#define OCCLUDED CV_STEREO_GC_OCCLUDED +#define CUTOFF 1000 +#define IS_BLOCKED(d1, d2) ((d1) > (d2)) + +typedef struct GCVtx +{ + GCVtx *next; + int parent; + int first; + int ts; + int dist; + short weight; + uchar t; +} +GCVtx; + +typedef struct GCEdge +{ + GCVtx* dst; + int next; + int weight; +} +GCEdge; + +typedef struct CvStereoGCState2 +{ + int Ithreshold, interactionRadius; + int lambda, lambda1, lambda2, K; + int dataCostFuncTab[CUTOFF+1]; + int smoothnessR[CUTOFF*2+1]; + int smoothnessGrayDiff[512]; + GCVtx** orphans; + int maxOrphans; +} +CvStereoGCState2; + +// truncTab[x+255] = MAX(x-255,0) +static uchar icvTruncTab[512]; +// cutoffSqrTab[x] = MIN(x*x, CUTOFF) +static int icvCutoffSqrTab[256]; + +static void icvInitStereoConstTabs() +{ + static volatile int initialized = 0; + if( !initialized ) + { + int i; + for( i = 0; i < 512; i++ ) + icvTruncTab[i] = (uchar)MIN(MAX(i-255,0),255); + for( i = 0; i < 256; i++ ) + icvCutoffSqrTab[i] = MIN(i*i, CUTOFF); + initialized = 1; + } +} + +static void icvInitStereoTabs( CvStereoGCState2* state2 ) +{ + int i, K = state2->K; + + for( i = 0; i <= CUTOFF; i++ ) + state2->dataCostFuncTab[i] = MIN(i*DENOMINATOR - K, 0); + + for( i = 0; i < CUTOFF*2 + 1; i++ ) + state2->smoothnessR[i] = MIN(abs(i-CUTOFF), state2->interactionRadius); + + for( i = 0; i < 512; i++ ) + { + int diff = abs(i - 255); + state2->smoothnessGrayDiff[i] = diff < state2->Ithreshold ? state2->lambda1 : state2->lambda2; + } +} + + +static int icvGCResizeOrphansBuf( GCVtx**& orphans, int norphans ) +{ + int i, newNOrphans = MAX(norphans*3/2, 256); + GCVtx** newOrphans = (GCVtx**)cvAlloc( newNOrphans*sizeof(orphans[0]) ); + for( i = 0; i < norphans; i++ ) + newOrphans[i] = orphans[i]; + cvFree( &orphans ); + orphans = newOrphans; + return newNOrphans; +} + +static int64 icvGCMaxFlow( GCVtx* vtx, int nvtx, GCEdge* edges, GCVtx**& _orphans, int& _maxOrphans ) +{ + const int TERMINAL = -1, ORPHAN = -2; + GCVtx stub, *nil = &stub, *first = nil, *last = nil; + int i, k; + int curr_ts = 0; + int64 flow = 0; + int norphans = 0, maxOrphans = _maxOrphans; + GCVtx** orphans = _orphans; + stub.next = nil; + + // initialize the active queue and the graph vertices + for( i = 0; i < nvtx; i++ ) + { + GCVtx* v = vtx + i; + v->ts = 0; + if( v->weight != 0 ) + { + last = last->next = v; + v->dist = 1; + v->parent = TERMINAL; + v->t = v->weight < 0; + } + else + v->parent = 0; + } + + first = first->next; + last->next = nil; + nil->next = 0; + + // run the search-path -> augment-graph -> restore-trees loop + for(;;) + { + GCVtx* v, *u; + int e0 = -1, ei = 0, ej = 0, min_weight, weight; + uchar vt; + + // grow S & T search trees, find an edge connecting them + while( first != nil ) + { + v = first; + if( v->parent ) + { + vt = v->t; + for( ei = v->first; ei != 0; ei = edges[ei].next ) + { + if( edges[ei^vt].weight == 0 ) + continue; + u = edges[ei].dst; + if( !u->parent ) + { + u->t = vt; + u->parent = ei ^ 1; + u->ts = v->ts; + u->dist = v->dist + 1; + if( !u->next ) + { + u->next = nil; + last = last->next = u; + } + continue; + } + + if( u->t != vt ) + { + e0 = ei ^ vt; + break; + } + + if( u->dist > v->dist+1 && u->ts <= v->ts ) + { + // reassign the parent + u->parent = ei ^ 1; + u->ts = v->ts; + u->dist = v->dist + 1; + } + } + if( e0 > 0 ) + break; + } + // exclude the vertex from the active list + first = first->next; + v->next = 0; + } + + if( e0 <= 0 ) + break; + + // find the minimum edge weight along the path + min_weight = edges[e0].weight; + assert( min_weight > 0 ); + // k = 1: source tree, k = 0: destination tree + for( k = 1; k >= 0; k-- ) + { + for( v = edges[e0^k].dst;; v = edges[ei].dst ) + { + if( (ei = v->parent) < 0 ) + break; + weight = edges[ei^k].weight; + min_weight = MIN(min_weight, weight); + assert( min_weight > 0 ); + } + weight = abs(v->weight); + min_weight = MIN(min_weight, weight); + assert( min_weight > 0 ); + } + + // modify weights of the edges along the path and collect orphans + edges[e0].weight -= min_weight; + edges[e0^1].weight += min_weight; + flow += min_weight; + + // k = 1: source tree, k = 0: destination tree + for( k = 1; k >= 0; k-- ) + { + for( v = edges[e0^k].dst;; v = edges[ei].dst ) + { + if( (ei = v->parent) < 0 ) + break; + edges[ei^(k^1)].weight += min_weight; + if( (edges[ei^k].weight -= min_weight) == 0 ) + { + if( norphans >= maxOrphans ) + maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); + orphans[norphans++] = v; + v->parent = ORPHAN; + } + } + + v->weight = (short)(v->weight + min_weight*(1-k*2)); + if( v->weight == 0 ) + { + if( norphans >= maxOrphans ) + maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); + orphans[norphans++] = v; + v->parent = ORPHAN; + } + } + + // restore the search trees by finding new parents for the orphans + curr_ts++; + while( norphans > 0 ) + { + GCVtx* v = orphans[--norphans]; + int d, min_dist = INT_MAX; + e0 = 0; + vt = v->t; + + for( ei = v->first; ei != 0; ei = edges[ei].next ) + { + if( edges[ei^(vt^1)].weight == 0 ) + continue; + u = edges[ei].dst; + if( u->t != vt || u->parent == 0 ) + continue; + // compute the distance to the tree root + for( d = 0;; ) + { + if( u->ts == curr_ts ) + { + d += u->dist; + break; + } + ej = u->parent; + d++; + if( ej < 0 ) + { + if( ej == ORPHAN ) + d = INT_MAX-1; + else + { + u->ts = curr_ts; + u->dist = 1; + } + break; + } + u = edges[ej].dst; + } + + // update the distance + if( ++d < INT_MAX ) + { + if( d < min_dist ) + { + min_dist = d; + e0 = ei; + } + for( u = edges[ei].dst; u->ts != curr_ts; u = edges[u->parent].dst ) + { + u->ts = curr_ts; + u->dist = --d; + } + } + } + + if( (v->parent = e0) > 0 ) + { + v->ts = curr_ts; + v->dist = min_dist; + continue; + } + + /* no parent is found */ + v->ts = 0; + for( ei = v->first; ei != 0; ei = edges[ei].next ) + { + u = edges[ei].dst; + ej = u->parent; + if( u->t != vt || !ej ) + continue; + if( edges[ei^(vt^1)].weight && !u->next ) + { + u->next = nil; + last = last->next = u; + } + if( ej > 0 && edges[ej].dst == v ) + { + if( norphans >= maxOrphans ) + maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); + orphans[norphans++] = u; + u->parent = ORPHAN; + } + } + } + } + + _orphans = orphans; + _maxOrphans = maxOrphans; + + return flow; +} + + +CvStereoGCState* cvCreateStereoGCState( int numberOfDisparities, int maxIters ) +{ + CvStereoGCState* state = 0; + + //CV_FUNCNAME("cvCreateStereoGCState"); + + __BEGIN__; + + state = (CvStereoGCState*)cvAlloc( sizeof(*state) ); + memset( state, 0, sizeof(*state) ); + state->minDisparity = 0; + state->numberOfDisparities = numberOfDisparities; + state->maxIters = maxIters <= 0 ? 3 : maxIters; + state->Ithreshold = 5; + state->interactionRadius = 1; + state->K = state->lambda = state->lambda1 = state->lambda2 = -1.f; + state->occlusionCost = OCCLUSION_PENALTY; + + __END__; + + return state; +} + +void cvReleaseStereoGCState( CvStereoGCState** _state ) +{ + CvStereoGCState* state; + + if( !_state && !*_state ) + return; + + state = *_state; + cvReleaseMat( &state->left ); + cvReleaseMat( &state->right ); + cvReleaseMat( &state->ptrLeft ); + cvReleaseMat( &state->ptrRight ); + cvReleaseMat( &state->vtxBuf ); + cvReleaseMat( &state->edgeBuf ); + cvFree( _state ); +} + +// ||I(x) - J(x')|| = +// min(CUTOFF, +// min( +// max( +// max(minJ(x') - I(x), 0), +// max(I(x) - maxJ(x'), 0)), +// max( +// max(minI(x) - J(x'), 0), +// max(J(x') - maxI(x), 0)))**2) == +// min(CUTOFF, +// min( +// max(minJ(x') - I(x), 0) + +// max(I(x) - maxJ(x'), 0), +// +// max(minI(x) - J(x'), 0) + +// max(J(x') - maxI(x), 0)))**2) +// where (I, minI, maxI) and +// (J, minJ, maxJ) are stored as interleaved 3-channel images. +// minI, maxI are computed from I, +// minJ, maxJ are computed from J - see icvInitGraySubPix. +static inline int icvDataCostFuncGraySubpix( const uchar* a, const uchar* b ) +{ + int va = a[0], vb = b[0]; + int da = icvTruncTab[b[1] - va + 255] + icvTruncTab[va - b[2] + 255]; + int db = icvTruncTab[a[1] - vb + 255] + icvTruncTab[vb - a[2] + 255]; + return icvCutoffSqrTab[MIN(da,db)]; +} + +static inline int icvSmoothnessCostFunc( int da, int db, int maxR, const int* stabR, int scale ) +{ + return da == db ? 0 : (da == OCCLUDED || db == OCCLUDED ? maxR : stabR[da - db])*scale; +} + +static void icvInitGraySubpix( const CvMat* left, const CvMat* right, + CvMat* left3, CvMat* right3 ) +{ + int k, x, y, rows = left->rows, cols = left->cols; + + for( k = 0; k < 2; k++ ) + { + const CvMat* src = k == 0 ? left : right; + CvMat* dst = k == 0 ? left3 : right3; + int sstep = src->step; + + for( y = 0; y < rows; y++ ) + { + const uchar* sptr = src->data.ptr + sstep*y; + const uchar* sptr_prev = y > 0 ? sptr - sstep : sptr; + const uchar* sptr_next = y < rows-1 ? sptr + sstep : sptr; + uchar* dptr = dst->data.ptr + dst->step*y; + int v_prev = sptr[0]; + + for( x = 0; x < cols; x++, dptr += 3 ) + { + int v = sptr[x], v1, minv = v, maxv = v; + + v1 = (v + v_prev)/2; + minv = MIN(minv, v1); maxv = MAX(maxv, v1); + v1 = (v + sptr_prev[x])/2; + minv = MIN(minv, v1); maxv = MAX(maxv, v1); + v1 = (v + sptr_next[x])/2; + minv = MIN(minv, v1); maxv = MAX(maxv, v1); + if( x < cols-1 ) + { + v1 = (v + sptr[x+1])/2; + minv = MIN(minv, v1); maxv = MAX(maxv, v1); + } + v_prev = v; + dptr[0] = (uchar)v; + dptr[1] = (uchar)minv; + dptr[2] = (uchar)maxv; + } + } + } +} + +// Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)), +// where k = number_of_disparities*0.25. +static float +icvComputeK( CvStereoGCState* state ) +{ + int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0; + int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd; + int k = MIN(MAX((nd + 2)/4, 3), nd); + int *arr = (int*)cvStackAlloc(k*sizeof(arr[0])), delta, t, sum = 0; + + for( y = 0; y < rows; y++ ) + { + const uchar* lptr = state->left->data.ptr + state->left->step*y; + const uchar* rptr = state->right->data.ptr + state->right->step*y; + + for( x = 0; x < cols; x++ ) + { + for( d = maxd-1, i = 0; d >= mind; d-- ) + { + x1 = x - d; + if( (unsigned)x1 >= (unsigned)cols ) + continue; + delta = icvDataCostFuncGraySubpix( lptr + x*3, rptr + x1*3 ); + if( i < k ) + arr[i++] = delta; + else + for( i = 0; i < k; i++ ) + if( delta < arr[i] ) + CV_SWAP( arr[i], delta, t ); + } + delta = arr[0]; + for( j = 1; j < i; j++ ) + delta = MAX(delta, arr[j]); + sum += delta; + n++; + } + } + + return (float)sum/n; +} + +static int64 icvComputeEnergy( const CvStereoGCState* state, const CvStereoGCState2* state2, + bool allOccluded ) +{ + int x, y, rows = state->left->rows, cols = state->left->cols; + int64 E = 0; + const int* dtab = state2->dataCostFuncTab; + int maxR = state2->interactionRadius; + const int* stabR = state2->smoothnessR + CUTOFF; + const int* stabI = state2->smoothnessGrayDiff + 255; + const uchar* left = state->left->data.ptr; + const uchar* right = state->right->data.ptr; + short* dleft = state->dispLeft->data.s; + short* dright = state->dispRight->data.s; + int step = state->left->step; + int dstep = (int)(state->dispLeft->step/sizeof(short)); + + assert( state->left->step == state->right->step && + state->dispLeft->step == state->dispRight->step ); + + if( allOccluded ) + return (int64)OCCLUSION_PENALTY*rows*cols*2; + + for( y = 0; y < rows; y++, left += step, right += step, dleft += dstep, dright += dstep ) + { + for( x = 0; x < cols; x++ ) + { + int d = dleft[x], x1, d1; + if( d == OCCLUDED ) + E += OCCLUSION_PENALTY; + else + { + x1 = x + d; + if( (unsigned)x1 >= (unsigned)cols ) + continue; + d1 = dright[x1]; + if( d == -d1 ) + E += dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )]; + } + + if( x < cols-1 ) + { + d1 = dleft[x+1]; + E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+3]] ); + } + if( y < rows-1 ) + { + d1 = dleft[x+dstep]; + E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+step]] ); + } + + d = dright[x]; + if( d == OCCLUDED ) + E += OCCLUSION_PENALTY; + + if( x < cols-1 ) + { + d1 = dright[x+1]; + E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+3]] ); + } + if( y < rows-1 ) + { + d1 = dright[x+dstep]; + E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+step]] ); + } + assert( E >= 0 ); + } + } + + return E; +} + +static inline void icvAddEdge( GCVtx *x, GCVtx* y, GCEdge* edgeBuf, int nedges, int w, int rw ) +{ + GCEdge *xy = edgeBuf + nedges, *yx = xy + 1; + + assert( x != 0 && y != 0 ); + xy->dst = y; + xy->next = x->first; + xy->weight = (short)w; + x->first = nedges; + + yx->dst = x; + yx->next = y->first; + yx->weight = (short)rw; + y->first = nedges+1; +} + +static inline int icvAddTWeights( GCVtx* vtx, int sourceWeight, int sinkWeight ) +{ + int w = vtx->weight; + if( w > 0 ) + sourceWeight += w; + else + sinkWeight -= w; + vtx->weight = (short)(sourceWeight - sinkWeight); + return MIN(sourceWeight, sinkWeight); +} + +static inline int icvAddTerm( GCVtx* x, GCVtx* y, int A, int B, int C, int D, + GCEdge* edgeBuf, int& nedges ) +{ + int dE = 0, w; + + assert(B - A + C - D >= 0); + if( B < A ) + { + dE += icvAddTWeights(x, D, B); + dE += icvAddTWeights(y, 0, A - B); + if( (w = B - A + C - D) != 0 ) + { + icvAddEdge( x, y, edgeBuf, nedges, 0, w ); + nedges += 2; + } + } + else if( C < D ) + { + dE += icvAddTWeights(x, D, A + D - C); + dE += icvAddTWeights(y, 0, C - D); + if( (w = B - A + C - D) != 0 ) + { + icvAddEdge( x, y, edgeBuf, nedges, w, 0 ); + nedges += 2; + } + } + else + { + dE += icvAddTWeights(x, D, A); + if( B != A || C != D ) + { + icvAddEdge( x, y, edgeBuf, nedges, B - A, C - D ); + nedges += 2; + } + } + return dE; +} + +static int64 icvAlphaExpand( int64 Eprev, int alpha, CvStereoGCState* state, CvStereoGCState2* state2 ) +{ + GCVtx *var, *var1; + int64 E = 0; + int delta, E00=0, E0a=0, Ea0=0, Eaa=0; + int k, a, d, d1, x, y, x1, y1, rows = state->left->rows, cols = state->left->cols; + int nvtx = 0, nedges = 2; + GCVtx* vbuf = (GCVtx*)state->vtxBuf->data.ptr; + GCEdge* ebuf = (GCEdge*)state->edgeBuf->data.ptr; + int maxR = state2->interactionRadius; + const int* dtab = state2->dataCostFuncTab; + const int* stabR = state2->smoothnessR + CUTOFF; + const int* stabI = state2->smoothnessGrayDiff + 255; + const uchar* left0 = state->left->data.ptr; + const uchar* right0 = state->right->data.ptr; + short* dleft0 = state->dispLeft->data.s; + short* dright0 = state->dispRight->data.s; + GCVtx** pleft0 = (GCVtx**)state->ptrLeft->data.ptr; + GCVtx** pright0 = (GCVtx**)state->ptrRight->data.ptr; + int step = state->left->step; + int dstep = (int)(state->dispLeft->step/sizeof(short)); + int pstep = (int)(state->ptrLeft->step/sizeof(GCVtx*)); + int aa[] = { alpha, -alpha }; + + double t = (double)cvGetTickCount(); + + assert( state->left->step == state->right->step && + state->dispLeft->step == state->dispRight->step && + state->ptrLeft->step == state->ptrRight->step ); + for( k = 0; k < 2; k++ ) + { + ebuf[k].dst = 0; + ebuf[k].next = 0; + ebuf[k].weight = 0; + } + + for( y = 0; y < rows; y++ ) + { + const uchar* left = left0 + step*y; + const uchar* right = right0 + step*y; + const short* dleft = dleft0 + dstep*y; + const short* dright = dright0 + dstep*y; + GCVtx** pleft = pleft0 + pstep*y; + GCVtx** pright = pright0 + pstep*y; + const uchar* lr[] = { left, right }; + const short* dlr[] = { dleft, dright }; + GCVtx** plr[] = { pleft, pright }; + + for( k = 0; k < 2; k++ ) + { + a = aa[k]; + for( y1 = y+(y>0); y1 <= y+(y<rows-1); y1++ ) + { + const short* disp = (k == 0 ? dleft0 : dright0) + y1*dstep; + GCVtx** ptr = (k == 0 ? pleft0 : pright0) + y1*pstep; + for( x = 0; x < cols; x++ ) + { + GCVtx* v = ptr[x] = &vbuf[nvtx++]; + v->first = 0; + v->weight = disp[x] == (short)(OCCLUDED ? -OCCLUSION_PENALTY2 : 0); + } + } + } + + for( x = 0; x < cols; x++ ) + { + d = dleft[x]; + x1 = x + d; + var = pleft[x]; + + // (left + x, right + x + d) + if( d != alpha && d != OCCLUDED && (unsigned)x1 < (unsigned)cols ) + { + var1 = pright[x1]; + d1 = dright[x1]; + if( d == -d1 ) + { + assert( var1 != 0 ); + delta = IS_BLOCKED(alpha, d) ? INFINITY : 0; + //add inter edge + E += icvAddTerm( var, var1, + dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )], + delta, delta, 0, ebuf, nedges ); + } + else if( IS_BLOCKED(alpha, d) ) + E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges ); + } + + // (left + x, right + x + alpha) + x1 = x + alpha; + if( (unsigned)x1 < (unsigned)cols ) + { + var1 = pright[x1]; + d1 = dright[x1]; + + E0a = IS_BLOCKED(d, alpha) ? INFINITY : 0; + Ea0 = IS_BLOCKED(-d1, alpha) ? INFINITY : 0; + Eaa = dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )]; + E += icvAddTerm( var, var1, 0, E0a, Ea0, Eaa, ebuf, nedges ); + } + + // smoothness + for( k = 0; k < 2; k++ ) + { + GCVtx** p = plr[k]; + const short* disp = dlr[k]; + const uchar* img = lr[k] + x*3; + int scale; + var = p[x]; + d = disp[x]; + a = aa[k]; + + if( x < cols - 1 ) + { + var1 = p[x+1]; + d1 = disp[x+1]; + scale = stabI[img[0] - img[3]]; + E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale ); + Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale ); + E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale ); + E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges ); + } + + if( y < rows - 1 ) + { + var1 = p[x+pstep]; + d1 = disp[x+dstep]; + scale = stabI[img[0] - img[step]]; + E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale ); + Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale ); + E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale ); + E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges ); + } + } + + // visibility term + if( d != OCCLUDED && IS_BLOCKED(alpha, -d)) + { + x1 = x + d; + if( (unsigned)x1 < (unsigned)cols ) + { + if( d != -dleft[x1] ) + { + var1 = pleft[x1]; + E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges ); + } + } + } + } + } + + t = (double)cvGetTickCount() - t; + ebuf[0].weight = ebuf[1].weight = 0; + E += icvGCMaxFlow( vbuf, nvtx, ebuf, state2->orphans, state2->maxOrphans ); + + if( E < Eprev ) + { + for( y = 0; y < rows; y++ ) + { + short* dleft = dleft0 + dstep*y; + short* dright = dright0 + dstep*y; + GCVtx** pleft = pleft0 + pstep*y; + GCVtx** pright = pright0 + pstep*y; + for( x = 0; x < cols; x++ ) + { + GCVtx* var = pleft[x]; + if( var && var->parent && var->t ) + dleft[x] = (short)alpha; + + var = pright[x]; + if( var && var->parent && var->t ) + dright[x] = (short)-alpha; + } + } + } + + return MIN(E, Eprev); +} + + +CV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right, + CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess ) +{ + CvStereoGCState2 state2; + state2.orphans = 0; + state2.maxOrphans = 0; + + CV_FUNCNAME( "cvFindStereoCorrespondenceGC" ); + + __BEGIN__; + + CvMat lstub, *left = cvGetMat( _left, &lstub ); + CvMat rstub, *right = cvGetMat( _right, &rstub ); + CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub ); + CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub ); + CvSize size; + int iter, i, nZeroExpansions = 0; + CvRNG rng = cvRNG(-1); + int* disp; + CvMat _disp; + int64 E; + + CV_ASSERT( state != 0 ); + CV_ASSERT( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) && + CV_MAT_TYPE(left->type) == CV_8UC1 ); + CV_ASSERT( !dispLeft || + (CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) ); + CV_ASSERT( !dispRight || + (CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) ); + + size = cvGetSize(left); + if( !state->left || state->left->width != size.width || state->left->height != size.height ) + { + int pcn = (int)(sizeof(GCVtx*)/sizeof(int)); + int vcn = (int)(sizeof(GCVtx)/sizeof(int)); + int ecn = (int)(sizeof(GCEdge)/sizeof(int)); + cvReleaseMat( &state->left ); + cvReleaseMat( &state->right ); + cvReleaseMat( &state->ptrLeft ); + cvReleaseMat( &state->ptrRight ); + cvReleaseMat( &state->dispLeft ); + cvReleaseMat( &state->dispRight ); + + state->left = cvCreateMat( size.height, size.width, CV_8UC3 ); + state->right = cvCreateMat( size.height, size.width, CV_8UC3 ); + state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 ); + state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 ); + state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) ); + state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) ); + state->vtxBuf = cvCreateMat( 1, size.height*size.width*2, CV_32SC(vcn) ); + state->edgeBuf = cvCreateMat( 1, size.height*size.width*12 + 16, CV_32SC(ecn) ); + } + + if( !useDisparityGuess ) + { + cvSet( state->dispLeft, cvScalarAll(OCCLUDED)); + cvSet( state->dispRight, cvScalarAll(OCCLUDED)); + } + else + { + CV_ASSERT( dispLeft && dispRight ); + cvConvert( dispLeft, state->dispLeft ); + cvConvert( dispRight, state->dispRight ); + } + + state2.Ithreshold = state->Ithreshold; + state2.interactionRadius = state->interactionRadius; + state2.lambda = cvRound(state->lambda*DENOMINATOR); + state2.lambda1 = cvRound(state->lambda1*DENOMINATOR); + state2.lambda2 = cvRound(state->lambda2*DENOMINATOR); + state2.K = cvRound(state->K*DENOMINATOR); + + icvInitStereoConstTabs(); + icvInitGraySubpix( left, right, state->left, state->right ); + disp = (int*)cvStackAlloc( state->numberOfDisparities*sizeof(disp[0]) ); + _disp = cvMat( 1, state->numberOfDisparities, CV_32S, disp ); + cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities ); + cvRandShuffle( &_disp, &rng ); + + if( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) ) + { + float L = icvComputeK(state)*0.2f; + state2.lambda = cvRound(L*DENOMINATOR); + } + + if( state2.K < 0 ) + state2.K = state2.lambda*5; + if( state2.lambda1 < 0 ) + state2.lambda1 = state2.lambda*3; + if( state2.lambda2 < 0 ) + state2.lambda2 = state2.lambda; + + icvInitStereoTabs( &state2 ); + + E = icvComputeEnergy( state, &state2, !useDisparityGuess ); + for( iter = 0; iter < state->maxIters; iter++ ) + { + for( i = 0; i < state->numberOfDisparities; i++ ) + { + int alpha = disp[i]; + int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 ); + if( Enew < E ) + { + nZeroExpansions = 0; + E = Enew; + } + else if( ++nZeroExpansions >= state->numberOfDisparities ) + break; + } + } + + if( dispLeft ) + cvConvert( state->dispLeft, dispLeft ); + if( dispRight ) + cvConvert( state->dispRight, dispRight ); + + __END__; + + cvFree( &state2.orphans ); +} |