GCoptimization.cpp

Go to the documentation of this file.

#ifdef MATLAB_MEX_FILE
#include <mex.h>
#endif
#include "GCoptimization.h"
#include "LinkedBlockList.h"
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <algorithm>

// will leave this one just for the laughs :)
//#define olga_assert(expr) assert(!(expr))

// Choose reasonably high-precision timer (sub-millisec resolution if possible).
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#define VC_EXTRALEAN
#define NOMINMAX
#include <windows.h>
extern "C" gcoclock_t GCO_CLOCKS_PER_SEC = 0;
extern "C" inline gcoclock_t gcoclock() // TODO: not thread safe; separate begin/end so that end doesn't have to check for query frequency
{
        gcoclock_t result = 0;
        if (GCO_CLOCKS_PER_SEC == 0)
                QueryPerformanceFrequency((LARGE_INTEGER*)&GCO_CLOCKS_PER_SEC);
        QueryPerformanceCounter((LARGE_INTEGER*)&result);
        return result;
}

#else
extern "C" gcoclock_t GCO_CLOCKS_PER_SEC = CLOCKS_PER_SEC;
extern "C" gcoclock_t gcoclock() { return clock(); }
#endif

#ifdef MATLAB_MEX_FILE
extern "C" bool utIsInterruptPending();
static void flushnow()
{
        // Don't flush to frequently, for overall speed.
        static gcoclock_t prevclock = 0;
        gcoclock_t now = gcoclock();
        if (now - prevclock > GCO_CLOCKS_PER_SEC/5) {
                prevclock = now;
                mexEvalString("drawnow;");
        }
}
#define INDEX0 1  // print 1-based label and site indices for MATLAB
#else
inline static bool utIsInterruptPending() { return false; }
static void flushnow() { }
#define INDEX0 0  // print 0-based label and site indices
#endif

// Singly-linked list helper functions; works on any struct with a 'next' member.
template <typename T>
void slist_clear(T*& head)
{
        while (head) {
                T* temp = head;
                head = head->next;
                delete temp;
        }
}

template <typename T>
void slist_prepend(T*& head, T* val)
{
        val->next = head;
        head = val;
}


void GCException::Report() 
{
        printf("\n%s\n",message);
        exit(0);
}



//   First we have functions for the base class
// Constructor for base class                                                       
GCoptimization::GCoptimization(SiteID nSites, LabelID nLabels) 
: m_num_labels(nLabels)
, m_num_sites(nSites)
, m_datacostIndividual(0)
, m_smoothcostIndividual(0)
, m_labelcostsAll(0)
, m_labelcostsByLabel(0)
, m_labelcostCount(0)
, m_smoothcostFn(0)
, m_datacostFn(0)
, m_numNeighborsTotal(0)
, m_queryActiveSitesExpansion(&GCoptimization::queryActiveSitesExpansion<DataCostFnFromArray>)
, m_setupDataCostsSwap(0)
, m_setupDataCostsExpansion(0)
, m_setupSmoothCostsSwap(0)
, m_setupSmoothCostsExpansion(0)
, m_applyNewLabeling(0)
, m_updateLabelingDataCosts(0)
, m_giveSmoothEnergyInternal(0)
, m_solveSpecialCases(&GCoptimization::solveSpecialCases<DataCostFnFromArray>)
, m_datacostFnDelete(0)
, m_smoothcostFnDelete(0)
, m_random_label_order(false)
, m_verbosity(0)
, m_labelingInfoDirty(true)
, m_lookupSiteVar(new SiteID[nSites])
, m_labeling(new LabelID[nSites])
, m_labelTable(new LabelID[nLabels])
, m_labelingDataCosts(new EnergyTermType[nSites])
, m_labelCounts(new SiteID[nLabels])
, m_activeLabelCounts(new SiteID[m_num_labels])
, m_stepsThisCycle(0)
, m_stepsThisCycleTotal(0)
{
        if ( nLabels <= 1 ) handleError("Number of labels must be >= 2");
        if ( nSites <= 0 )  handleError("Number of sites must be >= 1");
        
        if ( !m_lookupSiteVar || !m_labelTable || !m_labeling ){
                if (m_lookupSiteVar) delete [] m_lookupSiteVar;
                if (m_labelTable) delete [] m_labelTable;
                if (m_labeling) delete [] m_labeling;
                if (m_labelingDataCosts) delete [] m_labelingDataCosts;
                if (m_labelCounts) delete [] m_labelCounts;
                handleError("Not enough memory.");
        }
        
        memset(m_labeling, 0, m_num_sites*sizeof(LabelID));
        memset(m_lookupSiteVar,-1,m_num_sites*sizeof(SiteID));
        setLabelOrder(false);
        specializeSmoothCostFunctor(SmoothCostFnPotts());
}

//-------------------------------------------------------------------

GCoptimization::~GCoptimization()
{
        delete [] m_labelTable;
        delete [] m_lookupSiteVar;
        delete [] m_labeling;
        delete [] m_labelingDataCosts;
        delete [] m_labelCounts;
        delete [] m_activeLabelCounts;

        if (m_datacostFnDelete) m_datacostFnDelete(m_datacostFn);
        if (m_smoothcostFnDelete) m_smoothcostFnDelete(m_smoothcostFn);

        if (m_datacostIndividual) delete [] m_datacostIndividual;
        if (m_smoothcostIndividual) delete [] m_smoothcostIndividual;

        // Delete label cost bookkeeping structures
        //
        slist_clear(m_labelcostsAll);
        if (m_labelcostsByLabel) {
                for ( LabelID i = 0; i < m_num_labels; ++i )
                        slist_clear(m_labelcostsByLabel[i]);
                delete [] m_labelcostsByLabel;
        }
}

//-------------------------------------------------------------------

template <>
GCoptimization::SiteID GCoptimization::queryActiveSitesExpansion<GCoptimization::DataCostFnSparse>(LabelID alpha_label,SiteID *activeSites)
{
        return ((DataCostFnSparse*)m_datacostFn)->queryActiveSitesExpansion(alpha_label,m_labeling,activeSites);
}

//-------------------------------------------------------------------

template <>
void GCoptimization::setupDataCostsExpansion<GCoptimization::DataCostFnSparse>(SiteID size,LabelID alpha_label,EnergyT *e,SiteID *activeSites)
{
        DataCostFnSparse* dc = (DataCostFnSparse*)m_datacostFn;
        DataCostFnSparse::iterator dciter = dc->begin(alpha_label);
        for ( SiteID i = 0; i < size; ++i )
        {
                SiteID site = activeSites[i];
                while ( dciter.site() != site )
                        ++dciter;
                addterm1_checked(e,i,dciter.cost(),m_labelingDataCosts[site]);
        }
}

//-------------------------------------------------------------------

template <>
void GCoptimization::applyNewLabeling<GCoptimization::DataCostFnSparse>(EnergyT *e,SiteID *activeSites,SiteID size,LabelID alpha_label)
{
        DataCostFnSparse* dc = (DataCostFnSparse*)m_datacostFn;
        DataCostFnSparse::iterator dciter = dc->begin(alpha_label);
        for ( SiteID i = 0; i < size; i++ )
        {
                if ( e->get_var(i) == 0 )
                {
                        SiteID site = activeSites[i];
                        LabelID prev = m_labeling[site];
                        m_labeling[site] = alpha_label;
                        m_labelCounts[alpha_label]++;
                        m_labelCounts[prev]--;
                        while ( dciter.site() != site )
                                ++dciter;
                        m_labelingDataCosts[site] = dciter.cost();
                }
        }
        m_labelingInfoDirty = true;
        updateLabelingInfo(false,true,false); // labels have changed, so update necessary labeling info
}

//-------------------------------------------------------------------

template <typename UserFunctor>
void GCoptimization::specializeDataCostFunctor(const UserFunctor f) {
        if ( m_datacostFnDelete )
                m_datacostFnDelete(m_datacostFn);
        if ( m_datacostIndividual )
        {
                delete [] m_datacostIndividual;
                m_datacostIndividual = 0;
        }
        m_datacostFn = new UserFunctor(f);
        m_datacostFnDelete          = &GCoptimization::deleteFunctor<UserFunctor>;
        m_queryActiveSitesExpansion = &GCoptimization::queryActiveSitesExpansion<UserFunctor>;
        m_setupDataCostsExpansion   = &GCoptimization::setupDataCostsExpansion<UserFunctor>;
        m_setupDataCostsSwap        = &GCoptimization::setupDataCostsSwap<UserFunctor>;
        m_applyNewLabeling          = &GCoptimization::applyNewLabeling<UserFunctor>;
        m_updateLabelingDataCosts   = &GCoptimization::updateLabelingDataCosts<UserFunctor>;
        m_solveSpecialCases         = &GCoptimization::solveSpecialCases<UserFunctor>;
}

