C++ GraphType::add_node方法代码示例

GraphType* create_graph(std::vector< EdgeType >& edges,
                        std::vector<int>& curr_edge_cap,
                        std::vector<int>& curr_lambda_cap,
                        const std::vector<int>& fg_nodes, const int INFTY,
                        const int NUM_NODES, const bool fg_cap_inf) {
  bool is_fg;
  GraphType *g = new GraphType(NUM_NODES, edges.size());
  g->add_node(NUM_NODES);
  for (unsigned int i = 0; i < NUM_NODES; ++i) {
    is_fg = false;
    for (unsigned int j = 0; j < fg_nodes.size(); ++j) {
      if (i == fg_nodes[j]) {
        is_fg = true;
        break;
      }
    }
    if (is_fg)
      g->add_tweights(i, /* capacities */(fg_cap_inf ? INFTY : curr_lambda_cap[i]), 0);
    else
      g->add_tweights(i, /* capacities */0, curr_lambda_cap[i]);
  }

  /* capacity edges */
  for (unsigned int i = 0; i < edges.size(); ++i) {
    g->add_edge(edges[i].first, edges[i].second, curr_edge_cap[i],
                curr_edge_cap[i]);
  }

  return g;
}

void example1() {
  typedef Graph<int, int, int> GraphType;
  GraphType *g = new GraphType(/*estimated # of nodes*/2, /*estimated # of edges*/
                               1);

  g->add_node();
  g->add_node();

  g->add_tweights(0, /* capacities */3, 9);
  g->add_tweights(1, /* capacities */8, 1);
  g->add_edge(0, 1, /* capacities */3, 4);

  int flow = g->maxflow();

  std::cout << "Flow = " << flow << std::endl;
  std::cout << "Minimum cut:" << std::endl;
  for (int i = 0; i < g->get_node_num(); ++i) {
    if (g->what_segment(i) == GraphType::SOURCE)
      std::cout << "node " << i << " is in the SOURCE set" << std::endl;
    else
      std::cout << "node " << i << " is in the SINK set" << std::endl;
  }

  delete g;
}

GraphType* construct_graph(const int NUM_NODES, const int NUM_EDGES,
                           const int bg_cap[], const int edges_a[],
                           const int edges_b[], const int edges_cap[],
                           GraphType::FGSeedsType& fg_nodes,
                           const int lambda) {
  GraphType::SrcSeedNode* it;

  GraphType *g = new GraphType(NUM_NODES, NUM_EDGES);

  g->add_node(NUM_NODES);

  /* add unary edges */
  for (unsigned int i = 0; i < g->get_node_num(); ++i) {
    int fg_node_id = get_node_source_id(i, fg_nodes, it);

    if (fg_node_id == INVALID_SRC_ID) {
      g->add_tweights(i, /* capacities */0, bg_cap[i] + lambda);
    } else {
      g->add_tweights(i, /* capacities */infty, 0);
      it->second.first = infty;
      it->second.second = bg_cap[i] + lambda;
    }

    g->set_node_source_idx(i, fg_node_id);
  }

  /* add pairwise edges */
  for (unsigned int i = 0; i < NUM_EDGES; ++i)
    g->add_edge(edges_a[i], edges_b[i], edges_cap[i], edges_cap[i]);

  return g;
}

void GCOutputBoykovWorker::run()
{
  const int width = m_SourceImage.width();
  const int height = m_SourceImage.height();
  const int nLabels = 2;
  cv::Mat src(height, width, CV_8UC3, m_SourceImage.bits(), m_SourceImage.bytesPerLine());

  // Allocate graph
  int num_nodes = width * height;
  int num_edges = (width-1)*height + width*(height-1);
  typedef Graph<int,int,int> GraphType;
  GraphType* graph = new GraphType(num_nodes, num_edges);
  graph->add_node(num_nodes);

  // Initialize Data Term
  generateDataTerm([graph,width](int x, int y, int dterm1, int dterm2){
    GraphType::node_id node = y * width + x;
    graph->add_tweights(node, dterm1, dterm2);
  });

  // Initialize Smoothness Term
  generateSmoothTerm([graph,width](int x1, int y1, int x2, int y2, int cap1, int cap2){
    GraphType::node_id node1, node2;
    node1 = y1 * width + x1, node2 = y2 * width + x2;
    graph->add_edge(node1, node2, cap1, cap2);
  });

  // Compute
  int flow = graph->maxflow();
  qDebug("Flow = %d", flow);

  // Read Result
  QImage destImage(width, height, QImage::Format_RGB888);
  cv::Mat dst(height, width, CV_8UC3, destImage.bits(), destImage.bytesPerLine());
  for (int y = 0; y < dst.rows; ++y) {
    for (int x = 0; x < dst.cols; ++x) {
      GraphType::node_id node = y * width + x;

      if (graph->what_segment(node) == GraphType::SOURCE) {
        dst.at<cv::Vec3b>(y,x)[0] = 255;
        dst.at<cv::Vec3b>(y,x)[1] = 255;
        dst.at<cv::Vec3b>(y,x)[2] = 255;
      } else {
        dst.at<cv::Vec3b>(y,x)[0] = 0;
        dst.at<cv::Vec3b>(y,x)[1] = 0;
        dst.at<cv::Vec3b>(y,x)[2] = 0;
      }
    }
  }

  delete graph;

  emit completed(destImage);
}

