C++ Energy类代码示例

int main()
{
	// Minimize the following function of 3 binary variables:
	// E(x, y, z) = x - 2*y + 3*(1-z) - 4*x*y + 5*|y-z|
	   
	Energy<float,float,float>::Var varx, vary, varz;
	Energy<float,float,float> *e = new Energy<float,float,float>(3,2);
	varx = e -> add_variable();
	vary = e -> add_variable();
	varz = e -> add_variable();
	e -> add_term1(varx, 0, 1);  // add term x 
	e -> add_term1(vary, 0, -2); // add term -2*y
	e -> add_term1(varz, 3, 0);  // add term 3*(1-z)
	e -> add_term2(varx, vary, 0, 0, 0, -4); // add term -4*x*y
	e -> add_term2(vary, varz, 0, 5, 5, 0); // add term 5*|y-z|

	Energy<float,float,float>::TotalValue Emin = e -> minimize();
	
	printf("Minimum = %f\n", Emin);
	printf("Optimal solution:\n");
	printf("x = %d\n", e->get_var(varx));
	printf("y = %d\n", e->get_var(vary));
	printf("z = %d\n", e->get_var(varz));
	delete e;
	
	return 0;
}

//-----------------------------------------------------------------------------------
void GCoptimization::solveSwap(SiteID size,SiteID *activeSites,LabelID alpha_label,
							   LabelID beta_label)
{

	SiteID i,site;
	
	if ( !readyToOptimise() ) handleError("Set up data and smoothness terms first. ");
	if ( size == 0 ) return;

	
	Energy *e = new Energy();
	Energy::Var *variables = (Energy::Var *) new Energy::Var[size];

	for ( i = 0; i < size; i++ )
		variables[i] = e ->add_variable();

	set_up_swap_energy(size,alpha_label,beta_label,e,variables,activeSites);
		
	Energy::TotalValue Emin = e -> minimize();
		
	for ( i = 0; i < size; i++ )
	{
		site = activeSites[i];
		if ( e->get_var(variables[i]) == 0 )
			m_labeling[site] = alpha_label;
		else m_labeling[site] = beta_label;
		m_lookupSiteVar[site] = -1;
	}

	delete [] variables;
	delete e;

}

void Swap::perform_alpha_beta_swap(LabelType alpha_label, LabelType beta_label)
{
    PixelType i,size = 0;
    Energy *e = new Energy();


    for ( i = 0; i < m_nPixels; i++ )
    {
        if ( m_labeling[i] == alpha_label || m_labeling[i] == beta_label)
        {
            m_pixels[size]    = i;
            m_lookupPixVar[i] = size;
            size++;
        }
    }

    if ( size == 0 ) return;


    Energy::Var *variables = (Energy::Var *) new Energy::Var[size];
    if (!variables) { fprintf(stderr, "Not enough memory!\n"); exit(1); }
    

    for ( i = 0; i < size; i++ )
        variables[i] = e ->add_variable();

    if ( m_dataType == ARRAY ) add_t_links_ARRAY_swap(e,variables,size,alpha_label,beta_label,m_pixels);
    else  add_t_links_FnPix_swap(e,variables,size,alpha_label,beta_label,m_pixels);

    if ( m_grid_graph )
    {
        if ( m_smoothType != FUNCTION )
        {
            if (m_varWeights) set_up_swap_energy_G_ARRAY_VW(size,alpha_label,beta_label,m_pixels,e,variables);
            else set_up_swap_energy_G_ARRAY(size,alpha_label,beta_label,m_pixels,e,variables);
        }
        else  set_up_swap_energy_G_FnPix(size,alpha_label,beta_label,m_pixels,e,variables);
        
    }
    else
    {
        if ( m_smoothType  != FUNCTION  ) set_up_swap_energy_NG_ARRAY(size,alpha_label,beta_label,m_pixels,e,variables);
        else set_up_swap_energy_NG_FnPix(size,alpha_label,beta_label,m_pixels,e,variables);
    }
        

    e -> minimize();

    for ( i = 0; i < size; i++ )
        if ( e->get_var(variables[i]) == 0 )
            m_labeling[m_pixels[i]] = alpha_label;
        else m_labeling[m_pixels[i]] = beta_label;


    delete [] variables;
    delete e;

}