template <typename UserFunctor>
void GCoptimization::specializeSmoothCostFunctor(const UserFunctor f) {
        if ( m_smoothcostFnDelete )
                m_smoothcostFnDelete(m_smoothcostFn);
        if ( m_smoothcostIndividual )
        {
                delete [] m_smoothcostIndividual;
                m_smoothcostIndividual = 0;
        }
        m_smoothcostFn = new UserFunctor(f);
        m_smoothcostFnDelete        = &GCoptimization::deleteFunctor<UserFunctor>;
        m_giveSmoothEnergyInternal  = &GCoptimization::giveSmoothEnergyInternal<UserFunctor>;
        m_setupSmoothCostsExpansion = &GCoptimization::setupSmoothCostsExpansion<UserFunctor>;
        m_setupSmoothCostsSwap      = &GCoptimization::setupSmoothCostsSwap<UserFunctor>;
}

//-------------------------------------------------------------------

template <typename SmoothCostT>
GCoptimization::EnergyType GCoptimization::giveSmoothEnergyInternal()
{
        EnergyType eng = (EnergyType) 0;
        SiteID i,numN,*nPointer,nSite,n;
        EnergyTermType *weights;
        SmoothCostT* sc = (SmoothCostT*) m_smoothcostFn;
        for ( i = 0; i < m_num_sites; i++ )
        {
                giveNeighborInfo(i,&numN,&nPointer,&weights);
                for ( n = 0; n < numN; n++ )
                {
                        nSite = nPointer[n];
                        if ( nSite < i ) 
                                eng += weights[n]*(sc->compute(i,nSite,m_labeling[i],m_labeling[nSite]));
                }
        }

        return eng;
}

//-------------------------------------------------------------------

OLGA_INLINE void GCoptimization::addterm1_checked(EnergyT* e, VarID i, EnergyTermType e0, EnergyTermType e1)
{
        if ( e0 > GCO_MAX_ENERGYTERM || e1 > GCO_MAX_ENERGYTERM )
                handleError("Data cost term was larger than GCO_MAX_ENERGYTERM; danger of integer overflow.");
        m_beforeExpansionEnergy += e1;
        e->add_term1(i,e0,e1);
}

OLGA_INLINE void GCoptimization::addterm1_checked(EnergyT* e, VarID i, EnergyTermType e0, EnergyTermType e1, EnergyTermType w)
{
        if ( e0 > GCO_MAX_ENERGYTERM || e1 > GCO_MAX_ENERGYTERM )
                handleError("Smooth cost term was larger than GCO_MAX_ENERGYTERM; danger of integer overflow.");
        if ( w > GCO_MAX_ENERGYTERM )
                handleError("Smoothness weight was larger than GCO_MAX_ENERGYTERM; danger of integer overflow.");
        m_beforeExpansionEnergy += e1*w;
        e->add_term1(i,e0*w,e1*w);
}

OLGA_INLINE void GCoptimization::addterm2_checked(EnergyT* e, VarID i, VarID j, EnergyTermType e00, EnergyTermType e01, EnergyTermType e10, EnergyTermType e11, EnergyTermType w)
{
        if ( e00 > GCO_MAX_ENERGYTERM || e11 > GCO_MAX_ENERGYTERM || e01 > GCO_MAX_ENERGYTERM || e10 > GCO_MAX_ENERGYTERM )
                handleError("Smooth cost term was larger than GCO_MAX_ENERGYTERM; danger of integer overflow.");
        if ( w > GCO_MAX_ENERGYTERM )
                handleError("Smoothness weight was larger than GCO_MAX_ENERGYTERM; danger of integer overflow.");
        // Inside energy/maxflow code the submodularity check is performed as an assertion,
        // but is optimized out. We check it in release builds as well.
        if ( e00+e11 > e01+e10 )
                handleError("Non-submodular expansion term detected; smooth costs must be a metric for expansion");
        m_beforeExpansionEnergy += e11*w;
        e->add_term2(i,j,e00*w,e01*w,e10*w,e11*w);
}

//------------------------------------------------------------------

template <typename DataCostT>
GCoptimization::SiteID GCoptimization::queryActiveSitesExpansion(LabelID alpha_label,SiteID *activeSites)
{
        SiteID size = 0;
        for ( SiteID i = 0; i < m_num_sites; i++ )
                if ( m_labeling[i] != alpha_label )
                        activeSites[size++] = i;
        return size;
}

//-------------------------------------------------------------------

template <typename DataCostT>
void GCoptimization::setupDataCostsExpansion(SiteID size,LabelID alpha_label,EnergyT *e,SiteID *activeSites)
{
        DataCostT* dc = (DataCostT*)m_datacostFn;
        for ( SiteID i = 0; i < size; ++i )
                addterm1_checked(e,i,dc->compute(activeSites[i],alpha_label),m_labelingDataCosts[activeSites[i]]);
}

//-------------------------------------------------------------------

template <typename SmoothCostT>
void GCoptimization::setupSmoothCostsExpansion(SiteID size,LabelID alpha_label,EnergyT *e,SiteID *activeSites)
{
        SiteID i,nSite,site,n,nNum,*nPointer;
        EnergyTermType *weights;
        SmoothCostT* sc = (SmoothCostT*)m_smoothcostFn;

        for ( i = size - 1; i >= 0; i-- )
        {
                site = activeSites[i];
                giveNeighborInfo(site,&nNum,&nPointer,&weights);
                for ( n = 0; n < nNum; n++ )
                {
                        nSite = nPointer[n];
                        if ( m_lookupSiteVar[nSite] == -1 ) 
                                addterm1_checked(e,i,sc->compute(site,nSite,alpha_label,m_labeling[nSite]),
                                                     sc->compute(site,nSite,m_labeling[site],m_labeling[nSite]),weights[n]);
                        else if ( nSite < site ) 
                        {
                                addterm2_checked(e,i,m_lookupSiteVar[nSite],
                                                 sc->compute(site,nSite,alpha_label,alpha_label),
                                                 sc->compute(site,nSite,alpha_label,m_labeling[nSite]),
                                                 sc->compute(site,nSite,m_labeling[site],alpha_label),
                                                 sc->compute(site,nSite,m_labeling[site],m_labeling[nSite]),weights[n]);
                        }
                }
        }
}

//-----------------------------------------------------------------------------------

template <typename DataCostT>
void GCoptimization::setupDataCostsSwap(SiteID size, LabelID alpha_label, LabelID beta_label,
                                                                                 EnergyT *e,SiteID *activeSites )
{
        DataCostT* dc = (DataCostT*)m_datacostFn;
        for ( SiteID i = 0; i < size; i++ )
        {
                e->add_term1(i,dc->compute(activeSites[i],alpha_label),
                               dc->compute(activeSites[i],beta_label) );
        }
}

//-------------------------------------------------------------------

template <typename SmoothCostT>
void GCoptimization::setupSmoothCostsSwap(SiteID size, LabelID alpha_label,LabelID beta_label,
                                                                                 EnergyT *e,SiteID *activeSites )
{
        SiteID i,nSite,site,n,nNum,*nPointer;
        EnergyTermType *weights;
        SmoothCostT* sc = (SmoothCostT*)m_smoothcostFn;

        for ( i = size - 1; i >= 0; i-- )
        {
                site = activeSites[i];
                giveNeighborInfo(site,&nNum,&nPointer,&weights);
                for ( n = 0; n < nNum; n++ )
                {
                        nSite = nPointer[n];
                        if ( m_lookupSiteVar[nSite] == -1 )
                                addterm1_checked(e,i,sc->compute(site,nSite,alpha_label,m_labeling[nSite]),
                                                     sc->compute(site,nSite,beta_label, m_labeling[nSite]),weights[n]);
                        else if ( nSite < site )
                        {
                                addterm2_checked(e,i,m_lookupSiteVar[nSite],
                                                 sc->compute(site,nSite,alpha_label,alpha_label),
                                                 sc->compute(site,nSite,alpha_label,beta_label),
                                                 sc->compute(site,nSite,beta_label,alpha_label),
                                                 sc->compute(site,nSite,beta_label,beta_label),weights[n]);
                        }
                }
        }
}

//-----------------------------------------------------------------------------------

template <typename DataCostT>
void GCoptimization::applyNewLabeling(EnergyT *e,SiteID *activeSites,SiteID size,LabelID alpha_label)
{
        DataCostT* dc = (DataCostT*)m_datacostFn;
        for ( SiteID i = 0; i < size; i++ )
        {
                if ( e->get_var(i) == 0 )
                {
                        SiteID site = activeSites[i];
                        LabelID prev = m_labeling[site];
                        m_labeling[site] = alpha_label;
                        m_labelCounts[alpha_label]++;
                        m_labelCounts[prev]--;
                        m_labelingDataCosts[site] = dc->compute(site,alpha_label);
                }
        }
        m_labelingInfoDirty = true;
        updateLabelingInfo(false,true,false); // labels have changed, so update necessary labeling info
}

//-----------------------------------------------------------------------------------

