/*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 "_cvaux.h"
#if 0
//#include "windows.h"
//#define ALPHA_EXPANSION
#ifndef ALPHA_EXPANSION
#define ALPHA_BETA_EXCHANGE
#endif
#define MAX_LABEL 20
#define CV_MODULE(xxx) \
( (xxx) < 0 ? -(xxx) : (xxx) )
#define CV_MAX3(xxx1,xxx2,xxx3) \
( (xxx1) > (xxx2) && (xxx1) > (xxx3) ? (xxx1) : \
(xxx2) > (xxx3) ? (xxx2) : (xxx3) )
#define CV_MIN2(xxx1,xxx2) \
( (xxx1) < (xxx2) ? (xxx1) : (xxx2) )
#define getSizeForGraph(xxxType) \
( sizeof(xxxType) < 8 ? 8 : sizeof(xxxType) + 4 - sizeof(xxxType) % 4 )
#define INT_INFINITY 1000000000
#define MAX_DIFFERENCE 10
// struct Vertex is used for storing vertices of graph
// coord - coordinate corresponding pixel on the real image line
struct Vertex
{
CvGraphVtx vtx;
int coord;
};
// struct Edge is used for storing edges of graph
// weight - weight of the edge ( maximum flow via the edge )
// flow - current flow via the edge
// srcVtx - coordinate of source vertex on the real image line
// destVtx - coordinate of destination vertex on the real image line
struct Edge
{
CV_GRAPH_EDGE_FIELDS()
int weight;
int flow;
int srcVtx;
int destVtx;
};
// function vFunc is energy function which determines the difference
// between two labels ( alpha and beta )
// alpha - label number one
// beta - label number two
inline int vFunc( int alpha, int beta )
{
if( alpha == beta )
return 0;
else
return /*1*//*5*/10;
}
// function dFunc is energy function which determines energy of interaction
// between pixel ( having coordinate xCoord ) and label
// leftLine - line of left image
// rightLine - line of right image
// xCoord - coordinate of pixel on the left image
// label - label corresponding to the pixel
// width - width of the image line in pixels
inline int dFunc( unsigned char* leftLine,
unsigned char* rightLine,
int xCoord,
int label,
int width)
{
assert( xCoord >= 0 && xCoord < width );
int r, g, b;
int yCoord = xCoord + label;
if( yCoord >= width )
yCoord = width;
if( yCoord < 0 )
yCoord = 0;
r = leftLine[ 3 * xCoord ] - rightLine[ 3 * yCoord ];
g = leftLine[ 3 * xCoord + 1 ] - rightLine[ 3 * yCoord + 1 ];
b = leftLine[ 3 * xCoord + 2 ] - rightLine[ 3 * yCoord + 2 ];
r = CV_MODULE( r );
g = CV_MODULE( g );
b = CV_MODULE( b );
return CV_MAX3( r, g, b );
}
// function allocTempMem allocates all temporary memory needed for work
// of some function
// memPtr - pointer to pointer to the large block of memory
// verticesPtr - pointer to pointer to block of memory for
// temporary storing vertices
// width - width of line in pixels
void allocTempMem( int** memPtr,
int** verticesPtr,
int width )
{
int* tempPtr = ( int* ) malloc( ( width + 2 ) * 7 * sizeof( int ) );
*verticesPtr = tempPtr;
*memPtr = *verticesPtr + width + 2;
}
// function freeTempMem frees all allocated by allocTempMem function memory
// memPtr - pointer to pointer to the large block of memory
// verticesPtr - pointer to pointer to block of memory for
// temporary storing vertices
void freeTempMem( int** memPtr,
int** verticesPtr )
{
free( ( void* )( *verticesPtr ) );
*verticesPtr = NULL;
*memPtr = NULL;
}
// function makeGraph creates initial graph to find maximum flow in it
// graphPtr - pointer to pointer to CvGraph structure to be filled
// leftLine - pointer to the left image line
// rightLine - pointer to the right image line
// alpha - label number one for doing exchange
// beta - label number two for doing exchange
// corr - pointer to array of correspondences ( each element
// of array includes disparity of pixel on right image
// for pixel each on left image ). This pointer direct
// to correspondence ofr one line only
// width - width of image lines in pixels
// storage - pointer to CvMemStorage structure which contains
// memory storage
void makeGraph( CvGraph** graphPtr,
unsigned char* leftLine,
unsigned char* rightLine,
int alpha,
int beta,
int* corr,
int width,
CvMemStorage* storage )
{
int i;
if( *graphPtr ) {
cvClearGraph( *graphPtr );
}
/*else {*/
*graphPtr = cvCreateGraph( CV_SEQ_KIND_GRAPH | CV_GRAPH_FLAG_ORIENTED,
sizeof( CvGraph ),
getSizeForGraph( Vertex ),
getSizeForGraph( Edge ),
storage );
/*}*/
CvGraph* graph = *graphPtr;
#ifdef ALPHA_BETA_EXCHANGE
CvGraphVtx* newVtxPtr;
for( i = 0; i < width; i ++ )
{
if( corr[i] == alpha || corr[i] == beta ) {
cvGraphAddVtx( graph, NULL, &newVtxPtr );
( ( Vertex* )newVtxPtr ) -> coord = i;
}
} /* for( i = 0; i < width; i ++ ) */
cvGraphAddVtx( graph, NULL, &newVtxPtr );
if( newVtxPtr )
( ( Vertex* )newVtxPtr ) -> coord = -2; /* adding alpha vertex */
cvGraphAddVtx( graph, NULL, &newVtxPtr );
if( newVtxPtr )
( ( Vertex* )newVtxPtr ) -> coord = -1; /* adding beta vertex */
int alphaVtx = graph -> total - 2;
int betaVtx = graph -> total - 1;
CvGraphEdge* newEdgePtr;
CvGraphVtx* vtxPtr;
if( graph -> total > 2 )
{
for( i = 0; i < alphaVtx; i ++ )
{
vtxPtr = cvGetGraphVtx( graph, i );
/* adding edge oriented from alpha vertex to current vertex */
cvGraphAddEdge( graph, alphaVtx, i, NULL, &newEdgePtr );
( ( Edge* )newEdgePtr ) -> weight = dFunc( leftLine,
rightLine,
( ( Vertex* )vtxPtr ) -> coord,
alpha,
width );
( ( Edge* )newEdgePtr ) -> flow = 0;
if( i != 0 ) {
CvGraphVtx* tempVtxPtr = cvGetGraphVtx( graph, i - 1 );
/* if vertices are neighbours */
if( ( ( Vertex* )tempVtxPtr ) -> coord + 1 ==
( ( Vertex* )vtxPtr ) -> coord )
{
( ( Edge* )newEdgePtr ) -> weight +=
vFunc( corr[ ( ( Vertex* )tempVtxPtr ) -> coord ],
alpha );
/* adding neighbour edge oriented from current vertex
to the previous one */
CvGraphEdge* tempEdgePtr;
cvGraphAddEdge( graph, i, i - 1, NULL, &tempEdgePtr );
( ( Edge* )tempEdgePtr ) -> weight = vFunc( alpha, beta );
( ( Edge* )tempEdgePtr ) -> flow = 0;
( ( Edge* )tempEdgePtr ) -> srcVtx =
( ( Vertex* )vtxPtr ) -> coord;
( ( Edge* )tempEdgePtr ) -> destVtx =
( ( Vertex* )tempVtxPtr ) -> coord;
}
} /* if( i != 0 ) */
if( i != alphaVtx - 1 ) {
CvGraphVtx* tempVtxPtr = cvGetGraphVtx( graph, i + 1 );
/* if vertices are neighbours */
if( ( ( Vertex* )tempVtxPtr ) -> coord - 1 ==
( ( Vertex* )vtxPtr ) -> coord )
{
( ( Edge* )newEdgePtr ) -> weight +=
vFunc( corr[ ( ( Vertex* )tempVtxPtr ) -> coord ],
alpha );
/* adding neighbour edge oriented from current vertex
to the next one */
CvGraphEdge* tempEdgePtr;
cvGraphAddEdge( graph, i, i + 1, NULL, &tempEdgePtr );
( ( Edge* )tempEdgePtr ) -> weight = vFunc( alpha, beta );
( ( Edge* )tempEdgePtr ) -> flow = 0;
( ( Edge* )tempEdgePtr ) -> srcVtx =
( ( Vertex* )vtxPtr ) -> coord;
( ( Edge* )tempEdgePtr ) -> destVtx =
( ( Vertex* )tempVtxPtr ) -> coord;
}
} /* if( i != alphaVtx - 1 ) */
( ( Edge* )newEdgePtr ) -> flow = 0;
( ( Edge* )newEdgePtr ) -> srcVtx = -1; /* source vertex is alpha
vertex */
( ( Edge* )newEdgePtr ) -> destVtx = ( ( Vertex* )vtxPtr ) -> coord;
/* adding edge oriented from current vertex to beta vertex */
cvGraphAddEdge( graph, i, betaVtx, NULL, &newEdgePtr );
( ( Edge* )newEdgePtr ) -> weight = dFunc( leftLine,
rightLine,
( ( Vertex* )vtxPtr ) -> coord,
beta,
width );
( ( Edge* )newEdgePtr ) -> flow = 0;
if( i != 0 ) {
CvGraphVtx* tempVtxPtr = cvGetGraphVtx( graph, i - 1 );
/* if vertices are neighbours */
if( ( ( Vertex* )tempVtxPtr ) -> coord + 1 ==
( ( Vertex* )vtxPtr ) -> coord )
{
( ( Edge* )newEdgePtr ) -> weight +=
vFunc( corr[ ( ( Vertex* )tempVtxPtr ) -> coord ],
beta );
}
} /* if( i != 0 ) */
if( i != alphaVtx - 1 ) {
CvGraphVtx* tempVtxPtr = cvGetGraphVtx( graph, i + 1 );
/* if vertices are neighbours */
if( ( ( Vertex* )tempVtxPtr ) -> coord - 1 ==
( ( Vertex* )vtxPtr ) -> coord )
{
( ( Edge* )newEdgePtr ) -> weight +=
vFunc( corr[ ( ( Vertex* )tempVtxPtr ) -> coord ],
beta );
}
} /* if( i != alphaVtx - 1 ) */
( ( Edge* )newEdgePtr ) -> flow = 0;
( ( Edge* )newEdgePtr ) -> srcVtx =
( ( Vertex* )vtxPtr ) -> coord;
( ( Edge* )newEdgePtr ) -> destVtx = -2; /* destination vertex is
beta vertex */
} /* for( i = 0; i < graph -> total - 2; i ++ ) */
} /* if( graph -> total > 2 ) */
#endif /* #ifdef ALPHA_BETA_EXCHANGE */
#ifdef ALPHA_EXPANSION
#endif /* #ifdef ALPHA_EXPANSION */
} /* makeGraph */
// function makeHelpGraph creates help graph using initial graph
// graph - pointer to initial graph ( represented by CvGraph
// structure )
// hlpGraphPtr - pointer to pointer to new help graph
// storage - pointer to CvStorage structure
// mem - pointer to memory allocated by allocTempMem function
// vertices - pointer to memory allocated by allocTempMem function
// verticesCountPtr- pointer to value containing number of vertices
// in vertices array
// width - width of image line in pixels
int makeHelpGraph( CvGraph* graph,
CvGraph** hlpGraphPtr,
CvMemStorage* storage,
int* mem,
int* vertices,
int* verticesCountPtr,
int width )
{
int u, v;
int* order = mem;
int* lengthArr = order + width + 2;
int s = graph -> total - 2; /* source vertex */
int t = graph -> total - 1; /* terminate vertex */
int orderFirst;
int orderCount;
int &verticesCount = *verticesCountPtr;
CvGraph* hlpGraph;
if( *hlpGraphPtr ) {
cvClearGraph( *hlpGraphPtr );
}
else {
*hlpGraphPtr = cvCreateGraph( CV_SEQ_KIND_GRAPH |
CV_GRAPH_FLAG_ORIENTED,
sizeof( CvGraph ),
getSizeForGraph( Vertex ),
getSizeForGraph( Edge ),
storage );
}
hlpGraph = *hlpGraphPtr;
/* initialization */
for( u = 0; u < graph -> total; u ++ )
{
lengthArr[ u ] = INT_INFINITY;
cvGraphAddVtx( hlpGraph, NULL, NULL );
} /* for( u = 0; u < graph -> total - 1; u ++ ) */
orderFirst = 0;
orderCount = 0;
verticesCount = 0;
lengthArr[ s ] = 0;
/* add vertex s to order */
order[ orderCount ] = s;
orderCount ++;
while( orderCount != orderFirst )
{
/* getting u from order */
u = order[ orderFirst ];
orderFirst ++;
/* adding u to vertex array */
vertices[ verticesCount ] = u;
verticesCount ++;
int ofs;
CvGraphVtx* graphVtx = cvGetGraphVtx( graph, u );
/* processing all vertices outgoing from vertex u */
CvGraphEdge* graphEdge = graphVtx -> first;
while( graphEdge )
{
int tempVtxIdx = cvGraphVtxIdx( graph, graphEdge -> vtx[1] );
ofs = tempVtxIdx == u;
if( !ofs )
{
v = tempVtxIdx;
CvGraphEdge* tempGraphEdge = cvFindGraphEdge( graph, u, v );
if( ( lengthArr[ u ] < lengthArr[ v ] )
&& ( lengthArr[ v ] <= lengthArr[ t ] )
&& ( ( ( Edge* )tempGraphEdge ) -> flow <
( ( Edge* )tempGraphEdge ) -> weight ) )
{
if( lengthArr[ v ] == INT_INFINITY )
{
/* adding vertex v to order */
order[ orderCount ] = v;
orderCount ++;
lengthArr[ v ] = lengthArr[ u ] + 1;
CvGraphEdge* tempGraphEdge2;
cvGraphAddEdge( hlpGraph, u, v, NULL, &tempGraphEdge2 );
( ( Edge* )tempGraphEdge2 ) -> flow = 0;
( ( Edge* )tempGraphEdge2 ) -> weight =
( ( Edge* )tempGraphEdge ) -> weight -
( ( Edge* )tempGraphEdge ) -> flow;
} /* if( length[ v ] == INT_INFINITY ) */
} /* if( ( lengthArr[ u ] < lengthArr[ v ] ) ... */
} /* if( !ofs ) */
graphEdge = graphEdge -> next[ ofs ];
} /* while( graphEdge ) */
/* processing all vertices incoming to vertex u */
graphEdge = graphVtx -> first;
while( graphEdge )
{
int tempVtxIdx = cvGraphVtxIdx( graph, graphEdge -> vtx[1] );
ofs = tempVtxIdx == u;
if( ofs )
{
tempVtxIdx = cvGraphVtxIdx( graph, graphEdge -> vtx[0] );
v = tempVtxIdx;
CvGraphEdge* tempGraphEdge = cvFindGraphEdge( graph, v, u );
if( ( lengthArr[ u ] < lengthArr[ v ] )
&& ( lengthArr[ v ] <= lengthArr[ t ] )
&& ( ( ( Edge* )tempGraphEdge ) -> flow > 0 ) )
{
if( lengthArr[ v ] == INT_INFINITY )
{
/* adding vertex v to order */
order[ orderCount ] = v;
orderCount ++;
lengthArr[ v ] = lengthArr[ u ] + 1;
CvGraphEdge* tempGraphEdge3 = cvFindGraphEdge( hlpGraph, u, v );
if( tempGraphEdge3 == NULL ||
( ( Edge* )tempGraphEdge3 ) -> weight == 0 )
{
CvGraphEdge* tempGraphEdge2;
cvGraphAddEdge( hlpGraph, u, v, NULL,
&tempGraphEdge2 );
( ( Edge* )tempGraphEdge2 ) -> flow = 0;
( ( Edge* )tempGraphEdge2 ) -> weight = 0;
} /* if( tempGraphEdge3 == NULL || ... */
( ( Edge* )tempGraphEdge3 ) -> weight +=
( ( Edge* )tempGraphEdge ) -> flow;
} /* if( length[ v ] == INT_INFINITY ) */
} /* if( ( lengthArr[ u ] < lengthArr[ v ] ) ... */
} /* if( ofs ) */
graphEdge = graphEdge -> next[ ofs ];
} /* while( graphEdge ) */
} /* while( orderCount != orderFirst ) */
int i;
for( i = 0; i < hlpGraph -> total - 2; i ++ )
{
CvGraphVtx* hlpGraphVtxTemp = cvGetGraphVtx( hlpGraph, i );
if( hlpGraphVtxTemp ) {
if( !hlpGraphVtxTemp -> first ) {
cvGraphRemoveVtxByPtr( hlpGraph, hlpGraphVtxTemp );
}
}
} /* for( i = 0; i < hlpGraph -> total - 2; i ++ ) */
return lengthArr[ t ];
} /* makeHelpGraph */
// function makePseudoMaxFlow increases flow in graph by using hlpGraph
// graph - pointer to initial graph
// hlpGraph - pointer to help graph
// vertices - pointer to vertices array
// verticesCount - number of vertices in vertices array
// mem - pointer to memory allocated by allocTempMem function
// width - width of image line in pixels
void makePseudoMaxFlow( CvGraph* graph,
CvGraph* hlpGraph,
int* vertices,
int verticesCount,
int* mem,
int width )
{
int stekCount;
int orderFirst;
int orderCount;
int i;
int v, u;
int* stek = mem;
int* order = stek + width + 2;
int* incomFlow = order + width + 2;
int* outgoFlow = incomFlow + width + 2;
int* flow = outgoFlow + width + 2;
int* cargo = flow+ width + 2;
int s = graph -> total - 2; /* source vertex */
int t = graph -> total - 1; /* terminate vertex */
int realVerticesCount = verticesCount;
stekCount = 0;
for( i = 0; i < verticesCount; i ++ )
{
v = vertices[ i ];
incomFlow[ v ] = outgoFlow[ v ] = 0;
if( v == s ) {
incomFlow[ v ] = INT_INFINITY;
} /* if( v == s ) */
else {
CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
CvGraphEdge* hlpGraphEdge = hlpGraphVtx -> first;
int ofs;
while( hlpGraphEdge )
{
int vtxIdx = cvGraphVtxIdx( hlpGraph,
hlpGraphEdge -> vtx[1] );
ofs = vtxIdx == v;
if( ofs )
{
incomFlow[ v ] += ( ( Edge* )hlpGraphEdge ) -> weight;
} /* if( ofs ) */
hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
} /* while( hlpGraphEdge ) */
} /* if( v == s ) else */
if( v == t ) {
outgoFlow[ v ] = INT_INFINITY;
} /* if( v == t ) */
else {
CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
CvGraphEdge* hlpGraphEdge = hlpGraphVtx -> first;
int ofs;
while( hlpGraphEdge )
{
int vtxIdx = cvGraphVtxIdx( hlpGraph,
hlpGraphEdge -> vtx[1] );
ofs = vtxIdx == v;
if( !ofs )
{
outgoFlow[ v ] += ( ( Edge* )hlpGraphEdge ) -> weight;
} /* if( ofs ) */
hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
} /* while( hlpGraphEdge ) */
} /* if( v == t ) else */
flow[ v ] = CV_MIN2( incomFlow[ v ], outgoFlow[ v ] );
if( !flow[ v ] ) {
stek[ stekCount ] = v;
stekCount ++;
} /* if( !flow[ v ] ) */
} /* for( i = 0; i < verticesCount; i ++ ) */
for( i = 0; i < verticesCount; i ++ )
{
v = vertices[ i ];
cargo[ v ] = 0;
} /* for( i = 0; i < verticesCount; i ++ ) */
while( realVerticesCount > 2 )
{
/* deleting all vertices included in stek */
while( stekCount )
{
v = stek[ stekCount - 1 ];
stekCount --;
/* deleting edges incoming to v and outgoing from v */
int ofs;
CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
CvGraphEdge* hlpGraphEdge;
if( hlpGraphVtx ) {
hlpGraphEdge = hlpGraphVtx -> first;
}
else {
hlpGraphEdge = NULL;
}
while( hlpGraphEdge )
{
CvGraphVtx* hlpGraphVtx2 = hlpGraphEdge -> vtx[ 1 ];
int hlpGraphVtxIdx2 = cvGraphVtxIdx( hlpGraph,
hlpGraphVtx2 );
ofs = hlpGraphVtxIdx2 == v;
if( ofs )
{
/* hlpGraphEdge is incoming edge */
CvGraphVtx* hlpGraphVtx3 = hlpGraphEdge -> vtx[0];
u = cvGraphVtxIdx( hlpGraph,
hlpGraphVtx3 );
outgoFlow[ u ] -= ( ( Edge* )hlpGraphEdge ) -> weight
- ( ( Edge* )hlpGraphEdge ) -> flow;
cvGraphRemoveEdgeByPtr( hlpGraph,
hlpGraphVtx3,
hlpGraphVtx2 );
if( flow[ u ] != 0 ) {
flow[ u ] = CV_MIN2( incomFlow[u], outgoFlow[u] );
if( flow[ u ] == 0 ) {
stek[ stekCount ] = u;
stekCount ++;
}
}
} /* if( ofs ) */
else
{
/* hlpGraphEdge is outgoing edge */
CvGraphVtx* hlpGraphVtx3 = hlpGraphEdge -> vtx[1];
int u = cvGraphVtxIdx( hlpGraph,
hlpGraphVtx3 );
incomFlow[ u ] -= ( ( Edge* )hlpGraphEdge ) -> weight
- ( ( Edge* )hlpGraphEdge ) -> flow;
cvGraphRemoveEdgeByPtr( hlpGraph,
hlpGraphVtx2,
hlpGraphVtx3 );
if( flow[ u ] != 0 ) {
flow[ u ] = CV_MIN2( incomFlow[u], outgoFlow[u] );
if( flow[ u ] == 0 ) {
stek[ stekCount ] = u;
stekCount ++;
}
}
} /* if( ofs ) else */
hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
} /* while( hlpGraphEdge ) */
/* deleting vertex v */
cvGraphRemoveVtx( hlpGraph, v );
realVerticesCount --;
} /* while( stekCount ) */
if( realVerticesCount > 2 ) /* the flow is not max still */
{
int p = INT_INFINITY;
int r = -1;
CvGraphVtx* graphVtx;
if( realVerticesCount == 3 ) {
r = r;
}
for( i = 0; i < hlpGraph -> total - 2; i ++ )
{
graphVtx = cvGetGraphVtx( hlpGraph, i );
if( graphVtx ) {
v = cvGraphVtxIdx( hlpGraph, graphVtx );
if( flow[ v ] < p ) {
r = v;
p = flow[ v ];
}
}
} /* for( i = 0; i < hlpGraph -> total - 2; i ++ ) */
/* building of size p flow from r to t */
orderCount = orderFirst = 0;
order[ orderCount ] = r;
orderCount ++;
v = order[ orderFirst ];
orderFirst ++;
cargo[ r ] = p;
do /* while( v != t ) */
{
incomFlow[ v ] -= cargo[ v ];
outgoFlow[ v ] -= cargo[ v ];
flow[ v ] -= cargo[ v ];
if( flow[ v ] == 0 ) {
stek[ stekCount ] = v;
stekCount ++;
}
if( v == t ) {
cargo[ v ] = p;
}
else
{
int ofs;
CvGraphVtx* hlpGraphVtx2;
CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
CvGraphEdge* hlpGraphEdge = hlpGraphVtx -> first;
CvGraphEdge* hlpGraphEdge2 = NULL;
while( hlpGraphEdge && cargo[ v ] > 0 )
{
hlpGraphVtx2 = hlpGraphEdge -> vtx[ 1 ];
u = cvGraphVtxIdx( hlpGraph, hlpGraphVtx2 );
ofs = u == v;
if( !ofs )
{
if( cargo[ u ] == 0 ) {
order[ orderCount ] = u;
orderCount ++;
}
int delta = ( ( Edge* )hlpGraphEdge ) -> weight
- ( ( Edge* )hlpGraphEdge ) -> flow;
delta = CV_MIN2( cargo[ v ], delta );
( ( Edge* )hlpGraphEdge ) -> flow += delta;
cargo[ v ] -= delta;
cargo[ u ] += delta;
if( ( ( Edge* )hlpGraphEdge ) -> weight ==
( ( Edge* )hlpGraphEdge ) -> flow )
{
/* deleting hlpGraphEdge */
hlpGraphEdge2 = hlpGraphEdge -> next[ ofs ];
CvGraphEdge* graphEdge =
cvFindGraphEdge( graph, v, u );
( ( Edge* )graphEdge ) -> flow +=
( ( Edge* )hlpGraphEdge ) -> flow;
cvGraphRemoveEdgeByPtr( hlpGraph,
hlpGraphEdge -> vtx[0],
hlpGraphEdge -> vtx[1] );
}
} /* if( !ofs ) */
if( hlpGraphEdge2 ) {
hlpGraphEdge = hlpGraphEdge2;
hlpGraphEdge2 = NULL;
}
else {
hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
}
} /* while( hlpGraphEdge && cargo[ v ] > 0 ) */
} /* if( v == t ) else */
v = order[ orderFirst ];
orderFirst ++;
} while( v != t ); /* do */
/* building of size p flow from s to r */
orderCount = orderFirst = 0;
order[ orderCount ] = r;
orderCount ++;
v = order[ orderFirst ];
orderFirst ++;
cargo[ r ] = p;
do /* while( v != s ) */
{
if( v != r )
{
incomFlow[ v ] -= cargo[ v ];
outgoFlow[ v ] -= cargo[ v ];
flow[ v ] -= cargo[ v ];
if( flow[ v ] == 0 ) {
stek[ stekCount ] = v;
stekCount ++;
}
} /* if( v != r ) */
if( v == s ) {
cargo[ v ] = 0;
} /* if( v == s ) */
else
{
int ofs;
CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
CvGraphEdge* hlpGraphEdge = hlpGraphVtx -> first;
CvGraphEdge* hlpGraphEdge2 = NULL;
while( hlpGraphEdge && cargo[ v ] > 0 )
{
u = cvGraphVtxIdx( hlpGraph,
hlpGraphEdge -> vtx[ 1 ] );
ofs = u == v;
if( ofs )
{
u = cvGraphVtxIdx( hlpGraph,
hlpGraphEdge -> vtx[ 0 ] );
if( cargo[ u ] == 0 ) {
order[ orderCount ] = u;
orderCount ++;
}
int delta = ( ( Edge* )hlpGraphEdge ) -> weight
- ( ( Edge* )hlpGraphEdge ) -> flow;
delta = CV_MIN2( cargo[ v ], delta );
(( ( Edge* )hlpGraphEdge ) -> flow) += delta;
cargo[ v ] -= delta;
cargo[ u ] += delta;
if( ( ( Edge* )hlpGraphEdge ) -> weight ==
( ( Edge* )hlpGraphEdge ) -> flow )
{
hlpGraphEdge2 = hlpGraphEdge -> next[ ofs ];
CvGraphEdge* graphEdge =
cvFindGraphEdge( graph, u, v );
( ( Edge* )graphEdge ) -> flow +=
( ( Edge* )hlpGraphEdge ) -> flow;
cvGraphRemoveEdgeByPtr( hlpGraph,
hlpGraphEdge -> vtx[0],
hlpGraphEdge -> vtx[1] );
}
} /* if( ofs ) */
if( hlpGraphEdge2 ) {
hlpGraphEdge = hlpGraphEdge2;
hlpGraphEdge2 = NULL;
}
else {
hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
}
} /* while( hlpGraphEdge && cargo[ v ] > 0 ) */
} /* if( v == s ) else */
v = order[ orderFirst ]; //added
orderFirst ++; //added
} while( v != s ); /* do */
} /* if( hlpGraph -> total > 2 ) */
} /* while( hlpGraph -> total > 2 ) */
} /* makePseudoMaxFlow */
// function oneStep produces one alpha-beta exchange for one line of images
// leftLine - pointer to the left image line
// rightLine - pointer to the right image line
// alpha - label number one
// beta - label number two
// corr - pointer to correspondence array for this line
// width - width of image line in pixels
// mem - pointer to memory allocated by allocTempMem function
// vertices - pointer to vertices array allocated by allocTempMem
// function
// storage - pointer to CvMemStorage structure
bool oneStep( unsigned char* leftLine,
unsigned char* rightLine,
int alpha,
int beta,
int* corr,
int width,
int* mem,
int* vertices,
CvMemStorage* storage )
{
CvGraph* graph = NULL;
CvGraph* hlpGraph = NULL;
CvMemStoragePos storagePos;
int i;
bool change = false;
cvSaveMemStoragePos( storage, &storagePos );
int verticesCount;
makeGraph( &graph, leftLine, rightLine, alpha, beta, corr, width, storage );
int s = graph -> total - 2; /* source vertex - alpha vertex */
//int t = graph -> total - 1; /* terminate vertex - beta vertex */
int length = makeHelpGraph( graph,
&hlpGraph,
storage,
mem,
vertices,
&verticesCount,
width );
while( length != INT_INFINITY )
{
change = true;
makePseudoMaxFlow( graph,
hlpGraph,
vertices,
verticesCount,
mem,
width );
cvClearGraph( hlpGraph );
length = makeHelpGraph( graph,
&hlpGraph,
storage,
mem,
vertices,
&verticesCount,
width );
} /* while( length != INT_INFINITY ) */
int coord;
CvGraphVtx* graphVtx;
for( i = 0; i < s; i ++ )
{
CvGraphEdge* graphEdge = cvFindGraphEdge( graph, s, i );
if( ( ( Edge* )graphEdge ) -> weight ==
( ( Edge* )graphEdge ) -> flow )
{ /* this vertex must have alpha label */
graphVtx = cvGetGraphVtx( graph, i );
coord = ( ( Vertex* )graphVtx )-> coord;
if( corr[ coord ] != alpha ) {
corr[ coord ] = alpha; //added
change = true;
}
else {
corr[ coord ] = alpha;
}
} /* if( ( ( Edge* )graphEdge ) -> weight == ... */
else
{ /* this vertex must have beta label */
graphVtx = cvGetGraphVtx( graph, i );
coord = ( ( Vertex* )graphVtx )-> coord;
if( corr[ coord ] != beta ) {
corr[ coord ] = beta; //added
change = true;
}
else {
corr[ coord ] = beta;
}
} /* if( ( ( Edge* )graphEdge ) -> weight == ... else */
} /* for( i = 0; i < s; i ++ ) */
cvClearGraph( hlpGraph );
cvClearGraph( graph );
cvRestoreMemStoragePos( storage, &storagePos );
return change;
} /* oneStep */
// function initCorr fills correspondence array with initial values
// corr - pointer to correspondence array for this line
// width - width of image line in pixels
// maxPixelDifference - maximum value of difference between the same
// point painted on two images
void initCorr( int* corr, int width, int maxPixelDifference )
{
int i;
int pixelDifferenceRange = maxPixelDifference * 2 + 1;
for( i = 0; i < width; i ++ )
{
corr[ i ] = i % pixelDifferenceRange - maxPixelDifference;
}
} /* initCorr */
// function oneLineCorr fully computes one line of images
// leftLine - pointer to the left image line
// rightLine - pointer to the right image line
// corr - pointer to the correspondence array for one
// line
// mem - pointer to memory allocated by allocTempMem
// function
// vertices - pointer to memory allocated by allocTempMem
// function
// width - width of image line in pixels
// maxPixelDifference - maximum value of pixel differences in pixels
// storage - pointer to CvMemStorage struct which
// contains memory storage
void oneLineCorr( unsigned char* leftLine,
unsigned char* rightLine,
int* corr,
int* mem,
int* vertices,
int width,
int maxPixelDifference,
CvMemStorage* storage )
{
int result = 1;
int count = 0;
int i, j;
initCorr( corr, width, maxPixelDifference );
while( result )
{
result = 0;
for( i = - maxPixelDifference; i < maxPixelDifference; i ++ )
for( j = i + 1; j <= maxPixelDifference; j ++ )
{
result += (int)oneStep( leftLine,
rightLine,
i,
j,
corr,
width,
mem,
vertices,
storage );
} /* for( j = i + 1; j < width; j ++ ) */
count ++;
if( count > /*0*//*1*/2 ) {
break;
}
} /* while( result ) */
} /* oneLineCorr */
// function allLinesCorr computes all lines on the images
// leftImage - pointer to the left image
// leftLineStep - size of line on the left image in bytes
// rightImage - pointer to the right image
// rightLineStep - size of line on the right image in bytes
// width - width of line in pixels
// height - height of image in pixels
// corr - pointer to correspondence array for all lines
// maxPixelDifference - maximum value of difference between the same
// point painted on two images
// storage - pointer to CvMemStorage which contains memory
// storage
void allLinesCorr( unsigned char* leftImage,
int leftLineStep,
unsigned char* rightImage,
int rightLineStep,
int width,
int height,
int* corr,
int maxPixelDifference,
CvMemStorage* storage )
{
int i;
unsigned char* leftLine = leftImage;
unsigned char* rightLine = rightImage;
int* mem;
int* vertices;
allocTempMem( &mem,
&vertices,
width );
for( i = 0; i < height; i ++ )
{
oneLineCorr( leftLine,
rightLine,
corr + i * width,
mem,
vertices,
width,
maxPixelDifference,
storage );
leftLine += leftLineStep;
rightLine += rightLineStep;
} /* for( i = 0; i < height; i ++ ) */
freeTempMem( &mem,
&vertices );
} /* allLinesCorr */
// This function produces morphing of two images into one image, which includes morphed
// image or depth map
// _leftImage - pointer to left image
// _leftLineStep - size of line on left image in bytes
// _rightImage - pointer to right image
// _rightLineStep - size of line on right image in bytes
// _resultImage - pointer to result morphed image
// _resultLineStep - size of line on result image in bytes
// _corrArray - pointer to array with correspondences
// _numCorrArray - pointer to array with numbers correspondeces on each line
// width - width of images
// height - height of images
// alpha - position of virtual camera ( 0 corresponds to left image, 1 - to right one )
// imageNeed - defines your wishes. if you want to see normal morphed image you have to set
// this parameter to morphNormalImage ( this is default value ), else if you want
// to see depth map you have to set this parameter to morphDepthMap and set the
// next parameter ( maxPixelDifference ) to real value
// maxPixelDifference - maximum value of pixel difference on two images
void CCvGraphCutMorpher::Morph( unsigned char* _leftImage,
int _leftLineStep,
unsigned char* _rightImage,
int _rightLineStep,
unsigned char* _resultImage,
int _resultLineStep,
int* _corrArray,
int width,
int height,
float alpha,
morphImageType imageNeed,
int maxDifference
)
{
unsigned char* leftArray = _leftImage;
unsigned char* middleArray = _resultImage;
unsigned char* rightArray = _rightImage;
int leftLineSize = _leftLineStep;
int middleLineSize = _resultLineStep;
int rightLineSize = _rightLineStep;
int lineNumber;
unsigned char* leftTemp;
unsigned char* middleTemp;
unsigned char* rightTemp;
int leftPixel;
int prevLeftPixel;
int middlePixel;
int prevMiddlePixel;
int rightPixel;
int prevRightPixel;
int leftPixel3;
int middlePixel3;
int rightPixel3;
int i;
int j;
int tempIndex;
int* result;
int number;
float alpha1 = 1.0f - alpha;
for( lineNumber = 0; lineNumber < height; lineNumber ++ )
{
leftTemp = leftArray + leftLineSize * lineNumber;
middleTemp = middleArray + middleLineSize * lineNumber;
rightTemp = rightArray + rightLineSize * lineNumber;
memset( ( void* )middleTemp, 0, middleLineSize );
result = _corrArray + width * lineNumber;
number = width;
prevLeftPixel = 0;
prevRightPixel = prevLeftPixel + result[ 0 ];
if( prevRightPixel >= width ) {
prevRightPixel = width - 1;
}
else if ( prevRightPixel < 0 ) {
prevRightPixel = 0;
}
prevMiddlePixel =
(int)( prevLeftPixel * alpha1 + prevRightPixel * alpha );
for( i = 0; i < number - 1; i ++ )
{
leftPixel = i;
rightPixel = i + result[ i ];
if( rightPixel >= width ) {
rightPixel = width - 1;
}
else if( rightPixel < 0 ) {
rightPixel = 0;
}
middlePixel =
(int)( leftPixel * alpha1 + rightPixel * alpha );
leftPixel3 = leftPixel * 3;
middlePixel3 = middlePixel * 3;
rightPixel3 = rightPixel * 3;
if( imageNeed == morphDepthMap ) {
int t = leftPixel - rightPixel + maxDifference;
t = t < 0 ? -t : t;
t = t * 255 / maxDifference / 2;
middleTemp[ middlePixel3 ] = ( unsigned char )t;
middleTemp[ middlePixel3 + 1 ] = ( unsigned char )t;
middleTemp[ middlePixel3 + 2 ] = ( unsigned char )t;
} // if( imageNeed == morphDepthMap )
else
{
middleTemp[ middlePixel3 ] =
(unsigned char)( leftTemp[ leftPixel3 ] * alpha1
+ rightTemp[ rightPixel3 ] * alpha );
middleTemp[ middlePixel3 + 1 ] =
(unsigned char)( leftTemp[ leftPixel3 + 1 ] * alpha1
+ rightTemp[ rightPixel3 + 1 ] * alpha );
middleTemp[ middlePixel3 + 2 ] =
(unsigned char)( leftTemp[ leftPixel3 + 2 ] * alpha1
+ rightTemp[ rightPixel3 + 2 ] * alpha );
if( middlePixel - prevMiddlePixel > 1 ) // occlusion
{
if( leftPixel - prevLeftPixel > 1 )
{
int LenSrc = leftPixel - prevLeftPixel - 2;
int LenDest = middlePixel - prevMiddlePixel - 1;
for( j = prevMiddlePixel + 1; j < middlePixel; j ++ )
{
tempIndex = prevLeftPixel + 1 + LenSrc *
( j - prevMiddlePixel - 1 ) / LenDest;
middleTemp[ j * 3 ] =
leftTemp[ tempIndex * 3 ];
middleTemp[ j * 3 + 1 ] =
leftTemp[ tempIndex * 3 + 1 ];
middleTemp[ j * 3 + 2 ] =
leftTemp[ tempIndex * 3 + 2 ];
}
} // if( leftPixel - prevLeftPixel > 1 )
else
{
int LenSrc = rightPixel - prevRightPixel - 2;
int LenDest = middlePixel - prevMiddlePixel - 1;
for( j = prevMiddlePixel + 1; j < middlePixel; j ++ )
{
tempIndex = prevRightPixel + 1 + LenSrc *
( j - prevMiddlePixel - 1 ) / LenDest;
middleTemp[ j * 3 ] =
rightTemp[ tempIndex * 3 ];
middleTemp[ j * 3 + 1 ] =
rightTemp[ tempIndex * 3 + 1 ];
middleTemp[ j * 3 + 2 ] =
rightTemp[ tempIndex * 3 + 2 ];
}
} // if( leftPixel - prevLeftPixel > 1 ) else
} // if( middlePixel - prevMiddlePixel > 1 )
} // if( imageNeed == morphDepthMap ) else
if( middlePixel > prevMiddlePixel ) {
if( leftPixel > prevLeftPixel )
prevLeftPixel = leftPixel;
if( rightPixel > prevRightPixel )
prevRightPixel = rightPixel;
prevMiddlePixel = middlePixel;
}
} // for( i = number - 1; i >= 0; i -- )
} // for( lineNumber = 0; lineNumber < LeftImage -> m_Raster -> GetHeight() )
} // Morph
bool CCvGraphCutMorpher::OnCalculateStereo()
{
CvSize imageSizeLeft = GetImageSize( m_left_img ),
imageSizeRight = GetImageSize( m_right_img );
if( ( imageSizeLeft.width != imageSizeRight.width )
|| ( imageSizeLeft.height != imageSizeRight.height ) )
{
return false;
}
if( m_corr ) {
free( m_corr );
m_corr = NULL;
}
m_corr = ( int* ) malloc( m_left_img -> width
* m_left_img -> height
* sizeof( int ) );
if( !m_storage ) {
m_storage = cvCreateMemStorage( 0 );
m_isStorageAllocated = true;
}
// Find correspondence for full image and store it to corr array
allLinesCorr( ( unsigned char* )m_left_img -> imageData,
m_left_img -> widthStep,
( unsigned char* )m_right_img -> imageData,
m_right_img -> widthStep,
m_left_img -> width,
m_left_img -> height,
m_corr,
m_maxPixelDifference,
m_storage );
m_isStereoReady = true;
return true;
}
bool CCvGraphCutMorpher::OnCalculateVirtualImage()
{
// Output image to ResultImage window
Morph( ( unsigned char* )m_left_img -> imageData,
m_left_img ->widthStep,
( unsigned char* )m_right_img -> imageData,
m_right_img -> widthStep,
( unsigned char* )m_virtual_img -> imageData,
m_virtual_img -> widthStep,
m_corr,
m_left_img -> width,
m_left_img -> height,
m_pan );
m_isVirtualImageReady = true;
return true;
}
bool CCvGraphCutMorpher::OnCalculateDisparity()
{
Morph( ( unsigned char* )m_left_img -> imageData,
m_left_img ->widthStep,
( unsigned char* )m_right_img -> imageData,
m_right_img -> widthStep,
( unsigned char* )m_disparity_img -> imageData,
m_disparity_img -> widthStep,
m_corr,
m_left_img -> width,
m_left_img -> height,
m_pan,
morphDepthMap,
m_maxPixelDifference );
return true;
}
bool CCvGraphCutMorpher::OnCalculateDisparityImage()
{
Morph( ( unsigned char* )m_left_img -> imageData,
m_left_img ->widthStep,
( unsigned char* )m_right_img -> imageData,
m_right_img -> widthStep,
( unsigned char* )m_disparity_img -> imageData,
m_disparity_img -> widthStep,
m_corr,
m_left_img -> width,
m_left_img -> height,
m_pan,
morphDepthMap,
m_maxPixelDifference );
return true;
}
CCvGraphCutMorpher::CCvGraphCutMorpher()
{
m_maxPixelDifference = MAX_DIFFERENCE;
m_corr = 0;
m_isStereoReady = false;
m_isVirtualImageReady = false;
m_isDisparityReady = false;
m_storage = NULL;
m_isStorageAllocated = false;
}
#endif
/* End of file */