void EnergyAwareRandomWaypointMobility::receiveChangeNotification(int category, const cObject* details) {
    if(category == NF_BATTERY_CHANGED) {
        Energy* energyInformation = check_and_cast<Energy*>(details);
        if (energyInformation->GetEnergy() <= 0) {
            EV << "EnergyAwareRandomWaypointMobility detected that the battery is depleted and will stop moving after next target position has been reached" << std::endl;
            isEnergyDepleted = true;
        }
    }
}

int main(int argc, char** argv){
    Mammut m;
    Energy* energy = m.getInstanceEnergy();

    /** Gets the power plug counter. **/
    CounterPlug* counterPlug = dynamic_cast<CounterPlug*>(energy->getCounter(COUNTER_PLUG));
    if(!counterPlug){
        cout << "Plug counter not present on this machine." << endl;
    }

    /** Gets the CPUs energy counters. **/
    CounterCpus* counterCpus = dynamic_cast<CounterCpus*>(energy->getCounter(COUNTER_CPUS));
    if(!counterCpus){
        cout << "Cpu counters not present on this machine." << endl;
    }

    /** Prints information for each counter. **/
    if(counterCpus){
        cout << "Found Cpus counter ";
        cout << "Has cores counter: " << counterCpus->hasJoulesCores() << " ";
        cout << "Has graphic counter: " << counterCpus->hasJoulesGraphic() << " ";
        cout << "Has Dram counter: " << counterCpus->hasJoulesDram() << " ";
        cout << endl;
    }

    /** Gets the value of the counter every sleepingSecs seconds. **/
    unsigned int sleepingSecs = 10;
    unsigned int iterations = 4;
    for(unsigned int i = 0; i < iterations; i++){
        cout << "Sleeping " << sleepingSecs << " seconds." << endl;
        sleep(sleepingSecs);
        if(counterPlug){
            cout << "Joules consumed at power plug: " << counterPlug->getJoules() << endl;
            counterPlug->reset();
        }

	if(counterCpus){
            vector<Cpu*> cpus = m.getInstanceTopology()->getCpus();
            for(size_t j = 0; j < cpus.size(); j++){
                CpuId id = cpus.at(j)->getCpuId();
                cout << "Joules consumed for CPU " << id << ": ";
                cout << "Cpu: " << counterCpus->getJoulesCpu(id) << " ";
                if(counterCpus->hasJoulesCores()){
                	cout << "Cores: " << counterCpus->getJoulesCores(id) << " ";
                }
                if(counterCpus->hasJoulesGraphic()){
                    cout << "Graphic: " << counterCpus->getJoulesGraphic(id) << " ";
                }
                if(counterCpus->hasJoulesDram()){
                    cout << "Dram: " << counterCpus->getJoulesDram(id) << " ";
                }
                cout << endl;
            }
            counterCpus->reset();
	}
    }
}

double calculateEnergy(std::vector<particle> &p, Energy &ene, int opt) {
  double r = dist(p.at(0),p.at(1));
  if (r < 0.5e-10 || r > 10e-10) {
    return 1e9;
  }

  if (opt == 2 || opt == 4) {
    return ene.q2mu(p.at(0), p.at(1))/(pc::kT);
  } else if (opt == 1 || opt == 3) {
    return ene.mu2mu(p.at(0), p.at(1))/(pc::kT);
  } else if (opt == 5) {
    return ene.q2q(p.at(0),p.at(1))/(pc::kT);
  } else {
    return 0.0;
  }
}