template <typename DataCostT>
void GCoptimization::updateLabelingDataCosts()
{
        DataCostT* dc = (DataCostT*)m_datacostFn;
        for (int i = 0; i < m_num_sites; ++i)
                m_labelingDataCosts[i] = dc->compute(i,m_labeling[i]);
}

//-----------------------------------------------------------------------------------

template <typename DataCostT>
bool GCoptimization::solveSpecialCases(EnergyType& energy)
{
        finalizeNeighbors();

        DataCostT* dc = (DataCostT*)m_datacostFn;
        bool sc = m_numNeighborsTotal != 0;
        bool lc = m_labelcostsAll != 0;

        if ( !dc && !sc && !lc )
        {
                energy = 0;
                return true;
        }

        if ( dc && !sc && !lc ) {
                // Special case: No label costs, so return trivial solution
                energy = 0;
                for ( SiteID i = 0; i < m_num_sites; ++i ) {
                        LabelID minCostLabel = 0;
                        EnergyTermType minCost = dc->compute(i, 0);
                        for ( LabelID l = 1; l < m_num_labels; ++l ) {
                                EnergyTermType lcost = dc->compute(i, l);
                                if ( lcost < minCost ) {
                                        minCostLabel = l;
                                        minCost = lcost;
                                }
                        }
                        if ( minCostLabel > GCO_MAX_ENERGYTERM )
                                handleError("Data cost was larger than GCO_MAX_ENERGYTERM; danger of integer overflow.");
                        m_labeling[i] = minCostLabel;
                        energy += minCost;
                }
                m_labelingInfoDirty = true;
                updateLabelingInfo();
                return true;
        }

        if ( !dc && !sc && lc ) {
                // Special case: No data costs, so return trivial solution
                LabelID minLabel = 0;
                EnergyType minLabelCost = GCO_MAX_ENERGYTERM*(EnergyType)m_num_labels;
                for ( LabelID l = 0; l < m_num_labels; ++l ) {
                        EnergyType lcsum = 0;
                        for ( LabelCostIter* lci = m_labelcostsByLabel[l]; lci; lci = lci->next )
                                lcsum += lci->node->cost;
                        if ( lcsum < minLabelCost ) {
                                minLabel = l;
                                minLabelCost = lcsum;
                        }
                }
                for ( SiteID i = 0; i < m_num_sites; ++i )
                        m_labeling[i] = minLabel;
                energy = minLabelCost;
                m_labelingInfoDirty = true;
                updateLabelingInfo();
                return true;
        }

        if ( dc && !sc && lc ) {
                LabelCost* lc;
                for ( lc = m_labelcostsAll; lc; lc = lc->next )
                        if ( lc->numLabels > 1)
                                break;
                if ( !lc ) {
                        // Special case: Data costs and per-label costs 
                        energy = solveGreedy<DataCostT>();
                        return true;
                }
        }

        // Otherwise, use full-blown expansion/swap
        return false;
}

template <>
class GCoptimization::GreedyIter<GCoptimization::DataCostFnSparse> {
public:
        GreedyIter(DataCostFnSparse& dc, SiteID)
        : m_dc(dc), m_label(0), m_labelend(0)
        { }

        OLGA_INLINE void start(const LabelID* labels, LabelID labelCount=1)
        {
                m_label = labels;
                m_labelend = labels + labelCount;
                if (labelCount > 0) {
                        m_site = m_dc.begin(*labels);
                        m_siteend = m_dc.end(*labels);
                        while (m_site == m_siteend) {
                                if (++m_label == m_labelend)
                                        break;
                                m_site     = m_dc.begin(*m_label);
                                m_siteend  = m_dc.end(*m_label);
                        }
                }
        }
        OLGA_INLINE SiteID site()  const { return m_site.site(); }
        OLGA_INLINE SiteID label() const { return *m_label; }
        OLGA_INLINE bool   done()  const { return m_label >= m_labelend; }
        OLGA_INLINE GreedyIter& operator++() 
        {
                // The inner loop is over sites, not labels, because sparse data costs 
                // are stored as consecutive [sparse] SiteIDs with respect to each label.
                if (++m_site == m_siteend) {
                        while (++m_label < m_labelend) {
                                m_site     = m_dc.begin(*m_label);
                                m_siteend  = m_dc.end(*m_label);
                                if (m_site != m_siteend)
                                        break;
                        }
                }
                return *this;
        }
        OLGA_INLINE EnergyTermType compute() const { return m_site.cost(); }
        OLGA_INLINE SiteID feasibleSites() const { return (SiteID)(m_siteend - m_site); }

private:
        DataCostFnSparse::iterator m_site;
        DataCostFnSparse::iterator m_siteend;
        DataCostFnSparse& m_dc;
        const LabelID* m_label;
        const LabelID* m_labelend;
};

template <typename DataCostT>
GCoptimization::EnergyType GCoptimization::solveGreedy()
{
        printStatus1("starting greedy algorithm (1 cycle only)");
        m_stepsThisCycle = m_stepsThisCycleTotal = 0;

        EnergyType estart = compute_energy();
        EnergyType efinal = 0;
        LabelID* oldLabeling = m_labeling;
        m_labeling = new LabelID[m_num_sites];
        EnergyType* e = new EnergyType[m_num_labels];
        LabelID* order = new LabelID[m_num_labels];  // order[0..activeCount-1] contains the activated labels so far

        try {
                gcoclock_t ticks0all = gcoclock();
                gcoclock_t ticks0 = gcoclock();

                // clear active flags
                for ( LabelCost* lc = m_labelcostsAll; lc; lc = lc->next)
                        lc->active = false;

                DataCostT* dc = (DataCostT*)m_datacostFn;
                GreedyIter<DataCostT> iter(*dc,m_num_sites);
                LabelID alpha = 0;

                // Treat first iteration as special case. 
                // Ignore current labeling and just find the greedy initial label.
                for ( LabelID l = 0; l < m_num_labels; ++l ) {
                        e[l] = 0;
                        for ( LabelCostIter* lci = m_labelcostsByLabel[l]; lci; lci = lci->next )
                                e[l] += lci->node->cost;
                        iter.start(&l);
                        e[l] += (EnergyType)(m_num_sites - iter.feasibleSites()) * GCO_MAX_ENERGYTERM; // pre-add GCO_MAX_ENERGYTERM for all infeasible sites
                        for (; !iter.done(); ++iter) {
                                EnergyTermType dataCost = iter.compute();
                                if ( dataCost > GCO_MAX_ENERGYTERM )
                                        handleError("Data cost was larger than GCO_MAX_ENERGYTERM; danger of integer overflow.");
                                e[l] += dataCost;
                                if ( e[l] > e[alpha] ) // break out early if this will definitely 
                                        break;             // not be a good label to start from
                        }
                        if ( e[l] < e[alpha] ) // choose alpha with minimum energy e[alpha]
                                alpha = l;
                }
                for ( SiteID i = 0; i < m_num_sites; ++i ) {
                        m_labeling[i] = alpha;
                        m_labelingDataCosts[i] = dc->compute(i,alpha);
                }
                for ( LabelCostIter* lci = m_labelcostsByLabel[alpha]; lci; lci = lci->next )
                        lci->node->active = true;

                // List of labels in the order that they were expanded upon (order[0] first, order[1] second, ...)
                for ( LabelID l = 0; l < m_num_labels; ++l )
                        order[l] = l;
                order[alpha] = 0;
                order[0] = alpha;

                printStatus2(alpha,-1,m_num_sites,ticks0);

                // Greedily expand remaining labels
                for ( LabelID alpha_count = 1; alpha_count <= m_num_labels; ++alpha_count) {
                        checkInterrupt();
                        ticks0 = gcoclock();

                        // Energy e[l] for expanding on label 'l' starts at e[alpha] + new labelcosts for introducing l
                        LabelID alpha_prev = alpha;
                        for ( LabelID li = alpha_count; li < m_num_labels; ++li ) {
                                LabelID l = order[li];
                                e[l] = e[alpha_prev];
                                for ( LabelCostIter* lci = m_labelcostsByLabel[l]; lci; lci = lci->next )
                                        if ( !lci->node->active )
                                                e[l] += lci->node->cost;
                        }

                        // Loop over all sites and all remaining labels to calculate energy drop.
                        for ( iter.start(&order[alpha_count],m_num_labels-alpha_count); !iter.done(); ++iter ) {
                                EnergyTermType dc_l = iter.compute();
                                EnergyTermType dc_i = m_labelingDataCosts[iter.site()];
                                EnergyTermType delta_i = dc_l - dc_i;
                                if ( delta_i < 0 )
                                        e[iter.label()] += delta_i;
                        }

                        // Choose the next alpha based on lowest resulting energy
                        LabelID alpha_index = alpha_count-1;
                        for ( LabelID li = alpha_count; li < m_num_labels; ++li ) {
                                LabelID l = order[li];
                                if ( e[l] < e[alpha] ) {
                                        alpha = l;
                                        alpha_index = li;
                                }
                        }

                        if ( alpha == alpha_prev )
                                break;

                        // Append alpha to the list of activated labels
                        LabelID temp = order[alpha_count];
                        order[alpha_count] = order[alpha_index];
                        order[alpha_index] = temp;

                        // Apply the new labeling, updating m_labelingDataCosts and active labelcosts as necessary
                        iter.start(&alpha);
                        SiteID size = iter.feasibleSites();
                        for ( ; !iter.done(); ++iter ) {
                                EnergyTermType dc_l = iter.compute();
                                EnergyTermType dc_i = m_labelingDataCosts[iter.site()];
                                EnergyTermType delta_i = dc_l - dc_i;
                                if ( delta_i < 0 ) {
                                        m_labeling[iter.site()] = alpha;
                                        m_labelingDataCosts[iter.site()] = dc_l;
                                }
                        }
                        for ( LabelCostIter* lci = m_labelcostsByLabel[alpha]; lci; lci = lci->next )
                                lci->node->active = true;
                        printStatus2(alpha,-1,size,ticks0);
                }

                efinal = e[alpha];
                if ( efinal < estart ) {
                        // Greedy succeeded in lowering energy compared to initial labeling
                        delete [] oldLabeling;
                        m_labelingInfoDirty = true;
                        updateLabelingInfo(true,false,false); // update m_labelCounts only; m_labelingDataCosts and active labelcosts should be up to date
                        printStatus1(1,false,ticks0all);
                } else {
                        // Greedy failed to find a lower energy, so revert everything
                        efinal = estart;
                        delete [] m_labeling;
                        m_labeling = oldLabeling;
                        m_labelingInfoDirty = true;
                        updateLabelingInfo(); // put all labeling info back the way it was
                        printStatus1(1,false,ticks0all);
                }

                delete [] order;
                delete [] e;
        } catch (...) {
                delete [] order;
                delete [] e;
                throw;
        }
        return efinal;
}

