| //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 ); |
| } |