int main(int argc, char* argv[])
{
	if (0)
	{
		Energy* g = ReadUAI("../grid4x4.UAI.LG");
		
		g->SetFullEdges(1);
		
		Energy::Options options;
		options.method = Energy::Options::MPLP;
		options.iter_max = 100;
		g->Solve(options);
		exit(1);
	}

	int i;

	char* filename = NULL;
	char* save_filename = NULL;
	char* save_rep_filename = NULL;
	bool BLP_relaxation = false;
	bool FULL_relaxation = false;
	bool FULLDUAL_relaxation = false;
	int FULL_relaxation_flag = 0;
	int FULLDUAL_relaxation_flag = 0;
	bool print_stats = false;
	
	Run* current = new Run;

	Block<Run*> run_list(10);
	Run** run_ptr = run_list.New();
	*run_ptr = current;

	for (i=1; i<argc; i++)
	{
		if (argv[i][0] == '+' && !argv[i][1])
		{
			current = new Run;
			run_ptr = run_list.New();
			*run_ptr = current;
			continue;
		}
		if (argv[i][0] != '-')
		{
			if (filename) { printf("Error: filename can be specified only once\n"); ShowUsage(argv[0]); }
			filename = argv[i];
			continue;
		}
		if (!strcmp("h", &argv[i][1]))
		{
			ShowUsage(argv[0], true);
		}
		if (!strncmp("save=", &argv[i][1], 5))
		{
			if (save_filename) { printf("Error: save filename can be specified only once\n"); ShowUsage(argv[0]); }
			save_filename = &argv[i][1+5];
			continue;
		}
		if (!strncmp("saverep=", &argv[i][1], 8))
		{
			if (save_filename) { printf("Error: save filename can be specified only once\n"); ShowUsage(argv[0]); }
			save_rep_filename = &argv[i][1+8];
			continue;
		}
		if (!strncmp("t=", &argv[i][1], 2))
		{
			if (current->tighten_filename) { printf("Error: tightening filename can be specified only once\n"); ShowUsage(argv[0]); }
			current->tighten_filename = &argv[i][1+2];
			continue;
		}
		if (!strncmp("iter=", &argv[i][1], 5))
		{
			current->options.iter_max = atoi(&argv[i][1+5]);
			continue;
		}
		if (!strncmp("time=", &argv[i][1], 5))
		{
			current->options.time_max = atof(&argv[i][1+5]);
			continue;
		}
		if (!strncmp("eps=", &argv[i][1], 4))
		{
			current->options.eps = atof(&argv[i][1+4]);
			continue;
		}
		if (!strcmp("SRMP", &argv[i][1]))
		{
			current->options.method = Energy::Options::SRMP;
			continue;
		}
		if (!strcmp("CMP", &argv[i][1]))
		{
			current->options.method = Energy::Options::CMP;
			continue;
		}
		if (!strcmp("MPLP", &argv[i][1]))
		{
			current->options.method = Energy::Options::MPLP;
			continue;
		}
//.........这里部分代码省略.........

Energy* ReadUAI(const char* filename)
{
	ReadLinesFromFile F(filename);
	if (!F.exists()) { printf("Can't open %s\n", filename); exit(1); }

	Energy* g = NULL;
	int phase = 0; // 0: reading # nodes
	               // 1: reading # states
	               // 2: reading # terms
	               // 3: reading term descriptions
	               // 4: reading term costs
	               // 5: finished
	int node_num = 0, t = 0, term_num = 0;
	struct Term
	{
		int arity;
		int* nodes;
	};
	Term* terms = NULL;
	Buffer buf(1024);

	ReadNumbers<int> read_ints;
	ReadNumbers<double> read_doubles;

	char* line;
	while ( (line=F.NextLine()) != NULL )
	{
		int i, num = read_ints.ReadLine(line);
		if (num == 0) continue;

		if (phase == 0)
		{
			if (num != 1) { printf("Error in line %d: # nodes expected\n", F.CurrentLine()); exit(1); }
			node_num = read_ints.GetNumber(0);
			if (node_num < 1) { printf("Error in line %d: # of nodes should be positive\n", F.CurrentLine()); exit(1); }
			g = new Energy(node_num);
			phase ++;
		}
		else if (phase == 1)
		{
			if (num != node_num) { printf("Error in line %d: # of nodes do not match\n", F.CurrentLine()); exit(1); }
			for (i=0; i<node_num; i++) g->AddNode(read_ints.GetNumber(i));
			phase ++;
		}
		else if (phase == 2)
		{
			if (num != 1) { printf("Error in line %d: # terms expected\n", F.CurrentLine()); exit(1); }
			term_num = read_ints.GetNumber(0);
			if (term_num < 0) { printf("Error in line %d: # of terms should be non-negative\n", F.CurrentLine()); exit(1); }
			t = 0;
			terms = new Term[term_num];
			phase ++;
		}
		else if (phase == 3)
		{
			num --;
			if (num != read_ints.GetNumber(0)) { printf("Error in line %d: incorrect term entry\n", F.CurrentLine()); exit(1); }
			if (num < 1) { printf("Error in line %d: arity must be positive\n", F.CurrentLine()); exit(1); }
			terms[t].arity = num;
			terms[t].nodes = (int*)buf.Alloc(num*sizeof(int));
			for (i=0; i<num; i++)
			{
				int q = read_ints.GetNumber(i+1);
				if (q<0 || q>=node_num)  { printf("Error in line %d: incorrect index\n", F.CurrentLine()); exit(1); }
				terms[t].nodes[i] = q;
			}
			t ++;
			if (t == term_num) { phase ++; t = 0; }
		}
		else if (phase == 4)
		{
			if (num != 1) { printf("Error in line %d: # costs expected\n", F.CurrentLine()); exit(1); }
			num = read_ints.GetNumber(0);
			line = F.NextLine();
			if (!line) { printf("Error in line %d: missing cost table\n", F.CurrentLine()); exit(1); }
			int double_num = read_doubles.ReadLine(line);
			double* arr = read_doubles.GetNumbersArray();
			if (TAKE_LOG)
			{
				for (i=0; i<double_num; i++) arr[i] = log(arr[i]);
			}
			for (i=0; i<double_num; i++) arr[i] = -arr[i]; // we are minimizing, not maximizing!
			if (num == -1)
			{
				if (terms[t].arity != 2 || g->GetK(terms[t].nodes[0]) != g->GetK(terms[t].nodes[1]))
				{ printf("Error in line %d: not a potts term\n", F.CurrentLine()); exit(1); }
				if ( 1 )
				{
					if (!potts) potts = new PottsFactorType;
					g->AddFactor(2, terms[t].nodes, arr, potts);
				}
				else
				{
					int x, y, i=0, K = g->GetK(terms[t].nodes[0]);
					double lambda = arr[0];
					double* V = new double[K*K];
					for (x=0; x<K; x++)
					for (y=0; y<K; y++)
					{
						V[i ++] = (x == y) ? 0 : lambda;
//.........这里部分代码省略.........

void Match::BVZ_Expand(Coord a)
{
	Coord p, d, q, dq;
	Energy::Var var, qvar;
	int E_old, E00, E0a, Ea0;
	int k;

	/* node_vars stores variables corresponding to nodes */

	Energy *e = new Energy(BVZ_error_function);

	/* initializing */
	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));
		if (a == d)
		{
			IMREF(node_vars, p) = VAR_ACTIVE;
			e -> add_constant(BVZ_data_penalty(p, d));
		}
		else
		{
			IMREF(node_vars, p) = var = e -> add_variable();
			e -> ADD_TERM1(var, BVZ_data_penalty(p, d), BVZ_data_penalty(p, a));
		}
	}

	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));
		var = (Energy::Var) IMREF(node_vars, p);

		/* smoothness term */
		for (k=0; k<NEIGHBOR_NUM; k++)
		{
			q = p + NEIGHBORS[k];
			if ( ! ( q>=Coord(0,0) && q<im_size ) ) continue;
			qvar = (Energy::Var) IMREF(node_vars, q);
			dq = Coord(IMREF(x_left, q), IMREF(y_left, q));

			if (var != VAR_ACTIVE && qvar != VAR_ACTIVE)
				E00 = BVZ_smoothness_penalty(p, q, d, dq);
			if (var != VAR_ACTIVE)
				E0a = BVZ_smoothness_penalty(p, q, d, a);
			if (qvar != VAR_ACTIVE)
				Ea0 = BVZ_smoothness_penalty(p, q, a, dq);

			if (var != VAR_ACTIVE)
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM2(var, qvar, E00, E0a, Ea0, 0);
				else                    e -> ADD_TERM1(var, E0a, 0);
			}
			else
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM1(qvar, Ea0, 0);
				else                    {}
			}
			
		}
	}

	E_old = E;
	E = e -> minimize();

	if (E < E_old)
	{
		for (p.y=0; p.y<im_size.y; p.y++)
		for (p.x=0; p.x<im_size.x; p.x++)
		{
			var = (Energy::Var) IMREF(node_vars, p);
			if (var!=VAR_ACTIVE && e->get_var(var)==VALUE1)
			{
				IMREF(x_left, p) = a.x; IMREF(y_left, p) = a.y;
			}
		}
	}

	delete e;
}