//------------------------------------------------------------------

void GCoptimization::setDataCost(DataCostFn fn) { 
        specializeDataCostFunctor(DataCostFnFromFunction(fn));
        m_labelingInfoDirty = true;
}


//------------------------------------------------------------------

void GCoptimization::setDataCost(DataCostFnExtra fn, void *extraData) { 
        specializeDataCostFunctor(DataCostFnFromFunctionExtra(fn, extraData));
        m_labelingInfoDirty = true;
}

//-------------------------------------------------------------------

void GCoptimization::setDataCost(EnergyTermType *dataArray) {
        specializeDataCostFunctor(DataCostFnFromArray(dataArray, m_num_labels));
        m_labelingInfoDirty = true;
}

//-------------------------------------------------------------------

void GCoptimization::setDataCost(SiteID s, LabelID l, EnergyTermType e) {
        if ( !m_datacostIndividual )
        {
                EnergyTermType* table = new EnergyTermType[m_num_sites*m_num_labels];
                memset(table, 0, m_num_sites*m_num_labels*sizeof(EnergyTermType));
                specializeDataCostFunctor(DataCostFnFromArray(table, m_num_labels));
                m_datacostIndividual = table;
                m_labelingInfoDirty = true;
        }
        m_datacostIndividual[s*m_num_labels + l] = e;
        if ( m_labeling[s] == l )
                m_labelingInfoDirty = true; // m_labelingDataCosts is dirty
}

//-------------------------------------------------------------------

void GCoptimization::setDataCostFunctor(DataCostFunctor* f) {
        if ( m_datacostFnDelete )
                m_datacostFnDelete(m_datacostFn);
        if ( m_datacostIndividual )
        {
                delete [] m_datacostIndividual;
                m_datacostIndividual = 0;
        }
        m_datacostFn = f;
        m_datacostFnDelete          = 0;
        m_queryActiveSitesExpansion = &GCoptimization::queryActiveSitesExpansion<DataCostFunctor>;
        m_setupDataCostsExpansion   = &GCoptimization::setupDataCostsExpansion<DataCostFunctor>;
        m_setupDataCostsSwap        = &GCoptimization::setupDataCostsSwap<DataCostFunctor>;
        m_applyNewLabeling          = &GCoptimization::applyNewLabeling<DataCostFunctor>;
        m_updateLabelingDataCosts   = &GCoptimization::updateLabelingDataCosts<DataCostFunctor>;
        m_solveSpecialCases         = &GCoptimization::solveSpecialCases<DataCostFunctor>;
        m_labelingInfoDirty = true;
}

//-------------------------------------------------------------------

void GCoptimization::setDataCost(LabelID l, SparseDataCost *costs, SiteID count)
{
        if ( !m_datacostFn )
                specializeDataCostFunctor(DataCostFnSparse(numSites(),numLabels()));
        else if ( m_queryActiveSitesExpansion != (SiteID (GCoptimization::*)(LabelID,SiteID*))&GCoptimization::queryActiveSitesExpansion<DataCostFnSparse> )
                handleError("Cannot apply sparse data costs after dense data costs have been used.");
        m_labelingInfoDirty = true;
        DataCostFnSparse* dc = (DataCostFnSparse*)m_datacostFn;
        dc->set(l,costs,count);
}

//-------------------------------------------------------------------

void GCoptimization::setSmoothCost(SmoothCostFn fn) {
        specializeSmoothCostFunctor(SmoothCostFnFromFunction(fn));
}

//-------------------------------------------------------------------

void GCoptimization::setSmoothCost(SmoothCostFnExtra fn, void* extraData) {
        specializeSmoothCostFunctor(SmoothCostFnFromFunctionExtra(fn, extraData));
}

//-------------------------------------------------------------------

void GCoptimization::setSmoothCost(EnergyTermType *smoothArray) {
        specializeSmoothCostFunctor(SmoothCostFnFromArray(smoothArray, m_num_labels));
}

//-------------------------------------------------------------------

void GCoptimization::setSmoothCost(LabelID l1, LabelID l2, EnergyTermType e){
        if ( !m_smoothcostIndividual )
        {
                EnergyTermType* table = new EnergyTermType[m_num_labels*m_num_labels];
                memset(table, 0, m_num_labels*m_num_labels*sizeof(EnergyTermType));
                specializeSmoothCostFunctor(SmoothCostFnFromArray(table, m_num_labels));
                m_smoothcostIndividual = table;
        } 
        m_smoothcostIndividual[l1*m_num_labels + l2] = e;
}

//-------------------------------------------------------------------

void GCoptimization::setSmoothCostFunctor(SmoothCostFunctor* f) {
        if ( m_smoothcostFnDelete )
                m_smoothcostFnDelete(m_smoothcostFn);
        if ( m_smoothcostIndividual )
        {
                delete [] m_smoothcostIndividual;
                m_smoothcostIndividual = 0;
        }
        m_smoothcostFn = f;
        m_smoothcostFnDelete        = 0;
        m_giveSmoothEnergyInternal  = &GCoptimization::giveSmoothEnergyInternal<SmoothCostFunctor>;
        m_setupSmoothCostsExpansion = &GCoptimization::setupSmoothCostsExpansion<SmoothCostFunctor>;
        m_setupSmoothCostsSwap      = &GCoptimization::setupSmoothCostsSwap<SmoothCostFunctor>;
}

//-------------------------------------------------------------------

void GCoptimization::setLabelCost(EnergyTermType cost) 
{
        EnergyTermType* lc = new EnergyTermType[m_num_labels];
        for ( LabelID i = 0; i < m_num_labels; ++i )
                lc[i] = cost;
        setLabelCost(lc);
        delete [] lc;
}

//-------------------------------------------------------------------

void GCoptimization::setLabelCost(EnergyTermType *costArray) 
{
        for ( LabelID i = 0; i < m_num_labels; ++i )
                setLabelSubsetCost(&i, 1, costArray[i]);
}

//-------------------------------------------------------------------

void GCoptimization::setLabelSubsetCost(LabelID* labels, LabelID numLabels, EnergyTermType cost)
{
        if ( cost < 0 )
                handleError("Label costs must be non-negative.");
        if ( cost > GCO_MAX_ENERGYTERM )
                handleError("Label cost was larger than GCO_MAX_ENERGYTERM; danger of integer overflow.");
        for ( LabelID i = 0; i < numLabels; ++i)
                if ( labels[i] < 0 || labels[i] >= m_num_labels )
                        handleError("Invalid label id was found in label subset list.");

        if ( !m_labelcostsByLabel ) {
                m_labelcostsByLabel = new LabelCostIter*[m_num_labels];
                memset(m_labelcostsByLabel, 0, m_num_labels*sizeof(void*));
        }

        // If this particular subset already has a cost, simply replace it.
        for ( LabelCostIter* lci = m_labelcostsByLabel[labels[0]]; lci; lci = lci->next ) {
                if ( numLabels == lci->node->numLabels ) {
                        if ( !memcmp(labels, lci->node->labels, numLabels*sizeof(LabelID)) ) {
                                // This label subset already exists, so just update the cost and return
                                lci->node->cost = cost;
                                return;
                        }
                }
        }
        
        if (cost == 0)
                return;

        // Create a new LabelCost entry and add it to the appropriate lists
        m_labelcostCount++;
        LabelCost* lc = new LabelCost;
        lc->cost = cost; 
        lc->active = false;
        lc->aux = -1;
        lc->numLabels = numLabels;
        lc->labels = new LabelID[numLabels];
        memcpy(lc->labels, labels, numLabels*sizeof(LabelID));
        slist_prepend(m_labelcostsAll, lc);
        for ( LabelID i = 0; i < numLabels; ++i ) {
                LabelCostIter* lci = new LabelCostIter;
                lci->node = lc; 
                slist_prepend(m_labelcostsByLabel[labels[i]], lci);
        }
}