int main(int argc, char** argv)
{
  // Check arguments
  if (argc < 3) {
    std::cerr << "Usage: " << argv[0] << " NODES_FILE TETS_FILE\n";
    exit(1);
  }

  // Construct a Graph
  typedef Graph<int, int> GraphType;
  GraphType graph;
  std::vector<GraphType::node_type> nodes;

  // Create a nodes_file from the first input argument
  std::ifstream nodes_file(argv[1]);
  // Interpret each line of the nodes_file as a 3D Point and add to the Graph
  Point p;
  while (CME212::getline_parsed(nodes_file, p))
    nodes.push_back(graph.add_node(p));

  // Create a tets_file from the second input argument
  std::ifstream tets_file(argv[2]);
  // Interpret each line of the tets_file as four ints which refer to nodes
  std::array<int,4> t;
  while (CME212::getline_parsed(tets_file, t))
    for (unsigned i = 1; i < t.size(); ++i)
      for (unsigned j = 0; j < i; ++j)
        graph.add_edge(nodes[t[i]], nodes[t[j]]);

  // Print out the stats
  std::cout << graph.num_nodes() << " " << graph.num_edges() << std::endl;

  // Launch the SDLViewer
  CME212::SDLViewer viewer;
  viewer.launch();
  auto node_map = viewer.empty_node_map(graph);

  Point pref = Point(-1, 0, 1);
  int path = shortest_path_lengths(graph, pref);
  PathColorFn pcf = PathColorFn(path);
  viewer.add_nodes(graph.node_begin(), graph.node_end(), pcf, node_map);

  // Test the PositionColorFn, the color is presented according to the nodes' x coordinats.
  //PositionColorFn pocf = PositionColorFn();
  //viewer.add_nodes(graph.node_begin(), graph.node_end(), pocf, node_map);

  viewer.add_edges(graph.edge_begin(), graph.edge_end(), node_map);
  viewer.center_view();

  return 0;
}

int main(int argc, char* argv[])
{
  // Check arguments
  if (argc < 2) {
    std::cerr << "Usage: " << argv[0] << " NODES_FILE TETS_FILE\n";
    exit(1);
  }

  // Construct a Graph
  typedef Graph<int> GraphType;
  GraphType graph;
  std::vector<GraphType::node_type> nodes;

  // Create a nodes_file from the first input argument
  std::ifstream nodes_file(argv[1]);
  // Interprit each line of the nodes_file as a 3D Point and add to the Graph
  Point p;
  while (CS207::getline_parsed(nodes_file, p))
    nodes.push_back(graph.add_node(p));

  // Create a tets_file from the second input argument
  std::ifstream tets_file(argv[2]);
  // Interprit each line of the tets_file as four ints which refer to nodes
  std::array<int,4> t;
  while (CS207::getline_parsed(tets_file, t))
    for (unsigned i = 1; i < t.size(); ++i)
      for (unsigned j = 0; j < i; ++j)
        graph.add_edge(nodes[t[i]], nodes[t[j]]);

  // Print out the stats
  std::cout << graph.num_nodes() << " " << graph.num_edges() << std::endl;

  // Launch the SDLViewer
  CS207::SDLViewer viewer;
  viewer.launch();

  // Use shortest_path_lengths to set the node values to the path lengths
  // Construct a Color functor and view with the SDLViewer
  auto node_map = viewer.empty_node_map(graph);
  int distance = shortest_path_lengths(graph, {-1,0,1});
  viewer.add_nodes(graph.node_begin(), graph.node_end(), Color(distance), node_map);
  viewer.add_edges(graph.edge_begin(), graph.edge_end(), node_map);

  return 0;
}

int LasySnapping::runMaxflow()
{   
    const float INFINNITE_MAX = 1e10;
    int indexPt = 0;
    for(int h = 0; h < image->height; h ++){
        unsigned char* p = (unsigned char*)image->imageData + h *image->widthStep;
        for(int w = 0; w < image->width; w ++){
            // calculate energe E1
            float e1[2]={0};
            if(isPtInVector(cvPoint(w,h),forePts)){
                e1[0] =0;
                e1[1] = INFINNITE_MAX;
            }else if(isPtInVector(cvPoint(w,h),backPts)){
                e1[0] = INFINNITE_MAX;
                e1[1] = 0;
            }else {
                getE1(p,e1);
            }
            // add node
            graph->add_node();
            graph->add_tweights(indexPt, e1[0],e1[1]);
            // add edge, 4-connect
            if(h > 0 && w > 0){
                float e2 = getE2(p,p-3);
                graph->add_edge(indexPt,indexPt-1,e2,e2);
                e2 = getE2(p,p-image->widthStep);
                graph->add_edge(indexPt,indexPt-image->width,e2,e2);
            }
            
            p+= 3;
            indexPt ++;            
        }
    }
    
    return graph->maxflow();
}