//.........这里部分代码省略.........
					}
					else{
						BeginErrorState(Info,ERROR_NAMELEN_PART);		
					}
				}
				break;
			case NAME_PART:
				{
					//	Info->HeaderStr += ch;
					
					// ch ->[0-9] 并且 不能超过约定位数				
					if(Info->Buffer.size() < Info->DataLen)
					{
						Info->Buffer += ch;
					}
					else if(ch == '@'){
						eSTRING s;
						s.FromString(Info->Buffer,0);
						tstring Name = s;

						assert(Info->DataType == TYPE_PIPELINE);
						ePipeline* Data = (ePipeline*)Info->Data;
						Data->SetLabel(Name.c_str()); 
						
						Info->Buffer = "";
						Info->State = LENGTH_PART;
						Info->DataLen = DATA_LENGTH_LIMIT;
					}
					else{
						BeginErrorState(Info,ERROR_NAME_PART);
					}
				}
				break;
			default:
				assert(0);
				break;
				
			}
			
			if(bCompleteOneData){
				bCompleteOneData = FALSE;
				
				//处理得到的实际数据
				while(m_ContextStack.size()){	
					
					//Info->ParentPipe->Push_Directly(Info->Data);
					
					int32 Len = Info->InfoLen;
		            Energy* Data = Info->Data;
					
					if (m_ContextStack.size()>1)
					{
						RevContextInfo* PreInfo = m_ContextStack[1];
						
						PreInfo->InfoLen += Len;
						
						PreInfo->DataLen -= Len;
						
						if (PreInfo->DataLen  == 0) //已经完成一个嵌套Pipe,重复上述步骤
						{
						
							ePipeline  NotifData;
							NotifData.PushInt(Len);   //总长度								
							NotifData.PushInt(PreInfo->InfoLen); //Parent Pipe已经完成的
							NotifData.Push_Directly(Data->Clone());								
							m_Parent->NotifyLinkerState(this,LINKER_RECEIVE_STEP,NotifData);
															
							delete Info;
							m_ContextStack.pop_front();
							Info = m_ContextStack.front();
							
							assert(PreInfo == Info);	

						}else if (PreInfo->DataLen > 0) //继续接收下一个数据
						{
							Info->Reset();  
							break;
						}else{ //错误
							BeginErrorState(Info,ERROR_OTHER_PARNT);
							Info->Reset();
						}
					}else{ //已经完成一个完整信息的组装
						
						assert(m_ContextStack.size()==1);
						assert(m_CurRevMsg.Size()==1);

						eElectron E;
						m_CurRevMsg.Pop(&E);
						
					
						RevOneMsg(E);

						assert(Info->ParentPipe == &m_CurRevMsg);
						Info->Reset();
						break;
					}
				}//while
			}
      }//while
}