//-------------------------------------------------------------------

void GCoptimization::whatLabel(SiteID start, SiteID count, LabelID* labeling)
{
        assert(start >= 0 && start+count <= m_num_sites);
        memcpy(labeling, m_labeling+start, count*sizeof(LabelID));
}

//-------------------------------------------------------------------

GCoptimization::EnergyType GCoptimization::giveSmoothEnergy()
{
        finalizeNeighbors();
        if ( m_giveSmoothEnergyInternal ) 
                return( (this->*m_giveSmoothEnergyInternal)());
        return 0;
}

//-------------------------------------------------------------------

GCoptimization::EnergyType GCoptimization::giveDataEnergy()
{
        updateLabelingInfo();
        EnergyType energy = 0;
        for ( SiteID i = 0; i < m_num_sites; i++ )
                energy += m_labelingDataCosts[i];
        return energy;
}

GCoptimization::EnergyType GCoptimization::giveLabelEnergy()
{
        updateLabelingInfo();
        EnergyType energy = 0;
        for ( LabelCost* lc = m_labelcostsAll; lc; lc = lc->next)
                if ( lc->active )
                        energy += lc->cost;
        return energy;
}

//-------------------------------------------------------------------
 
GCoptimization::EnergyType GCoptimization::compute_energy()
{
        return giveDataEnergy() + giveSmoothEnergy() + giveLabelEnergy();
}

//-------------------------------------------------------------------

void GCoptimization::permuteLabelTable()
{
        if ( !m_random_label_order )
                return;
        for ( LabelID i = 0; i < m_num_labels; i++ )
        {
                LabelID j = i + (rand() % (m_num_labels-i));
                LabelID temp    = m_labelTable[i];
                m_labelTable[i] = m_labelTable[j];
                m_labelTable[j] = temp;
        }
}

//-------------------------------------------------------------------

GCoptimization::EnergyType GCoptimization::expansion(int max_num_iterations)
{
        EnergyType new_energy, old_energy;
        if ( (this->*m_solveSpecialCases)(new_energy) )
                return new_energy;

        permuteLabelTable();
        updateLabelingInfo();

        try 
        {
                if ( max_num_iterations == -1 )
                {
                        // Strategic expansion loop focuses on labels that successfuly reduced the energy
                        printStatus1("starting alpha-expansion w/ adaptive cycles");
                        std::vector<LabelID> queueSizes;
                        queueSizes.push_back(m_num_labels);

                        int cycle = 1;
                        LabelID next = 0;
                        do
                        {
                                gcoclock_t ticks0 = gcoclock();
                                m_stepsThisCycle = 0; 

                                // Make a pass over the unchecked labels in the current queue, i.e. m_labelTable[next..queueSize-1]
                                LabelID queueSize = queueSizes.back();
                                LabelID start = next;
                                m_stepsThisCycleTotal = queueSize - start;
                                do 
                                {
                                        if ( !alpha_expansion(m_labelTable[next]) )
                                                std::swap(m_labelTable[next],m_labelTable[--queueSize]); // don't put this label in a new queue
                                        else
                                                ++next; // keep this label for the next (smaller) queue
                                        m_stepsThisCycle++;
                                } while ( next < queueSize );

                                if ( next == start )  // No expansion was successful, so try more labels from the previous queue
                                {
                                        next = queueSizes.back();
                                        queueSizes.pop_back();
                                }
                                else if ( queueSize < queueSizes.back()/2 ) // Some expansions were successful, so focus on them in a new queue
                                {
                                        next = 0;
                                        queueSizes.push_back(queueSize);
                                }
                                else
                                        next = 0;  // All expansions were successful, so do another complete sweep
                                
                                printStatus1(cycle++,false,ticks0);
                        } while ( !queueSizes.empty() );
                        new_energy = compute_energy();
                }
                else
                {
                        // Standard expansion loop sweeps over all labels each cycle
                        printStatus1("starting alpha-expansion w/ standard cycles");
                        new_energy = compute_energy();
                        old_energy = new_energy+1;
                        for ( int cycle = 1; cycle <= max_num_iterations; cycle++ )
                        {
                                gcoclock_t ticks0 = gcoclock();
                                old_energy = new_energy;
                                new_energy = oneExpansionIteration();
                                printStatus1(cycle,false,ticks0);
                                if ( new_energy == old_energy )
                                        break;
                                permuteLabelTable();
                        }
                }
        } 
        catch (...)
        {
                m_stepsThisCycle = m_stepsThisCycleTotal = 0;
                throw;
        }
        m_stepsThisCycle = m_stepsThisCycleTotal = 0; // set so that alpha_expansion() knows it's no inside expansion() if called externally
        return new_energy;
}

//-------------------------------------------------------------------

void GCoptimization::setLabelOrder(bool isRandom)
{
        m_random_label_order = isRandom;
        for ( LabelID i = 0; i < m_num_labels; i++ )
                m_labelTable[i] = i;
}

//-------------------------------------------------------------------

void GCoptimization::setLabelOrder(const LabelID* order, LabelID size)
{
        if ( size > m_num_labels )
                handleError("setLabelOrder receieved too many labels");
        for ( LabelID i = 0; i < size; ++i )
                if ( order[i] < 0 || order[i] >= m_num_labels )
                        handleError("Invalid label id in setLabelOrder");
        m_random_label_order = false;
        memcpy(m_labelTable,order,size*sizeof(LabelID));
        memset(m_labelTable+size,-1,(m_num_labels-size)*sizeof(LabelID));
}

//------------------------------------------------------------------

void GCoptimization::handleError(const char *message)
{
        throw GCException(message);
}

//------------------------------------------------------------------

void GCoptimization::checkInterrupt()
{
        if ( utIsInterruptPending() )
                throw GCException("Interrupted.");
}


//-------------------------------------------------------------------//
//                  METHODS for EXPANSION MOVES                      //  
//-------------------------------------------------------------------//

GCoptimization::EnergyType GCoptimization::setupLabelCostsExpansion(SiteID size,LabelID alpha_label,EnergyT *e,SiteID *activeSites)
{
        EnergyType alphaCostCorrection = 0;
        if ( !m_labelcostsAll )
                return alphaCostCorrection;

        const SiteID DISABLE = -2;
        const SiteID UNINIT  = -1;
        for ( LabelCost* lc = m_labelcostsAll; lc; lc = lc->next )
                lc->aux = UNINIT;

        // Skip higher-order costs that include alpha_label or any label used
        // outside the activeSites, since they cannot be eliminated by the expansion.
        if ( m_queryActiveSitesExpansion == (SiteID (GCoptimization::*)(LabelID,SiteID*))&GCoptimization::queryActiveSitesExpansion<DataCostFnSparse> )
        {
                // For sparse data costs, things are more complicated, because we must ensure that
                // no label cost for a fixed (non-active) non-alpha label is encoded in the graph.
                memset(m_activeLabelCounts,0,m_num_labels*sizeof(SiteID));
                for ( SiteID i = 0; i < size; ++i )
                        m_activeLabelCounts[m_labeling[activeSites[i]]]++;

                for ( LabelID l = 0; l < m_num_labels; ++l )
                {
                        if ( m_activeLabelCounts[l] != m_labelCounts[l] )
                        {
                                for ( LabelCostIter* lcj = m_labelcostsByLabel[l]; lcj; lcj = lcj->next )
                                        lcj->node->aux = DISABLE;
                        }
                }
        }
        for ( LabelCostIter* lci = m_labelcostsByLabel[alpha_label]; lci; lci = lci->next )
                lci->node->aux = DISABLE;

        // Since we're explicitly omitting the alpha_label label costs from the binary energy, 
        // calculate what it would have been, so that we can potentially reject the expansion afterwards.
        if ( !m_labelCounts[alpha_label] )
        {
                for ( LabelCostIter* lci = m_labelcostsByLabel[alpha_label]; lci; lci = lci->next )
                        if ( !lci->node->active )
                                alphaCostCorrection += lci->node->cost;
        }

        // Add edges to the graph, including auxiliary vertices as needed
        for ( SiteID i = 0; i < size; i++ )
        {
                LabelID label_i = m_labeling[activeSites[i]];
                for ( LabelCostIter* lci = m_labelcostsByLabel[label_i]; lci; lci = lci->next ) 
                {
                        LabelCost* lc = lci->node;
                        if ( lc->aux == DISABLE )
                                continue;

                        // Add auxiliary variable if necessary, and add pairwise potential
                        if ( lc->aux == UNINIT ) 
                        {
                                lc->aux = e->add_variable();
                                e->add_term1(lc->aux,0,lc->cost);
                                m_beforeExpansionEnergy += lc->cost;
                        }
                        e->add_term2(i,lc->aux,0,0,lc->cost,0);
                }
        }

        return alphaCostCorrection;
}