void Preprocess::GraphCutBinarize(bool shiftDown, int xyDivs)
{
	float alpha_B, alpha_F, P_I;
	MinErrorThresholding(&alpha_B, &alpha_F, &P_I, false);

	//Some times you need to shift the means down.
	if(shiftDown)
	{
		alpha_F = std::max((alpha_F)/2,((alpha_B)+(alpha_F))/2);
		alpha_B = (alpha_B)/1.5;
	}

	//Compute the poisson probs 
	double F_H[256], B_H[256];
	for(int i=0; i<256; i++)
    {
		if( i >= alpha_F )
			F_H[i] = (1-P_I)*ComputePoissonProb((int)alpha_F,alpha_F);
		else
			F_H[i] = (1-P_I)*ComputePoissonProb(i,alpha_F);

		if( i <= alpha_B )
			B_H[i] = P_I*ComputePoissonProb(int(alpha_B),alpha_B);
		else
			B_H[i] = P_I*ComputePoissonProb(i,alpha_B);
    }

	ImageType3D::SizeType size = myImg->GetLargestPossibleRegion().GetSize();

	for(int i=0; i<(int)size[0]; i+=(int)size[0]/xyDivs)
	{
		for(int j=0; j<(int)size[1]; j+=(int)size[1]/xyDivs)
		{
			ImageType3D::IndexType rIndexStart;
			rIndexStart[0] = i;
			rIndexStart[1] = j;
			rIndexStart[2] = 0;
			ImageType3D::SizeType rIndexEnd;		//Plus 1
			rIndexEnd[0] = (int)(i + size[0]/xyDivs + 1);
			rIndexEnd[1] = (int)(j + size[1]/xyDivs + 1);
			if(rIndexEnd[0] > size[0])
				rIndexEnd[0] = size[0];
			if(rIndexEnd[1] > size[1])
				rIndexEnd[1] = size[1];

			ImageType3D::SizeType rSize;
			rSize[0] = rIndexEnd[0] - rIndexStart[0];
			rSize[1] = rIndexEnd[1] - rIndexStart[1];
			rSize[2] = size[2];

			//long num_nodes = size[0]*size[1]*size[2];
			//long num_edges = 3*(size[0]-1)*(size[1]-1)*(size[2]-1);
			long num_nodes = rSize[0]*rSize[1]*rSize[2];
			long num_edges = 3*(rSize[0]-1)*(rSize[1]-1)*(rSize[2]-1);

			//Construct a graph:
			typedef Graph_B<short,short,short> GraphType;
			GraphType * g = new GraphType(/*estimated # of nodes*/ num_nodes, /*estimated # of edges*/ num_edges);

			ImageType3D::RegionType rRegion;
			rRegion.SetIndex(rIndexStart);
			rRegion.SetSize(rSize);

			typedef itk::ImageRegionIteratorWithIndex< ImageType3D > IteratorType;
			IteratorType it( myImg, rRegion );

			//ADD NODES:
			for( it.GoToBegin(); !it.IsAtEnd(); ++it )
			{
				int intst = (int)it.Get();
				ImageType3D::IndexType index = it.GetIndex();
				//int curr_node = (index[2]*size[1]*size[0])+(index[1]*size[0])+index[0];
				int curr_node = ((index[2]-rIndexStart[2])*rSize[1]*rSize[0])+((index[1]-rIndexStart[1])*rSize[0])+(index[0]-rIndexStart[0]);

				//First Add Nodes
				double Df = -log( F_H[intst] );  //it was multiplied by .5                              
				if(Df > 1000.0)
					Df = 1000;
				double Db = -log( B_H[intst] );                  
				if(Db > 1000.0)
					Db=1000;     			
					        				
				g->add_node();
				g->add_tweights( curr_node, /* capacities */ Df, Db );
			}

			//ADD EDGES:
			double sig = 30.0; //30.0;
			double w = 10.0; //10.0;
			for( it.GoToBegin(); !it.IsAtEnd(); ++it )
			{
				ImageType3D::IndexType index = it.GetIndex();
				//int curr_node = (index[2]*size[1]*size[0])+(index[1]*size[0])+index[0];
				int curr_node = ((index[2]-rIndexStart[2])*rSize[1]*rSize[0])+((index[1]-rIndexStart[1])*rSize[0])+(index[0]-rIndexStart[0]);
				int intst = (int)it.Get();

				for(int i=0; i<3; ++i)
				{
					ImageType3D::IndexType index2 = index;
					index2[i]++;
//.........这里部分代码省略.........

//.........这里部分代码省略.........

    // 0 means a source and 1 means a sink vertex for current round; 2 means a vertex connected with a feature edge
    std::vector<int> featureFlagMap = vertexMap;

    // Initialize the eigen solver for linear system
    Eigen::SparseLU< Eigen::SparseMatrix<double, Eigen::ColMajor>, Eigen::COLAMDOrdering<int> > solver;

    //iteratively graph cut
    while (count < limitCutTimes)
    {
        //std::cout << "cut " << count << " times" << endl;
        unsigned fid = std::max_element(gcFeatureAward.begin(), gcFeatureAward.end()) - gcFeatureAward.begin();
        if (gcFeatureAward[fid] <= -sm_largeDouble) break;
        gcFeatureAward[fid] = -sm_largeDouble;

        std::vector<unsigned> features = gcFeatures[fid];
        std::vector<unsigned> sgfs = sgFeature[fid];
        sgFeature[fid].clear();

        while (!sgfs.empty())
        {
            unsigned sgl = sgfs.back();	sgfs.pop_back();
            SGIter s_it = sgIters[sgl];

            if (s_it->edges.empty())
                continue;

            //make graph
            for (int i = 0; i < (int)s_it->vers.size(); i++){
                vertexMap[s_it->vers[i]] = i;
            }
            GraphType *g = new GraphType(s_it->vers.size(), s_it->edges.size());

            g->add_node(s_it->vers.size());

            for (int i = 0; i < (int)s_it->edges.size(); i++){
                if (gph.es[s_it->edges[i]].isCut) continue;
                g->add_edge(vertexMap[gph.es[s_it->edges[i]].n1], vertexMap[gph.es[s_it->edges[i]].n2], gph.es[s_it->edges[i]].w, gph.es[s_it->edges[i]].w);
            }

            // Do potential feature grouping
            FeatureEdgeExtention(gph, gcFeatures, features, vertexMap, featureFlagMap, s_it, solver, g, myPolarizedThreshold);

            double flow = g->maxflow();

            count++;

            //update cut edge;
            for (int i = 0; i < (int)s_it->edges.size(); i++){
                if (g->what_segment(vertexMap[gph.es[s_it->edges[i]].n1]) != g->what_segment(vertexMap[gph.es[s_it->edges[i]].n2]))
                    gph.es[s_it->edges[i]].isCut = true;
            }

            for (int i = 0; i < (int)s_it->vers.size(); i++)
            {
                vertexMap[s_it->vers[i]] = -1;
                featureFlagMap[s_it->vers[i]] = -1;
            }

            //update subgraph;

            std::vector<unsigned> newLabel(1, sgl);
            std::vector<MGraphCut::SubGraph> newSG(1);

            for (unsigned i = 0; i < s_it->vers.size(); i++)
                visitedVer[s_it->vers[i]] = false;

//.........这里部分代码省略.........
	cout<<"feature number:"<<gcFeatures.size()<<endl;

	clock_t tstr = clock();
	//run graph cut
	//important datas: vertexLabel, edgeLabel, subgraphs, labelId, sgFeatures;

	unsigned count=0; //count how many "graph-cuts".
	std::vector<int> vertexMap(vlb.size(),-1);
// 	std::vector<bool> visitedVer(gph.vNum,false);
// 	std::vector<bool> visitedEdge(gph.eNum,false);
	while (count<limitCutTimes){//iteratively graph cut
// 		if(count%10 == 0) cout<<"cut "<<count<<" times"<<endl;
		unsigned fid = std::max_element(gcFeatureAward.begin(),gcFeatureAward.end()) - gcFeatureAward.begin();
        if (gcFeatureAward[fid] <= -sm_largeDouble) break;
        gcFeatureAward[fid] = -sm_largeDouble;

		std::vector<unsigned> features = gcFeatures[fid];
		std::vector<unsigned> sgfs = sgFeature[fid]; 
		sgFeature[fid].clear();

		while(!sgfs.empty()){
			unsigned sgl = sgfs.back();	sgfs.pop_back();
			SGIter s_it = sgIters[sgl];

			if(s_it->edges.empty())
				continue;

			//make graph
            for (int i = 0; i<(int)s_it->vers.size(); i++){
				vertexMap[s_it->vers[i]]=i;
			}
			GraphType *g = new GraphType(s_it->vers.size(),s_it->edges.size()); 

			g->add_node(s_it->vers.size());

            for (int i = 0; i<(int)s_it->edges.size(); i++){
				if(gph.es[s_it->edges[i]].isCut) continue;
				g->add_edge( vertexMap[ gph.es[s_it->edges[i]].n1], vertexMap[ gph.es[s_it->edges[i]].n2] , gph.es[s_it->edges[i]].w,gph.es[s_it->edges[i]].w );	
			}

			for(unsigned i=0;i<features.size();i++){//add source & sink;
				int vsrc = gph.es[features[i]].n1;
				int vsin = gph.es[features[i]].n2;
				if(!gph.es[features[i]].ort){
					vsrc = gph.es[features[i]].n2;
					vsin = gph.es[features[i]].n1;
				}

				vsrc = vertexMap[vsrc];
				vsin = vertexMap[vsin];

				if(vsrc == -1 || vsin == -1) continue;

                g->add_tweights(vsrc, sm_largeDouble, 0);
                g->add_tweights(vsin, 0, sm_largeDouble);
			}

			double flow = g->maxflow();

			count++;

			//update cut edge;
            for (int i = 0; i<(int)s_it->edges.size(); i++){
				if (g->what_segment(vertexMap[gph.es[s_it->edges[i]].n1]) != g->what_segment(vertexMap[gph.es[s_it->edges[i]].n2]))
					gph.es[s_it->edges[i]].isCut = true;
			}

void mexFunction(int nlhs,	/* number of expected outputs */
	mxArray *plhs[],					/* mxArray output pointer array */
	int nrhs, 								/* number of inputs */
	const mxArray *prhs[]			/* mxArray input pointer array */)
{
	unsigned int nodes;
	unsigned int pairs;
	bool *labels;
	unsigned int *source;
	unsigned int *destination;
	float *pairValue;
	float *unaryValue;
	float *flow;
	mxArray *sourceMxArray;
	mxArray *destinationMxArray;
	mxArray *pairValueMxArray;

	/** Input validation */
	if( nrhs != 2 )
		mexErrMsgTxt( USAGE_NOTIFICATION );
	
	const mxArray *A = prhs[ 0 ];
	const mxArray *T = prhs[ 1 ];
	
	if( !mxIsStruct( A ) )
	{
		mexErrMsgTxt( USAGE_NOTIFICATION );
	}
	else
	{
		sourceMxArray = mxGetField( A, 0, "source" );
		destinationMxArray = mxGetField( A, 0, "destination" );
		pairValueMxArray = mxGetField( A, 0, "value" );
		
		if( mxGetClassID( sourceMxArray ) != mxUINT32_CLASS ||
			mxGetClassID( destinationMxArray ) != mxUINT32_CLASS ||
			mxGetClassID( pairValueMxArray ) != mxSINGLE_CLASS )
		{
			mexErrMsgTxt( USAGE_NOTIFICATION );
		}
		
		if( mxIsComplex( sourceMxArray ) ||
			mxIsComplex( destinationMxArray ) ||
			mxIsComplex( pairValueMxArray ) )
		{
			mexErrMsgTxt( USAGE_NOTIFICATION );
		}
		
		pairs = mxGetNumberOfElements( sourceMxArray );
		if( pairs != mxGetNumberOfElements( destinationMxArray ) ||
			pairs != mxGetNumberOfElements( pairValueMxArray ) )
		{
			mexErrMsgTxt( USAGE_NOTIFICATION );
		}
	}
	
	if( mxGetClassID( T ) != mxSINGLE_CLASS )
	{
		mexErrMsgTxt( USAGE_NOTIFICATION );
	}
	else
	{
		if( mxIsComplex( T ) )
			mexErrMsgTxt( USAGE_NOTIFICATION );
	
		nodes = mxGetM( T );

		if( mxGetN( T ) != 2 )
			mexErrMsgTxt( USAGE_NOTIFICATION );
	}
	/** End of input validation */
	
	source = ( unsigned int * )mxGetData( sourceMxArray );
	destination = ( unsigned int * )mxGetData( destinationMxArray );
	pairValue = ( float * )mxGetData( pairValueMxArray );
	unaryValue = ( float * )mxGetData( T );
	
	typedef Graph< float, float, float > GraphType;
	GraphType *graph = new GraphType( nodes, pairs );
	graph->add_node( nodes );
	
	/** Add pairwise potentials */
	for( int edge = 0; edge < pairs; edge++ )
		graph->add_edge( source[ edge ], destination[ edge ], pairValue[ edge ], pairValue[ edge ] );
	
	for( int node = 0; node < nodes; node++ )
		graph->add_tweights( node, unaryValue[ node ], unaryValue[ node + nodes ] );
	
	/** Create outputs */
	plhs[ 0 ] = mxCreateNumericMatrix( 1, 1, mxSINGLE_CLASS, mxREAL );
	plhs[ 1 ] = mxCreateLogicalMatrix( nodes, 1 );
	
	flow = ( float * )mxGetData( plhs[ 0 ] );
	labels = ( bool * )mxGetData( plhs[ 1 ] );
	
	*flow = graph->maxflow();
	
	for( int node = 0; node < nodes; node++ )
		labels[ node ] = graph->what_segment( node );
	
//.........这里部分代码省略.........

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

	//printf("1\n");
//declare variables
    mxArray *D, *PairW, *tU, *kIn, *alpha;
    const mwSize *dimsPairW, *dimsD;
    double *alphaPtr, *DPtr, *PairWPtr, *tUPtr, *kInPtr;
    int dimxPairW, dimyPairW, dimzPairW, dimxD, dimyD;
    int i,j;

//associate inputs
   
	//printf("2\n");
    D = mxDuplicateArray(prhs[0]);
    PairW = mxDuplicateArray(prhs[1]);
    //tU = mxDuplicateArray(prhs[2]);
    //kIn = mxDuplicateArray(prhs[3]);
//figure out dimensions
    
	//printf("3\n");
    dimsPairW = mxGetDimensions(prhs[1]);
    dimyPairW = (int)dimsPairW[0]; dimxPairW = (int)dimsPairW[1]; dimzPairW = (int)dimsPairW[2];
    dimsD = mxGetDimensions(prhs[0]);
    dimyD = (int)dimsD[0]; dimxD = (int)dimsD[1];
    

	//printf("4\n");
//associate outputs
    alpha = plhs[0] = mxCreateDoubleMatrix(1,dimxPairW,mxREAL);
    
	//printf("5\n");
//associate pointers
    alphaPtr = mxGetPr(alpha);
    DPtr = mxGetPr(D);
    PairWPtr = mxGetPr(PairW);
    //tUPtr = mxGetPr(tU);
    //kInPtr = mxGetPr(kIn);
	//printf("6\n");
    typedef Graph<double,double,double> GraphType;
	int numNodes=dimxPairW;
	GraphType *g = new GraphType(/*estimated # of nodes*/ numNodes, /*estimated # of edges*/ numNodes*6); 
	g->add_node(numNodes);
    // loop through pixels
    
    /*
        for(i=0;i<dimxPairW;i++)
    {
            //mexPrintf("D[0][%d] = %f\n",i,DPtr[i*2]);
            //mexPrintf("D[1][%d] = %f\n",i,DPtr[i*2+1]);
        for(j=0;j<dimyPairW;j++)
        {
            int xy = j+i*dimyPairW;
            int matSize = dimxPairW*dimyPairW;
            
            //mexPrintf("V[%d][%d][0] = %f\n",j,i,PairWPtr[xy]);
            mexPrintf("V[%d][%d][1] = %f\n",j,i,PairWPtr[matSize + xy - 1]);
        }
    }
    */
    
    for (i=0;i<dimxPairW;i++)
    {

        //if (i % 10000 == 0)
	//printf("7: %d/%d\n", i, dimxPairW);
        //if (((int)tUPtr[i])==1) {
            //printf("7: %d/%d\n", i, dimxPairW);
            g->add_tweights(i,DPtr[2*i+1],DPtr[2*i]);
        //}
        /*else {
            //printf("7: %d/%d\n", i, dimxPairW);
            g->add_tweights(i,0,kInPtr[0]);
        }*/
        for (j=0;j<dimyPairW;j++)
        {
            int xy = j+i*dimyPairW;
            int matSize = dimxPairW*dimyPairW;
            
	// printf("8: %d/%d\n", j, dimyPairW);
            if ((int)PairWPtr[matSize + xy]-1<=0) {
	//printf("8-2\n");
        continue;
            }
            else {
	// printf("8-3: i=%d, j=%d, dimyPairW=%d, dimxPairW=%d, dimzPairW=%d, dimxD=%d, dimyD=%d, matSize=%d, xy=%d ---- ", i, j, dimyPairW, dimxPairW, dimzPairW, dimxD, dimyD, matSize, xy);
	// printf("8-3-2: PairWPtr1=%d, PairWPtr2=%f\n", (int)PairWPtr[matSize + xy]-1, PairWPtr[xy]);
    g->add_edge(i,(int)PairWPtr[matSize + xy]-1,PairWPtr[xy],PairWPtr[xy]);
            }
        }
    }
    double flow = g->maxflow();
	printf("Flow = %f\n", flow);
	//printf("9\n");
    for (i=0;i<dimxPairW;i++)
	{
	//printf("10: %d/%d\n", i, dimxPairW);
		if (g->what_segment(i) == GraphType::SOURCE) alphaPtr[i]=1;
		else alphaPtr[i]=0;
    }
//.........这里部分代码省略.........

void mexFunction(
		 int          nlhs,
		 mxArray      *plhs[],
		 int          nrhs,
		 const mxArray *prhs[]
		 )
{
  /* Check for proper number of arguments */
  if (nrhs != 4) {
    mexErrMsgIdAndTxt("MATLAB:mexmaxflow:nargin", 
            "MEXCPP requires four input arguments.");
  } else if (nlhs != 1 && nlhs != 2) {
    mexErrMsgIdAndTxt("MATLAB:mexmaxflow:nargout",
            "MEXCPP requires one output argument.");
  }

  //parse out arguments

  double *backWeights = (double *) mxGetPr(prhs[0]);
  double *foreWeights = (double *) mxGetPr(prhs[1]);
  double *smoothIndices = (double *) mxGetPr(prhs[2]);
  double *smoothWeights = (double *) mxGetPr(prhs[3]);
  size_t numNodes = mxGetNumberOfElements(prhs[0]);
  size_t numDirections = mxGetN(prhs[2]);

  //Error checking
  if (numNodes != mxGetNumberOfElements(prhs[1])) mexErrMsgIdAndTxt("MATLAB:mexmaxflow:argin", "Weight arrays must be same length");
  if (numNodes != mxGetM(prhs[2])) mexErrMsgIdAndTxt("MATLAB:mexmaxflow:argin", "Number of rows for edge matrix does not match number of nodes");
  if (mxGetN(prhs[2]) != mxGetN(prhs[3]) || mxGetM(prhs[2]) != mxGetM(prhs[3])) mexErrMsgIdAndTxt("MATLAB:mexmaxflow:argin", "Edge weights matrix does not match edge indices matrix");

  //Create and fill graph
  typedef Graph<double, double, double> GraphType;
  GraphType *g = new GraphType(((int)numNodes),((int)(numNodes*numDirections)));
  g->add_node(((int)numNodes));

  //Background is source, foreground is sink
  for (size_t i=0; i<numNodes; i++)
  {
	g->add_tweights(i, backWeights[i], foreWeights[i]);
	for (size_t j=0; j<numDirections; j++)
	{
        //mexPrintf("Value1 = %g\n", smoothIndices[numNodes*j + i]);
		int edgeIndex = (int)(smoothIndices[numNodes*j + i]-1);
		double edgeWeight = smoothWeights[numNodes*j + i];
		if (edgeIndex < 0) continue;
        else if (edgeIndex >= numNodes) mexErrMsgIdAndTxt("MATLAB:mexmaxflow:argin", "Illegal edge index");
        else if (edgeIndex >= i) continue;
		g->add_edge(edgeIndex, i, edgeWeight, edgeWeight);
	}
  }

  //Calc flow
  double energy = g->maxflow();

  plhs[0] = mxCreateNumericMatrix(numNodes, 1, mxDOUBLE_CLASS, mxREAL);	
  double* arrPtr = mxGetPr(plhs[0]);
  //Set alpha
  for (size_t i=0; i<numNodes; i++)
  {
	  arrPtr[i] = g->what_segment(i) == GraphType::SINK;
  }
  plhs[1] = mxCreateDoubleScalar(energy);
  
  delete g;

  return;
}

// Column generation and prediction
void minimize(map<int, double*>& nodeFeatures,
              map<int,int>* nodeLabels,
              map<int, double*>& edgeFeatures,
              double* nodeTheta,
              double* edgeTheta,
              map<int,int>& res,
              int DN,
              int DE,
              double lossPositive,
              double lossNegative,
              map<int, pair<int,int> >& indexEdge,
              pair<int,double>* confidences,
              int colgen,
              map<int, double>* firstOrderResponses
             )
{
  int N = nodeFeatures.size();
  int E = edgeFeatures.size();
  
  typedef Graph<Cost,Cost,Cost> GraphType;
  GraphType* g = new GraphType(N, E);
  
  for (int i = 0; i < N; i ++)
    g->add_node();
  
  for (map<int, double*>::iterator it = nodeFeatures.begin(); it != nodeFeatures.end(); it ++)
  {
    Cost c = inner_product(it->second, nodeTheta, DN);
    if (confidences)
    {
      confidences[it->first].first = NEGATIVE;
      if (firstOrderResponses)
        confidences[it->first].second = firstOrderResponses->at(it->first);
      else
        confidences[it->first].second = c;
    }

    if (nodeLabels->at(it->first) == EVIDENCE_NEGATIVE) c = -1000;
    if (nodeLabels->at(it->first) == EVIDENCE_POSITIVE) c = 1000;

    double l0 = 0;
    double l1 = 0;
    if (colgen)
    {
      l0 = loss1(nodeLabels->at(it->first), NEGATIVE, lossPositive, lossNegative, LOSS);
      l1 = loss1(nodeLabels->at(it->first), POSITIVE, lossPositive, lossNegative, LOSS);
    }

    setNodeCost(g, it->first, c - l0, -c - l1);

    if (DE == 0)
    {
      if (c - l0 <= -c - l1)
        res[it->first] = NEGATIVE;
      else
        res[it->first] = POSITIVE;
      if (nodeLabels->at(it->first) == EVIDENCE_NEGATIVE)
        res[it->first] = EVIDENCE_NEGATIVE;
      if (nodeLabels->at(it->first) == EVIDENCE_POSITIVE)
        res[it->first] = EVIDENCE_POSITIVE;
    }
  }

  if (not DE)
  {
    delete g;
    return;
  }

  Cost* innerProducts00 = new Cost [E];
  
  #pragma omp parallel for
  for (int i = 0; i < E; i ++)
  {
    innerProducts00[i] = inner_product(edgeFeatures[i], edgeTheta, DE);
  }

  for (map<int, double*>::iterator it = edgeFeatures.begin(); it != edgeFeatures.end(); it ++)
  {    
    Cost c00 = innerProducts00[it->first];
    pair<int,int> edge = indexEdge[it->first]; 


    setEdgeCost(g, edge.first, edge.second, -c00, 0, 0, -c00);
  }
  
  delete [] innerProducts00;

  g->maxflow();
  
  for (int i = 0; i < N; i ++)
  {
    if (g->what_segment(i) == GraphType::SOURCE)
      res[i] = NEGATIVE;
    else
      res[i] = POSITIVE;
    if (nodeLabels->at(i) == EVIDENCE_POSITIVE)
    {
      if (res[i] != POSITIVE)
//.........这里部分代码省略.........

void mexFunction(int nOut, mxArray *pOut[], int nIn, const mxArray *pIn[]) {
  clock_t mexBegin = tic();

  const char *usage = "[energy, xmap, misc] = submodularMAPFull_mex(theta, pW, pots).\n"
                      " Solve MAP problem with overcomplete parameterization.\n"
                      " theta is nNodes x 2, first column is 0 potential, second is 1.\n"
                      " W is nNodes x nNodes sparse *indexing* a potential in pots, 1-based.\n"
                      " pots is nPots cell array, each 2x2.\n";

  if (nIn != 3) {
    mexErrMsgIdAndTxt("submodularMAP_mex:args", usage);
  }

  /////////////////////////////////////////////////
  // Extract arguments and outputs
  /////////////////////////////////////////////////
  Mat<double> theta         (pIn[0]);
  cscMatrix   W = extractCSC(pIn[1]);
  CellMat<Mat<double> > pots(pIn[2]);

  int nNodes = theta.M;
  mwSize nEdges = W.nzMax / 2; // symmetric mat
  int nPots  = pots.length;

  auto misc = StructMat(1, 1, {"maxFlow", "mexTotTime"});
  pOut[oMisc] = misc;

  std::vector<int> x(nNodes);

	typedef Graph<double,double,double> GraphType;
  GraphType *g = new GraphType(nNodes, nEdges + 2 * nNodes);
  g->add_node(nNodes);

  // Pairwise are normal (bidirectional) edges.
  mwIndex i, off;
  // Potential idx
  int pidx;
  double w;

  // Use Kolmogorov's submodular construction, Kolmogorov04 pp. 151
  // is there a cache-friendlier way to write this? does it even matter?
  std::vector<double> sAug(nNodes);
  std::vector<double> tAug(nNodes);

  for (mwIndex j = 0; j < nNodes; j++) {
    for (mwIndex wIdx = W.jc[j]; wIdx < W.jc[j+1]; wIdx++) {
      // Upper triangular only
      i = W.ir[wIdx];

      // some dirty trickery since we represent an int as a double.
      if (j > i) {
        if (pidx >= 0 && pidx < nPots) {
          pidx = W.pr[wIdx] - 1;
          // We may have weird numerical issues.
          if (pidx >= 0 && pidx < nPots) {
            sAug[i] += pots[pidx](1,0) - pots[pidx](0,0); // C - A
            tAug[j] += pots[pidx](1,0) - pots[pidx](1,1); // C - D
            w = pots[pidx](0,1) + pots[pidx](1,0) - pots[pidx](0,0) - pots[pidx](1,1); // B + C - A - D
            g->add_edge(i, j, w, 0);

            mexPrintf("%d->%d = %g\n", i, j, w);
          }
        }
      }
    }
  }

  double dTheta;
  double s;
  double t;
  for (int j = 0; j < nNodes; j++) {
    dTheta = theta(j,1) - theta(j,0);
    s      = MAX(0, dTheta)  + sAug[j];
    t      = MAX(0, -dTheta) + tAug[j];

    g->add_tweights(j, s, t);

    mexPrintf("s->%d = %g\t; %d->t = %g\t; sAug = %g\t; tAug = %g\n", j, s, j, t, sAug[j], tAug[j]);
  }

  double maxFlow = g->maxflow();

  for (int i = 0; i < nNodes; i++) {
    x[i] = g->what_segment(i) == GraphType::SINK;
    //mexPrintf("x[%d] = %d, ws = %d\n", i, x[i], g->what_segment(i));
  }
  misc.set("maxFlow", scalar<double>(maxFlow));

  // Compute final energies
  double energy = 0.0;
  for (mwIndex j = 0; j < nNodes; j++) {
    energy += theta(j,x[j]);

    for (mwIndex wIdx = W.jc[j]; wIdx < W.jc[j+1]; wIdx++) {
      // Upper triangular only
      i = W.ir[wIdx];
      if (j > i) {
        if (pidx >= 0 && pidx < nPots) {
          pidx    = W.pr[wIdx] - 1;
          energy += pots[pidx](x[i],x[j]);
//.........这里部分代码省略.........