cvstereogc.cpp revision 6acb9a7ea3d7564944e12cbc73a857b88c1301ee
16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#undef INFINITY 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define INFINITY 10000 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define OCCLUSION_PENALTY 10000 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define OCCLUSION_PENALTY2 1000 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define DENOMINATOR 16 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#undef OCCLUDED 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define OCCLUDED CV_STEREO_GC_OCCLUDED 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define CUTOFF 1000 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define IS_BLOCKED(d1, d2) ((d1) > (d2)) 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef struct GCVtx 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx *next; 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int parent; 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int first; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ts; 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dist; 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short weight; 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar t; 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennGCVtx; 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef struct GCEdge 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx* dst; 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int next; 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int weight; 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennGCEdge; 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef struct CvStereoGCState2 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Ithreshold, interactionRadius; 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int lambda, lambda1, lambda2, K; 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dataCostFuncTab[CUTOFF+1]; 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int smoothnessR[CUTOFF*2+1]; 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int smoothnessGrayDiff[512]; 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** orphans; 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxOrphans; 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvStereoGCState2; 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// truncTab[x+255] = MAX(x-255,0) 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic uchar icvTruncTab[512]; 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// cutoffSqrTab[x] = MIN(x*x, CUTOFF) 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int icvCutoffSqrTab[256]; 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void icvInitStereoConstTabs() 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static volatile int initialized = 0; 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !initialized ) 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 512; i++ ) 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTruncTab[i] = (uchar)MIN(MAX(i-255,0),255); 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 256; i++ ) 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCutoffSqrTab[i] = MIN(i*i, CUTOFF); 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn initialized = 1; 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void icvInitStereoTabs( CvStereoGCState2* state2 ) 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, K = state2->K; 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i <= CUTOFF; i++ ) 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2->dataCostFuncTab[i] = MIN(i*DENOMINATOR - K, 0); 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < CUTOFF*2 + 1; i++ ) 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2->smoothnessR[i] = MIN(abs(i-CUTOFF), state2->interactionRadius); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 512; i++ ) 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int diff = abs(i - 255); 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2->smoothnessGrayDiff[i] = diff < state2->Ithreshold ? state2->lambda1 : state2->lambda2; 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int icvGCResizeOrphansBuf( GCVtx**& orphans, int norphans ) 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, newNOrphans = MAX(norphans*3/2, 256); 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** newOrphans = (GCVtx**)cvAlloc( newNOrphans*sizeof(orphans[0]) ); 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < norphans; i++ ) 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newOrphans[i] = orphans[i]; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &orphans ); 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn orphans = newOrphans; 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return newNOrphans; 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int64 icvGCMaxFlow( GCVtx* vtx, int nvtx, GCEdge* edges, GCVtx**& _orphans, int& _maxOrphans ) 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int TERMINAL = -1, ORPHAN = -2; 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx stub, *nil = &stub, *first = nil, *last = nil; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, k; 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int curr_ts = 0; 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int64 flow = 0; 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int norphans = 0, maxOrphans = _maxOrphans; 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** orphans = _orphans; 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stub.next = nil; 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize the active queue and the graph vertices 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nvtx; i++ ) 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx* v = vtx + i; 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->ts = 0; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( v->weight != 0 ) 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn last = last->next = v; 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->dist = 1; 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->parent = TERMINAL; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->t = v->weight < 0; 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->parent = 0; 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn first = first->next; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn last->next = nil; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nil->next = 0; 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // run the search-path -> augment-graph -> restore-trees loop 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(;;) 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx* v, *u; 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int e0 = -1, ei = 0, ej = 0, min_weight, weight; 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar vt; 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // grow S & T search trees, find an edge connecting them 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( first != nil ) 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = first; 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( v->parent ) 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vt = v->t; 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ei = v->first; ei != 0; ei = edges[ei].next ) 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( edges[ei^vt].weight == 0 ) 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u = edges[ei].dst; 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !u->parent ) 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->t = vt; 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->parent = ei ^ 1; 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->ts = v->ts; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->dist = v->dist + 1; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !u->next ) 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->next = nil; 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn last = last->next = u; 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( u->t != vt ) 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e0 = ei ^ vt; 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( u->dist > v->dist+1 && u->ts <= v->ts ) 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // reassign the parent 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->parent = ei ^ 1; 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->ts = v->ts; 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->dist = v->dist + 1; 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( e0 > 0 ) 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // exclude the vertex from the active list 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn first = first->next; 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->next = 0; 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( e0 <= 0 ) 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // find the minimum edge weight along the path 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_weight = edges[e0].weight; 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( min_weight > 0 ); 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // k = 1: source tree, k = 0: destination tree 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 1; k >= 0; k-- ) 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( v = edges[e0^k].dst;; v = edges[ei].dst ) 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (ei = v->parent) < 0 ) 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn weight = edges[ei^k].weight; 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_weight = MIN(min_weight, weight); 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( min_weight > 0 ); 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn weight = abs(v->weight); 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_weight = MIN(min_weight, weight); 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( min_weight > 0 ); 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // modify weights of the edges along the path and collect orphans 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[e0].weight -= min_weight; 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[e0^1].weight += min_weight; 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn flow += min_weight; 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // k = 1: source tree, k = 0: destination tree 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 1; k >= 0; k-- ) 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( v = edges[e0^k].dst;; v = edges[ei].dst ) 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (ei = v->parent) < 0 ) 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[ei^(k^1)].weight += min_weight; 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (edges[ei^k].weight -= min_weight) == 0 ) 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( norphans >= maxOrphans ) 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn orphans[norphans++] = v; 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->parent = ORPHAN; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->weight = (short)(v->weight + min_weight*(1-k*2)); 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( v->weight == 0 ) 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( norphans >= maxOrphans ) 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn orphans[norphans++] = v; 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->parent = ORPHAN; 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // restore the search trees by finding new parents for the orphans 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn curr_ts++; 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( norphans > 0 ) 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx* v = orphans[--norphans]; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int d, min_dist = INT_MAX; 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e0 = 0; 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vt = v->t; 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ei = v->first; ei != 0; ei = edges[ei].next ) 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( edges[ei^(vt^1)].weight == 0 ) 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u = edges[ei].dst; 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( u->t != vt || u->parent == 0 ) 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // compute the distance to the tree root 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0;; ) 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( u->ts == curr_ts ) 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d += u->dist; 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ej = u->parent; 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d++; 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ej < 0 ) 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ej == ORPHAN ) 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = INT_MAX-1; 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->ts = curr_ts; 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->dist = 1; 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u = edges[ej].dst; 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // update the distance 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ++d < INT_MAX ) 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d < min_dist ) 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_dist = d; 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e0 = ei; 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( u = edges[ei].dst; u->ts != curr_ts; u = edges[u->parent].dst ) 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->ts = curr_ts; 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->dist = --d; 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (v->parent = e0) > 0 ) 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->ts = curr_ts; 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->dist = min_dist; 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* no parent is found */ 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->ts = 0; 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ei = v->first; ei != 0; ei = edges[ei].next ) 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u = edges[ei].dst; 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ej = u->parent; 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( u->t != vt || !ej ) 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( edges[ei^(vt^1)].weight && !u->next ) 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->next = nil; 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn last = last->next = u; 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ej > 0 && edges[ej].dst == v ) 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( norphans >= maxOrphans ) 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxOrphans = icvGCResizeOrphansBuf( orphans, norphans ); 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn orphans[norphans++] = u; 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u->parent = ORPHAN; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _orphans = orphans; 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _maxOrphans = maxOrphans; 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return flow; 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvStereoGCState* cvCreateStereoGCState( int numberOfDisparities, int maxIters ) 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoGCState* state = 0; 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //CV_FUNCNAME("cvCreateStereoGCState"); 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = (CvStereoGCState*)cvAlloc( sizeof(*state) ); 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( state, 0, sizeof(*state) ); 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->minDisparity = 0; 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->numberOfDisparities = numberOfDisparities; 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->maxIters = maxIters <= 0 ? 3 : maxIters; 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->Ithreshold = 5; 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->interactionRadius = 1; 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->K = state->lambda = state->lambda1 = state->lambda2 = -1.f; 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->occlusionCost = OCCLUSION_PENALTY; 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return state; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvReleaseStereoGCState( CvStereoGCState** _state ) 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoGCState* state; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !_state && !*_state ) 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = *_state; 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->left ); 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->right ); 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->ptrLeft ); 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->ptrRight ); 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->vtxBuf ); 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->edgeBuf ); 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( _state ); 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// ||I(x) - J(x')|| = 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// min(CUTOFF, 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// min( 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max( 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max(minJ(x') - I(x), 0), 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max(I(x) - maxJ(x'), 0)), 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max( 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max(minI(x) - J(x'), 0), 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max(J(x') - maxI(x), 0)))**2) == 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// min(CUTOFF, 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// min( 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max(minJ(x') - I(x), 0) + 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max(I(x) - maxJ(x'), 0), 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max(minI(x) - J(x'), 0) + 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// max(J(x') - maxI(x), 0)))**2) 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// where (I, minI, maxI) and 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (J, minJ, maxJ) are stored as interleaved 3-channel images. 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// minI, maxI are computed from I, 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// minJ, maxJ are computed from J - see icvInitGraySubPix. 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic inline int icvDataCostFuncGraySubpix( const uchar* a, const uchar* b ) 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int va = a[0], vb = b[0]; 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int da = icvTruncTab[b[1] - va + 255] + icvTruncTab[va - b[2] + 255]; 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int db = icvTruncTab[a[1] - vb + 255] + icvTruncTab[vb - a[2] + 255]; 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return icvCutoffSqrTab[MIN(da,db)]; 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic inline int icvSmoothnessCostFunc( int da, int db, int maxR, const int* stabR, int scale ) 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return da == db ? 0 : (da == OCCLUDED || db == OCCLUDED ? maxR : stabR[da - db])*scale; 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void icvInitGraySubpix( const CvMat* left, const CvMat* right, 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* left3, CvMat* right3 ) 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int k, x, y, rows = left->rows, cols = left->cols; 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* src = k == 0 ? left : right; 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* dst = k == 0 ? left3 : right3; 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sstep = src->step; 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < rows; y++ ) 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* sptr = src->data.ptr + sstep*y; 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* sptr_prev = y > 0 ? sptr - sstep : sptr; 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* sptr_next = y < rows-1 ? sptr + sstep : sptr; 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dptr = dst->data.ptr + dst->step*y; 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int v_prev = sptr[0]; 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols; x++, dptr += 3 ) 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int v = sptr[x], v1, minv = v, maxv = v; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v1 = (v + v_prev)/2; 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minv = MIN(minv, v1); maxv = MAX(maxv, v1); 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v1 = (v + sptr_prev[x])/2; 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minv = MIN(minv, v1); maxv = MAX(maxv, v1); 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v1 = (v + sptr_next[x])/2; 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minv = MIN(minv, v1); maxv = MAX(maxv, v1); 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < cols-1 ) 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v1 = (v + sptr[x+1])/2; 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minv = MIN(minv, v1); maxv = MAX(maxv, v1); 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v_prev = v; 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[0] = (uchar)v; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[1] = (uchar)minv; 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[2] = (uchar)maxv; 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)), 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// where k = number_of_disparities*0.25. 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic float 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvComputeK( CvStereoGCState* state ) 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0; 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd; 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int k = MIN(MAX((nd + 2)/4, 3), nd); 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *arr = (int*)cvStackAlloc(k*sizeof(arr[0])), delta, t, sum = 0; 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < rows; y++ ) 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* lptr = state->left->data.ptr + state->left->step*y; 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* rptr = state->right->data.ptr + state->right->step*y; 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols; x++ ) 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = maxd-1, i = 0; d >= mind; d-- ) 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = x - d; 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (unsigned)x1 >= (unsigned)cols ) 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delta = icvDataCostFuncGraySubpix( lptr + x*3, rptr + x1*3 ); 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( i < k ) 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr[i++] = delta; 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < k; i++ ) 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( delta < arr[i] ) 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( arr[i], delta, t ); 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delta = arr[0]; 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 1; j < i; j++ ) 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delta = MAX(delta, arr[j]); 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += delta; 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n++; 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return (float)sum/n; 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int64 icvComputeEnergy( const CvStereoGCState* state, const CvStereoGCState2* state2, 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool allOccluded ) 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y, rows = state->left->rows, cols = state->left->cols; 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int64 E = 0; 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int* dtab = state2->dataCostFuncTab; 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxR = state2->interactionRadius; 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int* stabR = state2->smoothnessR + CUTOFF; 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int* stabI = state2->smoothnessGrayDiff + 255; 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* left = state->left->data.ptr; 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* right = state->right->data.ptr; 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short* dleft = state->dispLeft->data.s; 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short* dright = state->dispRight->data.s; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = state->left->step; 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dstep = (int)(state->dispLeft->step/sizeof(short)); 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( state->left->step == state->right->step && 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->dispLeft->step == state->dispRight->step ); 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( allOccluded ) 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return (int64)OCCLUSION_PENALTY*rows*cols*2; 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < rows; y++, left += step, right += step, dleft += dstep, dright += dstep ) 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols; x++ ) 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int d = dleft[x], x1, d1; 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d == OCCLUDED ) 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += OCCLUSION_PENALTY; 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = x + d; 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (unsigned)x1 >= (unsigned)cols ) 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = dright[x1]; 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d == -d1 ) 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )]; 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < cols-1 ) 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = dleft[x+1]; 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+3]] ); 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < rows-1 ) 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = dleft[x+dstep]; 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+step]] ); 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = dright[x]; 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d == OCCLUDED ) 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += OCCLUSION_PENALTY; 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < cols-1 ) 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = dright[x+1]; 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+3]] ); 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < rows-1 ) 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = dright[x+dstep]; 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+step]] ); 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( E >= 0 ); 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return E; 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic inline void icvAddEdge( GCVtx *x, GCVtx* y, GCEdge* edgeBuf, int nedges, int w, int rw ) 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCEdge *xy = edgeBuf + nedges, *yx = xy + 1; 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( x != 0 && y != 0 ); 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy->dst = y; 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy->next = x->first; 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy->weight = (short)w; 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x->first = nedges; 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yx->dst = x; 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yx->next = y->first; 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yx->weight = (short)rw; 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y->first = nedges+1; 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic inline int icvAddTWeights( GCVtx* vtx, int sourceWeight, int sinkWeight ) 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int w = vtx->weight; 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( w > 0 ) 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sourceWeight += w; 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sinkWeight -= w; 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vtx->weight = (short)(sourceWeight - sinkWeight); 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return MIN(sourceWeight, sinkWeight); 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic inline int icvAddTerm( GCVtx* x, GCVtx* y, int A, int B, int C, int D, 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCEdge* edgeBuf, int& nedges ) 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dE = 0, w; 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(B - A + C - D >= 0); 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( B < A ) 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dE += icvAddTWeights(x, D, B); 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dE += icvAddTWeights(y, 0, A - B); 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (w = B - A + C - D) != 0 ) 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvAddEdge( x, y, edgeBuf, nedges, 0, w ); 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nedges += 2; 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( C < D ) 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dE += icvAddTWeights(x, D, A + D - C); 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dE += icvAddTWeights(y, 0, C - D); 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (w = B - A + C - D) != 0 ) 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvAddEdge( x, y, edgeBuf, nedges, w, 0 ); 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nedges += 2; 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dE += icvAddTWeights(x, D, A); 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( B != A || C != D ) 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvAddEdge( x, y, edgeBuf, nedges, B - A, C - D ); 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nedges += 2; 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return dE; 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int64 icvAlphaExpand( int64 Eprev, int alpha, CvStereoGCState* state, CvStereoGCState2* state2 ) 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx *var, *var1; 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int64 E = 0; 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int delta, E00=0, E0a=0, Ea0=0, Eaa=0; 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int k, a, d, d1, x, y, x1, y1, rows = state->left->rows, cols = state->left->cols; 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int nvtx = 0, nedges = 2; 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx* vbuf = (GCVtx*)state->vtxBuf->data.ptr; 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCEdge* ebuf = (GCEdge*)state->edgeBuf->data.ptr; 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxR = state2->interactionRadius; 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int* dtab = state2->dataCostFuncTab; 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int* stabR = state2->smoothnessR + CUTOFF; 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int* stabI = state2->smoothnessGrayDiff + 255; 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* left0 = state->left->data.ptr; 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* right0 = state->right->data.ptr; 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short* dleft0 = state->dispLeft->data.s; 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short* dright0 = state->dispRight->data.s; 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** pleft0 = (GCVtx**)state->ptrLeft->data.ptr; 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** pright0 = (GCVtx**)state->ptrRight->data.ptr; 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = state->left->step; 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dstep = (int)(state->dispLeft->step/sizeof(short)); 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int pstep = (int)(state->ptrLeft->step/sizeof(GCVtx*)); 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int aa[] = { alpha, -alpha }; 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = (double)cvGetTickCount(); 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( state->left->step == state->right->step && 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->dispLeft->step == state->dispRight->step && 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->ptrLeft->step == state->ptrRight->step ); 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ebuf[k].dst = 0; 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ebuf[k].next = 0; 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ebuf[k].weight = 0; 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < rows; y++ ) 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* left = left0 + step*y; 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* right = right0 + step*y; 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const short* dleft = dleft0 + dstep*y; 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const short* dright = dright0 + dstep*y; 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** pleft = pleft0 + pstep*y; 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** pright = pright0 + pstep*y; 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* lr[] = { left, right }; 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const short* dlr[] = { dleft, dright }; 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** plr[] = { pleft, pright }; 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a = aa[k]; 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y1 = y+(y>0); y1 <= y+(y<rows-1); y1++ ) 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const short* disp = (k == 0 ? dleft0 : dright0) + y1*dstep; 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** ptr = (k == 0 ? pleft0 : pright0) + y1*pstep; 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols; x++ ) 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx* v = ptr[x] = &vbuf[nvtx++]; 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->first = 0; 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v->weight = disp[x] == (short)(OCCLUDED ? -OCCLUSION_PENALTY2 : 0); 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols; x++ ) 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = dleft[x]; 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = x + d; 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var = pleft[x]; 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // (left + x, right + x + d) 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != alpha && d != OCCLUDED && (unsigned)x1 < (unsigned)cols ) 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var1 = pright[x1]; 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = dright[x1]; 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d == -d1 ) 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( var1 != 0 ); 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delta = IS_BLOCKED(alpha, d) ? INFINITY : 0; 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //add inter edge 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvAddTerm( var, var1, 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )], 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delta, delta, 0, ebuf, nedges ); 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( IS_BLOCKED(alpha, d) ) 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges ); 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // (left + x, right + x + alpha) 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = x + alpha; 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (unsigned)x1 < (unsigned)cols ) 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var1 = pright[x1]; 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = dright[x1]; 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E0a = IS_BLOCKED(d, alpha) ? INFINITY : 0; 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Ea0 = IS_BLOCKED(-d1, alpha) ? INFINITY : 0; 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Eaa = dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )]; 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvAddTerm( var, var1, 0, E0a, Ea0, Eaa, ebuf, nedges ); 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // smoothness 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** p = plr[k]; 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const short* disp = dlr[k]; 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* img = lr[k] + x*3; 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int scale; 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var = p[x]; 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = disp[x]; 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a = aa[k]; 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < cols - 1 ) 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var1 = p[x+1]; 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = disp[x+1]; 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = stabI[img[0] - img[3]]; 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale ); 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale ); 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale ); 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges ); 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < rows - 1 ) 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var1 = p[x+pstep]; 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1 = disp[x+dstep]; 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = stabI[img[0] - img[step]]; 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale ); 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale ); 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale ); 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges ); 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // visibility term 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != OCCLUDED && IS_BLOCKED(alpha, -d)) 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = x + d; 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (unsigned)x1 < (unsigned)cols ) 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != -dleft[x1] ) 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var1 = pleft[x1]; 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges ); 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = (double)cvGetTickCount() - t; 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ebuf[0].weight = ebuf[1].weight = 0; 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E += icvGCMaxFlow( vbuf, nvtx, ebuf, state2->orphans, state2->maxOrphans ); 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( E < Eprev ) 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < rows; y++ ) 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short* dleft = dleft0 + dstep*y; 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short* dright = dright0 + dstep*y; 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** pleft = pleft0 + pstep*y; 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx** pright = pright0 + pstep*y; 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols; x++ ) 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GCVtx* var = pleft[x]; 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( var && var->parent && var->t ) 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dleft[x] = (short)alpha; 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var = pright[x]; 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( var && var->parent && var->t ) 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dright[x] = (short)-alpha; 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return MIN(E, Eprev); 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right, 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess ) 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoGCState2 state2; 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.orphans = 0; 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.maxOrphans = 0; 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvFindStereoCorrespondenceGC" ); 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat lstub, *left = cvGetMat( _left, &lstub ); 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat rstub, *right = cvGetMat( _right, &rstub ); 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub ); 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub ); 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size; 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int iter, i, nZeroExpansions = 0; 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRNG rng = cvRNG(-1); 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* disp; 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _disp; 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int64 E; 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( state != 0 ); 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) && 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(left->type) == CV_8UC1 ); 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( !dispLeft || 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) ); 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( !dispRight || 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) ); 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size = cvGetSize(left); 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !state->left || state->left->width != size.width || state->left->height != size.height ) 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int pcn = (int)(sizeof(GCVtx*)/sizeof(int)); 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int vcn = (int)(sizeof(GCVtx)/sizeof(int)); 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ecn = (int)(sizeof(GCEdge)/sizeof(int)); 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->left ); 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->right ); 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->ptrLeft ); 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->ptrRight ); 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->dispLeft ); 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->dispRight ); 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->left = cvCreateMat( size.height, size.width, CV_8UC3 ); 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->right = cvCreateMat( size.height, size.width, CV_8UC3 ); 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 ); 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 ); 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) ); 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) ); 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->vtxBuf = cvCreateMat( 1, size.height*size.width*2, CV_32SC(vcn) ); 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->edgeBuf = cvCreateMat( 1, size.height*size.width*12 + 16, CV_32SC(ecn) ); 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !useDisparityGuess ) 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSet( state->dispLeft, cvScalarAll(OCCLUDED)); 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSet( state->dispRight, cvScalarAll(OCCLUDED)); 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( dispLeft && dispRight ); 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( dispLeft, state->dispLeft ); 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( dispRight, state->dispRight ); 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.Ithreshold = state->Ithreshold; 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.interactionRadius = state->interactionRadius; 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.lambda = cvRound(state->lambda*DENOMINATOR); 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.lambda1 = cvRound(state->lambda1*DENOMINATOR); 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.lambda2 = cvRound(state->lambda2*DENOMINATOR); 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.K = cvRound(state->K*DENOMINATOR); 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitStereoConstTabs(); 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitGraySubpix( left, right, state->left, state->right ); 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disp = (int*)cvStackAlloc( state->numberOfDisparities*sizeof(disp[0]) ); 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _disp = cvMat( 1, state->numberOfDisparities, CV_32S, disp ); 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities ); 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRandShuffle( &_disp, &rng ); 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) ) 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float L = icvComputeK(state)*0.2f; 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.lambda = cvRound(L*DENOMINATOR); 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state2.K < 0 ) 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.K = state2.lambda*5; 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state2.lambda1 < 0 ) 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.lambda1 = state2.lambda*3; 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state2.lambda2 < 0 ) 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state2.lambda2 = state2.lambda; 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitStereoTabs( &state2 ); 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E = icvComputeEnergy( state, &state2, !useDisparityGuess ); 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( iter = 0; iter < state->maxIters; iter++ ) 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < state->numberOfDisparities; i++ ) 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int alpha = disp[i]; 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 ); 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( Enew < E ) 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nZeroExpansions = 0; 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn E = Enew; 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( ++nZeroExpansions >= state->numberOfDisparities ) 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dispLeft ) 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( state->dispLeft, dispLeft ); 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dispRight ) 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( state->dispRight, dispRight ); 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &state2.orphans ); 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 961