//-------------------------------------------------------------------
void GCoptimization::updateLabelingInfo(bool updateCounts, bool updateActive, bool updateCosts)
{
        if ( !m_labelingInfoDirty )
                return;

        m_labelingInfoDirty = false;

        if ( m_labelcostsAll )
        {
                if ( updateCounts )
                {
                        memset(m_labelCounts,0,m_num_labels*sizeof(SiteID));
                        for ( SiteID i = 0; i < m_num_sites; ++i )
                                m_labelCounts[m_labeling[i]]++;
                }

                if ( updateActive )
                {
                        for ( LabelCost* lc = m_labelcostsAll; lc; lc = lc->next )
                                lc->active = false;

                        EnergyType energy = 0;
                        for ( LabelID l = 0; l < m_num_labels; ++l ) 
                                if ( m_labelCounts[l] )
                                        for ( LabelCostIter* lci = m_labelcostsByLabel[l]; lci; lci = lci->next ) 
                                                lci->node->active = true;
                }
        }

        if ( updateCosts )
        {
                if (m_updateLabelingDataCosts)
                        (this->*m_updateLabelingDataCosts)();
                else
                        memset(m_labelingDataCosts,0,m_num_sites*sizeof(EnergyTermType));
        }
}

//-------------------------------------------------------------------
// Sets up the binary expansion energy, optimizes it, and updates the current labeling.
//
bool GCoptimization::alpha_expansion(LabelID alpha_label)
{
        if (alpha_label < 0)
                return false; // label was disabled due to setLabelOrder on subset of labels

        finalizeNeighbors();
        gcoclock_t ticks0 = gcoclock();

        if ( m_stepsThisCycleTotal == 0 )
                m_labelingInfoDirty = true; // if not inside expansion(), assume data cost function could have changed since last expansion
        updateLabelingInfo();

        // Determine list of active sites for this expansion move
        SiteID size = 0;
        SiteID *activeSites = new SiteID[m_num_sites];
        EnergyType afterExpansionEnergy = 0;
        try 
        {
                // Get list of active sites based on alpha and current labeling
                if ( m_queryActiveSitesExpansion )
                        size = (this->*m_queryActiveSitesExpansion)(alpha_label,activeSites);
                if ( size == 0 )  // Nothing to do
                {
                        delete [] activeSites;
                        printStatus2(alpha_label,-1,size,ticks0);
                        return false;
                }

                // Initialise reverse-lookup so that non-active neighbours can be identified
                // while constructing the graph
                for ( SiteID i = 0; i < size; i++ )
                        m_lookupSiteVar[activeSites[i]] = i;

                // Create binary variables for each remaining site, add the data costs,
                // and compute the smooth costs between variables.
                EnergyT e(size+m_labelcostCount, // poor guess at number of pairwise terms needed :(
                                 m_numNeighborsTotal+(m_labelcostCount?size+m_labelcostCount : 0),
                                 (void(*)(char*))handleError);
                e.add_variable(size);
                m_beforeExpansionEnergy = 0;
                if ( m_setupDataCostsExpansion   ) (this->*m_setupDataCostsExpansion  )(size,alpha_label,&e,activeSites);
                if ( m_setupSmoothCostsExpansion ) (this->*m_setupSmoothCostsExpansion)(size,alpha_label,&e,activeSites);
                EnergyType alphaCorrection = setupLabelCostsExpansion(size,alpha_label,&e,activeSites);
                checkInterrupt();
                afterExpansionEnergy = e.minimize() + alphaCorrection;
                checkInterrupt();

                if ( afterExpansionEnergy < m_beforeExpansionEnergy )
                        (this->*m_applyNewLabeling)(&e,activeSites,size,alpha_label);

                for ( SiteID i = 0; i < size; i++ )
                        m_lookupSiteVar[activeSites[i]] = -1; // restore m_lookupSite to all -1s

                printStatus2(alpha_label,-1,size,ticks0);
        } 
        catch (...)
        {
                delete [] activeSites;
                throw;
        }
        delete [] activeSites;
        return afterExpansionEnergy < m_beforeExpansionEnergy;
}

//-------------------------------------------------------------------

GCoptimization::EnergyType GCoptimization::oneExpansionIteration()
{
        permuteLabelTable();
        m_stepsThisCycle = 0;
        m_stepsThisCycleTotal = m_num_labels;

        // Each cycle is exactly one pass over the labels
        for (LabelID next = 0; next < m_num_labels; next++, m_stepsThisCycle++ )
                alpha_expansion(m_labelTable[next]);

        return compute_energy();
}

//-------------------------------------------------------------------//
//                  METHODS for SWAP MOVES                           //  
//-------------------------------------------------------------------//

GCoptimization::EnergyType GCoptimization::swap(int max_num_iterations)
{
        EnergyType new_energy,old_energy;
        if ( (this->*m_solveSpecialCases)(new_energy) )
                return new_energy;
        
        new_energy = compute_energy();
        old_energy = new_energy+1;
        printStatus1("starting alpha/beta-swap");

        if ( max_num_iterations == -1 )
                max_num_iterations = 10000000;
        int curr_cycle = 1;
        m_stepsThisCycleTotal = (m_num_labels*(m_num_labels-1))/2;
        try
        {
                while ( old_energy > new_energy && curr_cycle <= max_num_iterations)
                {
                        gcoclock_t ticks0 = gcoclock();
                        old_energy = new_energy;
                        new_energy = oneSwapIteration();
                        printStatus1(curr_cycle,true,ticks0);
                        curr_cycle++;
                }
        } 
        catch (...)
        {
                m_stepsThisCycle = m_stepsThisCycleTotal = 0;
                throw;
        }
        m_stepsThisCycle = m_stepsThisCycleTotal = 0;

        return(new_energy);
}

//--------------------------------------------------------------------------------

GCoptimization::EnergyType GCoptimization::oneSwapIteration()
{
        LabelID next,next1;
        permuteLabelTable();
        m_stepsThisCycle = 0;

        for (next = 0;  next < m_num_labels;  next++ )
                for (next1 = m_num_labels - 1;  next1 >= 0;  next1-- )
                        if ( m_labelTable[next] < m_labelTable[next1] )
                        {
                                alpha_beta_swap(m_labelTable[next],m_labelTable[next1]); 
                                m_stepsThisCycle++;
                        }

        return(compute_energy());
}

//---------------------------------------------------------------------------------

void GCoptimization::alpha_beta_swap(LabelID alpha_label, LabelID beta_label)
{
        assert( alpha_label >= 0 && alpha_label < m_num_labels && beta_label >= 0 && beta_label < m_num_labels);
        if ( m_labelcostsAll )
                handleError("Label costs only implemented for alpha-expansion.");

        finalizeNeighbors();
        gcoclock_t ticks0 = gcoclock();

        // Determine the list of active sites for this swap move
        SiteID size = 0;
        SiteID *activeSites = new SiteID[m_num_sites];
        try
        {
                for ( SiteID i = 0; i < m_num_sites; i++ )
                {
                        if ( m_labeling[i] == alpha_label || m_labeling[i] == beta_label )
                        {
                                activeSites[size] = i;
                                m_lookupSiteVar[i] = size;
                                size++;
                        }
                }
                if ( size == 0 )
                {
                        delete [] activeSites;
                        printStatus2(alpha_label,beta_label,size,ticks0);
                        return;
                }

                // Create binary variables for each remaining site, add the data costs,
                // and compute the smooth costs between variables.
                EnergyT e(size,m_numNeighborsTotal,(void(*)(char*))handleError);
                e.add_variable(size);
                if ( m_setupDataCostsSwap   ) (this->*m_setupDataCostsSwap  )(size,alpha_label,beta_label,&e,activeSites);
                if ( m_setupSmoothCostsSwap ) (this->*m_setupSmoothCostsSwap)(size,alpha_label,beta_label,&e,activeSites);
                checkInterrupt();
                e.minimize();
                checkInterrupt();
                
                // Apply the new labeling
                for ( SiteID i = 0; i < size; i++ )
                {
                        m_labeling[activeSites[i]] = (e.get_var(i) == 0) ? alpha_label : beta_label;
                        m_lookupSiteVar[activeSites[i]] = -1; // restore lookupSiteVar to all -1s
                }
                m_labelingInfoDirty = true;
        } 
        catch (...)
        {
                delete [] activeSites;
                throw;
        }
        delete [] activeSites;

        printStatus2(alpha_label,beta_label,size,ticks0);
}