void Expansion::perform_alpha_expansion(LabelType alpha_label)
{
    PixelType i,size = 0;
    Energy *e = new Energy();

    for ( i = 0; i < m_nPixels; i++ )
    {
        if ( m_labeling[i] != alpha_label )
        {
            m_lookupPixVar[size] = i;
            size++;
        }
    }


    if ( size > 0 )
    {
        Energy::Var *variables = (Energy::Var *) new Energy::Var[size];
        if ( !variables) {
            printf("\nOut of memory, exiting");
            exit(1);
        }

        for ( i = 0; i < size; i++ )
            variables[i] = e ->add_variable();

        if ( m_dataType == ARRAY ) add_t_links_ARRAY(e,variables,size,alpha_label);
        else  add_t_links_FnPix(e,variables,size,alpha_label);


        if ( m_grid_graph )
        {
            if ( m_smoothType != FUNCTION )
            {
                if (m_varWeights) set_up_expansion_energy_G_ARRAY_VW(size,alpha_label,e,variables);
                else set_up_expansion_energy_G_ARRAY(size,alpha_label,e,variables);
            }
            else set_up_expansion_energy_G_FnPix(size,alpha_label,e,variables);

        }
        else
        {
            if ( m_smoothType != FUNCTION ) set_up_expansion_energy_NG_ARRAY(size,alpha_label,e,variables);
            else if ( m_smoothType == FUNCTION) set_up_expansion_energy_NG_FnPix(size,alpha_label,e,variables);
        }

        e -> minimize();

        for ( i = 0,size = 0; i < m_nPixels; i++ )
        {
            if ( m_labeling[i] != alpha_label )
            {
                if ( e->get_var(variables[size]) == 0 )
                    m_labeling[i] = alpha_label;

                size++;
            }
        }

        delete [] variables;
    }

    delete e;
}

 /// Here we use E=1/2m.v²
 Mass::Mass(const Energy& energy,const Speed& speed)
 : m_value(2*energy.Joule()/(speed.MeterPerSecond().length()*speed.MeterPerSecond().length())), 
   m_unit(_Kilogram)
 {}

