xref: /external/opencv/cvaux/src/enmin.cpp

/*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 */