// Functions for the GCoptimizationGridGraph, derived from GCoptimization

GCoptimizationGridGraph::GCoptimizationGridGraph(SiteID width, SiteID height,LabelID num_labels)
                                                :GCoptimization(width*height,num_labels)
{
        assert( (width > 1) && (height > 1) && (num_labels > 1 ));

        m_weightedGraph = 0;
        for (int  i = 0; i < 4; i ++ )  m_unityWeights[i] = 1;

        m_width  = width;
        m_height = height;

        m_numNeighbors = new SiteID[m_num_sites];
        m_neighbors = new SiteID[4*m_num_sites];

        SiteID indexes[4] = {-1,1,-m_width,m_width};

        SiteID indexesL[3] = {1,-m_width,m_width};
        SiteID indexesR[3] = {-1,-m_width,m_width};
        SiteID indexesU[3] = {1,-1,m_width};
        SiteID indexesD[3] = {1,-1,-m_width};

        SiteID indexesUL[2] = {1,m_width};
        SiteID indexesUR[2] = {-1,m_width};
        SiteID indexesDL[2] = {1,-m_width};
        SiteID indexesDR[2] = {-1,-m_width};

        setupNeighbData(1,m_height-1,1,m_width-1,4,indexes);

        setupNeighbData(1,m_height-1,0,1,3,indexesL);
        setupNeighbData(1,m_height-1,m_width-1,m_width,3,indexesR);
        setupNeighbData(0,1,1,width-1,3,indexesU);
        setupNeighbData(m_height-1,m_height,1,m_width-1,3,indexesD);

        setupNeighbData(0,1,0,1,2,indexesUL);
        setupNeighbData(0,1,m_width-1,m_width,2,indexesUR);
        setupNeighbData(m_height-1,m_height,0,1,2,indexesDL);
        setupNeighbData(m_height-1,m_height,m_width-1,m_width,2,indexesDR);
}

//-------------------------------------------------------------------

GCoptimizationGridGraph::~GCoptimizationGridGraph()
{
        delete [] m_numNeighbors;
        if ( m_neighbors )
                delete [] m_neighbors;
        if (m_weightedGraph) delete [] m_neighborsWeights;
}


//-------------------------------------------------------------------

void GCoptimizationGridGraph::setupNeighbData(SiteID startY,SiteID endY,SiteID startX,
                                                                                          SiteID endX,SiteID maxInd,SiteID *indexes)
{
        SiteID x,y,pix;
        SiteID n;

        for ( y = startY; y < endY; y++ )
                for ( x = startX; x < endX; x++ )
                {
                        pix = x+y*m_width;
                        m_numNeighbors[pix] = maxInd;
                        m_numNeighborsTotal += maxInd;

                        for (n = 0; n < maxInd; n++ )
                                m_neighbors[pix*4+n] = pix+indexes[n];
                }
}

//-------------------------------------------------------------------

void GCoptimizationGridGraph::finalizeNeighbors()
{
}

//-------------------------------------------------------------------

void GCoptimizationGridGraph::setSmoothCostVH(EnergyTermType *smoothArray, EnergyTermType *vCosts, EnergyTermType *hCosts)
{
        setSmoothCost(smoothArray);
        m_weightedGraph = 1;
        computeNeighborWeights(vCosts,hCosts);
}

//-------------------------------------------------------------------

void GCoptimizationGridGraph::giveNeighborInfo(SiteID site, SiteID *numSites, SiteID **neighbors, EnergyTermType **weights)
{
        *numSites  = m_numNeighbors[site];
        *neighbors = &m_neighbors[site*4];
        
        if (m_weightedGraph) *weights  = &m_neighborsWeights[site*4];
        else *weights = m_unityWeights;
}

//-------------------------------------------------------------------

void GCoptimizationGridGraph::computeNeighborWeights(EnergyTermType *vCosts,EnergyTermType *hCosts)
{
        SiteID i,n,nSite;
        GCoptimization::EnergyTermType weight;
        
        m_neighborsWeights = new EnergyTermType[m_num_sites*4];

        for ( i = 0; i < m_num_sites; i++ )
        {
                for ( n = 0; n < m_numNeighbors[i]; n++ )
                {
                        nSite = m_neighbors[4*i+n];
                        if ( i-nSite == 1 )            weight = hCosts[nSite];
                        else if (i-nSite == -1 )       weight = hCosts[i];
                        else if ( i-nSite == m_width ) weight = vCosts[nSite];
                        else if (i-nSite == -m_width ) weight = vCosts[i];
        
                        m_neighborsWeights[i*4+n] = weight;
                }
        }

}
// Functions for the GCoptimizationGeneralGraph, derived from GCoptimization

GCoptimizationGeneralGraph::GCoptimizationGeneralGraph(SiteID num_sites,LabelID num_labels):GCoptimization(num_sites,num_labels)
{
        assert( num_sites > 1 && num_labels > 1 );

        m_neighborsIndexes = 0;
        m_neighborsWeights = 0;
        m_numNeighbors     = 0;
        m_neighbors        = 0;

        m_needTodeleteNeighbors        = true;
        m_needToFinishSettingNeighbors = true;
}

//------------------------------------------------------------------

GCoptimizationGeneralGraph::~GCoptimizationGeneralGraph()
{
                
        if ( m_neighbors )
                delete [] m_neighbors;

        if ( m_numNeighbors && m_needTodeleteNeighbors )
        {
                for ( SiteID i = 0; i < m_num_sites; i++ )
                {
                        if (m_numNeighbors[i] != 0 )
                        {
                                delete [] m_neighborsIndexes[i];
                                delete [] m_neighborsWeights[i];
                        }
                }

                delete [] m_numNeighbors;
                delete [] m_neighborsIndexes;
                delete [] m_neighborsWeights;
        }
}

//------------------------------------------------------------------

void GCoptimizationGeneralGraph::finalizeNeighbors()
{
        if ( !m_needToFinishSettingNeighbors )
                return;
        m_needToFinishSettingNeighbors = false;

        Neighbor *tmp;
        SiteID i,site,count;

        EnergyTermType *tempWeights = new EnergyTermType[m_num_sites];
        SiteID *tempIndexes         = new SiteID[m_num_sites];
        
        if ( !tempWeights || !tempIndexes ) handleError("Not enough memory");

        m_numNeighbors     = new SiteID[m_num_sites];
        m_neighborsIndexes = new SiteID*[m_num_sites];
        m_neighborsWeights = new EnergyTermType*[m_num_sites];
        
        if ( !m_numNeighbors || !m_neighborsIndexes || !m_neighborsWeights ) handleError("Not enough memory.");

        for ( site = 0; site < m_num_sites; site++ )
        {
                if ( m_neighbors && !m_neighbors[site].isEmpty() )
                {
                        m_neighbors[site].setCursorFront();
                        count = 0;
                        
                        while ( m_neighbors[site].hasNext() )
                        {
                                tmp = (Neighbor *) (m_neighbors[site].next());
                                tempIndexes[count] =  tmp->to_node;
                                tempWeights[count] =  tmp->weight;
                                delete tmp;
                                count++;
                        }
                        m_numNeighbors[site]     = count;
                        m_numNeighborsTotal     += count;
                        m_neighborsIndexes[site] = new SiteID[count];
                        m_neighborsWeights[site] = new EnergyTermType[count];
                        
                        if ( !m_neighborsIndexes[site] || !m_neighborsWeights[site] ) handleError("Not enough memory.");
                        
                        for ( i = 0; i < count; i++ )
                        {
                                m_neighborsIndexes[site][i] = tempIndexes[i];
                                m_neighborsWeights[site][i] = tempWeights[i];
                        }
                }
                else m_numNeighbors[site] = 0;

        }

        delete [] tempIndexes;
        delete [] tempWeights;
        if (m_neighbors) {
                delete [] m_neighbors;
                m_neighbors = 0;
        }
}
//------------------------------------------------------------------------------

void GCoptimizationGeneralGraph::giveNeighborInfo(SiteID site, SiteID *numSites, 
                                                                                                  SiteID **neighbors, EnergyTermType **weights)
{
        if (m_numNeighbors) {
                (*numSites)  =  m_numNeighbors[site];
                (*neighbors) = m_neighborsIndexes[site];
                (*weights)   = m_neighborsWeights[site];
        } else {
                *numSites = 0;
                *neighbors = 0;
                *weights = 0;
        }
}


//------------------------------------------------------------------

void GCoptimizationGeneralGraph::setNeighbors(SiteID site1, SiteID site2, EnergyTermType weight)
{

        assert( site1 < m_num_sites && site1 >= 0 && site2 < m_num_sites && site2 >= 0);
        if ( m_needToFinishSettingNeighbors == false )
                handleError("Already set up neighborhood system.");

        if ( !m_neighbors )
        {
                m_neighbors = (LinkedBlockList *) new LinkedBlockList[m_num_sites];
                if ( !m_neighbors ) handleError("Not enough memory.");
        }

        Neighbor *temp1 = (Neighbor *) new Neighbor;
        Neighbor *temp2 = (Neighbor *) new Neighbor;

        temp1->weight  = weight;
        temp1->to_node = site2;

        temp2->weight  = weight;
        temp2->to_node = site1;

        m_neighbors[site1].addFront(temp1);
        m_neighbors[site2].addFront(temp2);
        
}
//------------------------------------------------------------------