void robustpn(int nout, mxArray* pout[], int nin, const mxArray* pin[])
{

    /*
     *********************************************************
     * Check inputs and construct the energy
     **********************************************************
     */
    
    int nLabel, nVar, nPair, nHigher, ii;
    int hop_fields_indices[HOP_N_OF_FIELDS]; // indices to fields in hop struct
    
    // check pin[0] is sparse
    if ( ! mxIsSparse(pin[0]) || ! mxIsDouble(pin[0]) || mxIsComplex(pin[0]))
        mexErrMsgIdAndTxt("robustpn:inputs","sparseG must be a sparse double matrix");
    // check pin[0] is square
    const mwSize *spd = mxGetDimensions(pin[0]);
    if (spd[0] != spd[1])
        mexErrMsgIdAndTxt("robustpn:inputs","sparseG must be a square matrix");
    nVar = spd[0];
    
    nPair = 0;
    // read the sparse matrix
    double* Pr = mxGetPr(pin[0]);
    mwIndex  *ir = mxGetIr(pin[0]);
    mwIndex *jc = mxGetJc(pin[0]);
    mwIndex col, starting_row_index, stopping_row_index, current_row_index, tot(0);
    
    mwSize max_npair = mxGetNzmax(pin[0]);
    int * pairs = new int[2 * max_npair]; // will be de-alocate on ~Energy
    termType* sc = new termType[max_npair]; // will be de-alocate on ~Energy

    // mexPrintf("Preparing to read sG\n");
    
    // traverse the sparseG matrix - pick only connections from the upper tri of the matrix
    // (since its symmetric and we don't want to count each potential twice).
    for (col=0; col<nVar; col++)  {
        starting_row_index = jc[col];
        stopping_row_index = jc[col+1];
        if (starting_row_index == stopping_row_index)
            continue;
        else {
            for (current_row_index = starting_row_index;
              current_row_index < stopping_row_index  ; 
              current_row_index++)  {
                if ( ir[current_row_index] >= col ) { // ignore lower tri of matrix
                    pairs[nPair*2] = ir[current_row_index]; // from
                    pairs[nPair*2 + 1] = col; // to
                    sc[nPair] = (termType)Pr[tot]; // potential weight
                    nPair++;
                }
                tot++;
            }
        }
    }
       
    // mexPrintf("Done reading sG got %d pairs\n", nPair);
    
    // check pin[1] has enough columns (=#nodes)
    const mwSize *sdc = mxGetDimensions(pin[1]);
    if (sdc[1] != spd[0])
        mexErrMsgIdAndTxt("robustpn:inputs","Dc must have %d columns to match graph structure", spd[0]);
    nLabel = sdc[0];
    
    
    // check pin[2] is struct array with proper feilds
    if ( mxGetClassID(pin[2]) != mxSTRUCT_CLASS )
        mexErrMsgIdAndTxt("robustpn:inputs","hop must be a struct array");
    nHigher = mxGetNumberOfElements(pin[2]);
    // expecting HOP_N_OF_FIELDS fieds
    if ( mxGetNumberOfFields(pin[2]) != HOP_N_OF_FIELDS )
        mexErrMsgIdAndTxt("robustpn:inputs","hop must have %d fields", HOP_N_OF_FIELDS);
    // chack that we have the right fields
    for ( ii = 0; ii < HOP_N_OF_FIELDS ; ii++ ) {
        hop_fields_indices[ii] = mxGetFieldNumber(pin[2], HOP_FIELDS[ii]);
        if ( hop_fields_indices[ii] < 0 )
            mexErrMsgIdAndTxt("robustpn:inputs","hop is missing %s field", HOP_FIELDS[ii]);
    }
    
    Energy<termType> *energy = new Energy<termType>(nLabel, nVar, nPair, nHigher);

    energy->SetUnaryCost( (termType*)mxGetData(pin[1]) );
    energy->SetPairCost(pairs, sc);
    delete[] pairs; // were copied into energy
    delete[] sc;
    
    // Add the HO potentials
    mxArray *xind, *xw, *xgamma, *xQ;
    int * ind, n;
    termType* w;
    termType* gamma;
    termType Q;
    for ( ii = 0 ; ii < nHigher; ii++ ) {
        xind = mxGetFieldByNumber(pin[2], ii, hop_fields_indices[0]);
        n = mxGetNumberOfElements(xind);
        ind = new int[n]; // allocation for energy
        GetArr(xind, ind, -1); // bias = -1 convert from 1-ind of matlab to 0-ind of C
        
        xw = mxGetFieldByNumber(pin[2], ii, hop_fields_indices[1]);
        if ( mxGetNumberOfElements(xw) != n ) {
//.........这里部分代码省略.........

void expandOnLabel(int label,int width,int height,int num_pixels,
				   vector<int> &Seeds,int numSeeds, vector<int> &labeling,
				   vector<Value> &horizWeights,vector<Value> &vertWeights,Value lambda,
				   vector<Value> &diag1Weights,vector<Value> &diag2Weights,int PATCH_SIZE,
				   vector<int> &changeMask, vector<int> &changeMaskNew,image<uchar> *I,
				   int TYPE,float variance)
{
	int seedX,seedY,startX,startY,endX,endY,numVars,blockWidth;
	getBounds(width,height,Seeds,&seedX,&seedY,&startX,&startY,&endX,&endY,label,PATCH_SIZE);

	int somethingChanged = 0;

	for ( int y = startY; y <= endY; y++ )
		for ( int x = startX; x <= endX; x++ )
			if ( changeMask[x+y*width] == 1 )
			{
				somethingChanged = 1;
				break;
			}

	
	if ( somethingChanged == 0)
		return;
		
	blockWidth = endX-startX+1;
	numVars   = (endY-startY+1)*blockWidth;
	

	vector<Var> variables(numVars);
	Energy<int,int,int> *e = new Energy<int,int,int>(numVars,numVars*3);

	for ( int i = 0; i < numVars; i++ )
		variables[i] = e->add_variable();

	Value LARGE_WEIGHT = lambda*NUM_COLORS*8;

	// First fix the border to old labels, except the edges of the image
	for ( int y = startY; y <= endY; y++ ){
		if ( startX != 0 )
			e->add_term1(variables[(y-startY)*blockWidth],0,LARGE_WEIGHT);
		else if ( y == startY || y == endY)
			e->add_term1(variables[(y-startY)*blockWidth],0,LARGE_WEIGHT);

		if( endX != width -1 )
			e->add_term1(variables[(endX-startX)+(y-startY)*blockWidth],0,LARGE_WEIGHT);
		else if ( y == startY || y == endY)
			e->add_term1(variables[(endX-startX)+(y-startY)*blockWidth],0,LARGE_WEIGHT);
	}

	for ( int x = startX+1; x < endX; x++){
		if ( startY != 0 )
			e->add_term1(variables[(x-startX)],0,LARGE_WEIGHT);
		if ( endY != height - 1)
			e->add_term1(variables[(x-startX)+(endY-startY)*blockWidth],0,LARGE_WEIGHT);
	}

	// add links to center of the patch for color constant superpixels
	if ( TYPE == 1 )
	{
		int color = imRef(I,seedX,seedY);

		for ( int y = startY+1; y < endY; y++ )
			for ( int x = startX+1; x < endX; x++){
				Value E00=0,E01=0,E10=LARGE_WEIGHT,E11=0;

				if (seedX != x && seedY != y)
					e->add_term2(variables[(x-startX)+(y-startY)*blockWidth],
					         variables[(seedX-startX)+(seedY-startY)*blockWidth],E00,E01,E10,E11);

				int diff = abs(imRef(I,x,y)-color);
				int maxD = (int) variance*MULTIPLIER_VAR;
				if ( diff > maxD ) diff = maxD;

				int oldLabel = labeling[x+y*width];
				int oldY = Seeds[oldLabel]/width;
				int oldX = Seeds[oldLabel]-oldY*width;
				int oldColor = imRef(I,oldX,oldY);
				int oldDiff = abs(imRef(I,x,y)-oldColor);
				if ( oldDiff > maxD ) oldDiff = maxD;

				if ( oldDiff > diff) 
					 e->add_term1(variables[(x-startX)+(y-startY)*blockWidth],oldDiff-diff,0);
				else e->add_term1(variables[(x-startX)+(y-startY)*blockWidth],0,diff-oldDiff);
			}
	}


	// First set up horizontal links 
	for ( int y = startY; y <= endY; y++ )
		for ( int x = startX+1; x <=endX; x++){
			int oldLabelPix       = labeling[x+y*width];
			int oldLabelNeighbPix = labeling[x-1+y*width];
			Value E00,E01,E10,E11=0;

			if ( oldLabelPix != oldLabelNeighbPix ) 
				E00 = horizWeights[x-1+y*width];
			else E00 = 0;
			if ( oldLabelNeighbPix != label ) 
				E01 = horizWeights[x-1+y*width];
			else E01 = 0;
//.........这里部分代码省略.........

/* computes the minimum a-expansion configuration */
void Match::KZ1_Expand(Coord a)
{
	Coord p, q, d, dq;
	Energy::Var var, qvar;
	int E_old, delta, E00, E0a, Ea0, Eaa;
	int k;

	Energy *e = new Energy(KZ1_error_function);

	/* initializing */
	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));
		if (a == d) IMREF(node_vars_left, p) = VAR_ACTIVE;
		else
		{
			IMREF(node_vars_left, p) = var = e -> add_variable();
			if (d.x == OCCLUDED) e -> ADD_TERM1(var, KZ1_OCCLUSION_PENALTY, 0);
		}

		d = Coord(IMREF(x_right, p), IMREF(y_right, p));
		if (a == -d) IMREF(node_vars_right, p) = VAR_ACTIVE;
		else
		{
			IMREF(node_vars_right, p) = var = e -> add_variable();
			if (d.x == OCCLUDED) e -> ADD_TERM1(var, KZ1_OCCLUSION_PENALTY, 0);
		}
	}

	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		/* data and visibility terms */
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));
		var = (Energy::Var) IMREF(node_vars_left, p);
		if (d != a && d.x != OCCLUDED)
		{
			q = p + d;
			if (q>=Coord(0,0) && q<im_size)
			{
				qvar = (Energy::Var) IMREF(node_vars_right, q);
				dq = Coord(IMREF(x_right, q), IMREF(y_right, q));
				if (d == -dq)
				{
					delta = (is_blocked(a, d)) ? INFINITY : 0;
					e -> ADD_TERM2(var, qvar, KZ1_data_penalty(p, q), delta, delta, 0);
				}
				else if (is_blocked(a, d))
				{
					e -> ADD_TERM2(var, qvar, 0, INFINITY, 0, 0);
				}
			}
		}

		q = p + a;
		if (q>=Coord(0,0) && q<im_size)
		{
			qvar = (Energy::Var) IMREF(node_vars_right, q);
			dq = Coord(IMREF(x_right, q), IMREF(y_right, q));

			E0a = (is_blocked(d, a)) ? INFINITY : 0;
			Ea0 = (is_blocked(-dq, a)) ? INFINITY : 0;
			Eaa = KZ1_data_penalty(p, q);

			if (var != VAR_ACTIVE)
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM2(var, qvar, 0, E0a, Ea0, Eaa);
				else                    e -> ADD_TERM1(var, E0a, Eaa);
			}
			else
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM1(qvar, Ea0, Eaa);
				else                    e -> add_constant(Eaa);
			}
		}

		/* left smoothness term */
		for (k=0; k<NEIGHBOR_NUM; k++)
		{
			q = p + NEIGHBORS[k];
			if ( ! ( q>=Coord(0,0) && q<im_size ) ) continue;
			qvar = (Energy::Var) IMREF(node_vars_left, q);
			dq = Coord(IMREF(x_left, q), IMREF(y_left, q));

			if (var != VAR_ACTIVE && qvar != VAR_ACTIVE)
				E00 = KZ1_smoothness_penalty_left(p, q, d, dq);
			if (var != VAR_ACTIVE)
				E0a = KZ1_smoothness_penalty_left(p, q, d, a);
			if (qvar != VAR_ACTIVE)
				Ea0 = KZ1_smoothness_penalty_left(p, q, a, dq);

			if (var != VAR_ACTIVE)
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM2(var, qvar, E00, E0a, Ea0, 0);
				else                    e -> ADD_TERM1(var, E0a, 0);
			}
			else
			{
//.........这里部分代码省略.........