void GCoptimizationGeneralGraph::setAllNeighbors(SiteID *numNeighbors,SiteID **neighborsIndexes,
                                                                                                 EnergyTermType **neighborsWeights)
{
        m_needTodeleteNeighbors = false;
        m_needToFinishSettingNeighbors = false;
        if ( m_numNeighborsTotal > 0 )
                handleError("Already set up neighborhood system.");
        m_numNeighbors     = numNeighbors;
        m_numNeighborsTotal = 0;
        for (int site = 0; site < m_num_sites; site++ ) m_numNeighborsTotal += m_numNeighbors[site];
        m_neighborsIndexes = neighborsIndexes;
        m_neighborsWeights = neighborsWeights;
}



//------------------------------------------------------------------
// boring status messages

void GCoptimization::printStatus1(const char* extraMsg)
{
        if ( m_verbosity < 1 )
                return;
        if ( extraMsg )
                printf("gco>> %s\n",extraMsg);
        printf("gco>> initial energy: \tE=%lld (E=%lld+%lld+%lld)\n",(long long)compute_energy(),
                (long long)giveDataEnergy(), (long long)giveSmoothEnergy(), (long long)giveLabelEnergy()); 
        flushnow(); 
}

void GCoptimization::printStatus1(int cycle, bool isSwap, gcoclock_t ticks0)
{
        if ( m_verbosity < 1 )
                return;
        gcoclock_t ticks1 = gcoclock();
        printf("gco>> after cycle %2d: \tE=%lld (E=%lld+%lld+%lld);",cycle,(long long)compute_energy(),
                (long long)giveDataEnergy(),(long long)giveSmoothEnergy(),(long long)giveLabelEnergy());
        if ( m_stepsThisCycleTotal > 0 )
                printf(isSwap ? " \t%d swaps(s);" : " \t%d expansions(s);",m_stepsThisCycleTotal);
        if ( m_verbosity == 1 )
        {
                // Don't print time if time is already printed at finer scale, since printing
                // itself takes time (esp in MATLAB) and makes time useless at this level
                int ms = (int)(1000*(ticks1 - ticks0) / GCO_CLOCKS_PER_SEC);
                printf(" \t%d ms",ms);
        }
        printf("\n");
        flushnow(); 
}

void GCoptimization::printStatus2(int alpha, int beta, int numVars, gcoclock_t ticks0)
{
        if ( m_verbosity < 2 )
                return;
        int microsec = (int)(1000000*(gcoclock() - ticks0) / GCO_CLOCKS_PER_SEC);
        if ( beta >= 0 )
                printf("gco>>   after swap(%d,%d):",alpha+INDEX0,beta+INDEX0);
        else
                printf("gco>>   after expansion(%d):",alpha+INDEX0);
        printf(" \tE=%lld (E=%lld+%lld+%lld);\t %lld vars;",
                (long long)compute_energy(),(long long)giveDataEnergy(),
                (long long)giveSmoothEnergy(),(long long)giveLabelEnergy(),(long long)numVars);
        if ( m_stepsThisCycleTotal > 0 )
                printf(" \t(%d of %d);",m_stepsThisCycle+1,m_stepsThisCycleTotal);

        printf(microsec > 100 ? "\t %.2f ms\n" : "\t %.3f ms\n",(double)microsec/1000.0);
        flushnow();
}




//-------------------------------------------------------------------
// DataCostFnSparse methods
//-------------------------------------------------------------------


GCoptimization::DataCostFnSparse::DataCostFnSparse(SiteID num_sites, LabelID num_labels)
: m_num_sites(num_sites)
, m_num_labels(num_labels)
, m_buckets_per_label((m_num_sites + cSitesPerBucket-1)/cSitesPerBucket)
, m_buckets(0)
{
}

GCoptimization::DataCostFnSparse::DataCostFnSparse(const DataCostFnSparse& src)
: m_num_sites(src.m_num_sites)
, m_num_labels(src.m_num_labels)
, m_buckets_per_label(src.m_buckets_per_label)
, m_buckets(0)
{
        assert(!src.m_buckets); // not implemented
}

GCoptimization::DataCostFnSparse::~DataCostFnSparse()
{
        if (m_buckets) {
                for (LabelID l = 0; l < m_num_labels; ++l)
                        if (m_buckets[l*m_buckets_per_label].begin)
                                delete [] m_buckets[l*m_buckets_per_label].begin;
                delete [] m_buckets;
        }
}

void GCoptimization::DataCostFnSparse::set(LabelID l, const SparseDataCost* costs, SiteID count)
{
        // Create the bucket if necessary, and copy all the costs
        //
        if (!m_buckets) {
                m_buckets = new DataCostBucket[m_num_labels*m_buckets_per_label];
                memset(m_buckets, 0, m_num_labels*m_buckets_per_label*sizeof(DataCostBucket));
        }

        DataCostBucket* b = &m_buckets[l*m_buckets_per_label];
        if (b->begin)
                delete [] b->begin;
        SparseDataCost* next = new SparseDataCost[count];
        memcpy(next,costs,count*sizeof(SparseDataCost));

        //
        // Scan the list of costs and remember pointers to delimit the 'buckets', i.e. where 
        // ranges of SiteIDs lie along the array. Buckets can be empty (begin == end).
        //
        const SparseDataCost* end  = next+count;
        SiteID prev_site = -1;
        for (int i = 0; i < m_buckets_per_label; ++i) {
                b[i].begin = b[i].predict = next;
                SiteID end_site = (i+1)*cSitesPerBucket;
                while (next < end && next->site < end_site) {
                        if (next->site < 0 || next->site >= m_num_sites)
                                throw GCException("Invalid site id given for sparse data cost; must be within range.");
                        if (next->site <= prev_site)
                                throw GCException("Sparse data costs must be sorted in increasing order of SiteID");
                        prev_site = next->site;
                        ++next;
                }
                b[i].end = next;
        }
}

GCoptimization::EnergyTermType GCoptimization::DataCostFnSparse::search(DataCostBucket& b, SiteID s)
{
        // Perform binary search for requested SiteID
        //
        const SparseDataCost* L = b.begin;
        const SparseDataCost* R = b.end-1;
        if ( R - L == m_num_sites )
                return b.begin[s].cost; // special case: this particular label is actually dense
        do {
                const SparseDataCost* mid = (const SparseDataCost*)((((size_t)L+(size_t)R) >> 1) & cDataCostPtrMask);
                if (s < mid->site)
                        R = mid-1;         // eliminate upper range
                else if (mid->site < s)
                        L = mid+1;         // eliminate lower range
                else {
                        b.predict = mid+1;
                        return mid->cost;  // found it!
                }
        } while (R - L > cLinearSearchSize);
        
        // Finish off with linear search over the remaining elements
        //
        do {
                if (L->site >= s) {
                        if (L->site == s) {
                                b.predict = L+1;
                                return L->cost;
                        }
                        break;
                }
        } while (++L <= R);
        b.predict = L;

        return GCO_MAX_ENERGYTERM; // the site belongs to this bucket but with no cost specified
}

OLGA_INLINE GCoptimization::EnergyTermType GCoptimization::DataCostFnSparse::compute(SiteID s, LabelID l)
{
        DataCostBucket& b = m_buckets[l*m_buckets_per_label + (s >> cLogSitesPerBucket)];
        if (b.begin == b.end)
                return GCO_MAX_ENERGYTERM;
        if (b.predict < b.end) {
                // Check for correct prediction
                if (b.predict->site == s)
                        return (b.predict++)->cost; // predict++ for next time
                
                // If the requested site is missing from the site ids near 'predict'
                // then we know it doesn't exist in the bucket, so return INF
                if (b.predict->site > s && b.predict > b.begin && (b.predict-1)->site < s)
                        return GCO_MAX_ENERGYTERM;
        }
        if ( (size_t)b.end - (size_t)b.begin == cSitesPerBucket*sizeof(SparseDataCost) )
                return b.begin[s-b.begin->site].cost; // special case: this particular bucket is actually dense!

        return search(b,s);
}

GCoptimization::SiteID GCoptimization::DataCostFnSparse::queryActiveSitesExpansion(LabelID alpha_label, const LabelID* labeling, SiteID* activeSites)
{
        const SparseDataCost* next = m_buckets[alpha_label*m_buckets_per_label].begin;
        const SparseDataCost* end  = m_buckets[alpha_label*m_buckets_per_label + m_buckets_per_label-1].end;
        SiteID count = 0;
        for (; next < end; ++next) {
                if ( labeling[next->site] != alpha_label )
                        activeSites[count++] = next->site;
        }
        return count;
}