lgra.txt

动态场景中运动目标检测提取与跟踪 对新手很有用

/* graph.cpp */
/* Vladimir Kolmogorov ([e-mail]), 2001. */

//#include <stdio.h>
//#include <time.h>
#include "stdafx.h"
#include "graph.h"

Graph::captype MAX(Graph::captype a, Graph::captype b)
{
  if (a>b) return a;
  else return b;
}

Graph::captype MIN(Graph::captype a, Graph::captype b)
{
  if (a>b) return b;
  else return a;
}


Graph::Graph(void (*err_function)(char *))
{
	error_function = err_function;
	node_block = new Block<node>(NODE_BLOCK_SIZE, error_function);
	arc_block  = new Block<arc>(NODE_BLOCK_SIZE, error_function);
	flow = 0;
}

Graph::~Graph()
{
	delete node_block;
	delete arc_block;
}

Graph::node_id Graph::add_node()
{
	node *i = node_block -> New();
	i -> first = NULL;
	i -> tr_cap = 0;
	return (node_id) i;
}

void Graph::add_edge(node_id from, node_id to, captype cap, captype rev_cap)
{
	arc *a, *a_rev;
	a = arc_block -> New(2);
	a_rev = a + 1;
	a -> sister = a_rev;
	a_rev -> sister = a;
	a -> next = ((node*)from) -> first;
	((node*)from) -> first = a;
	a_rev -> next = ((node*)to) -> first;
	((node*)to) -> first = a_rev;
	a -> head = (node*)to;
	a_rev -> head = (node*)from;

	a -> e_cap = cap;					////   This change was made to get the directed graph version
	a_rev -> e_cap = rev_cap;			////

	a -> r_cap = cap;
	a_rev -> r_cap = rev_cap;
}

void Graph::set_tweights(node_id i, captype cap_source, captype cap_sink)
{
	((node*)i) -> tr_cap = cap_source - cap_sink;
	((node*)i) -> t_cap = cap_source - cap_sink;
	((node*)i) -> con_flow = MIN(cap_source, cap_sink);
	flow += MIN(cap_source, cap_sink);
}

void Graph::add_tweights(node_id i, captype cap_source, captype cap_sink)
{
	register captype delta = ((node*)i) -> tr_cap;
	if (delta > 0) cap_source += delta;
	else           cap_sink   -= delta;
	flow += (cap_source < cap_sink) ? cap_source : cap_sink;
	((node*)i) -> tr_cap = cap_source - cap_sink;
	((node*)i) ->t_cap = cap_source - cap_sink;
}

void Graph::edit_tweights(node_id i, captype cap_source, captype cap_sink)
{
	node *f_node = (node*)i;
	
	f_node->tr_cap = (cap_source - cap_sink) - (f_node->t_cap - f_node->tr_cap);
	f_node->t_cap = cap_source - cap_sink;
}


void Graph::edit_edge(node_id from, node_id to, captype cap, captype rev_cap)
{
	arc *a, *a_rev;
	node *f_node = (node*)from;
	node *to_node = (node*)to;

	a=f_node->first;

	while((a!=NULL) && a!=a->next && a->head!=to_node )
		a= a->next;

	if (a->head!=to_node) printf("Error: Specified edge doesn't exist");
	else
	{
		captype ecap_orig, eflow, excess;
		a_rev= a->sister;
		ecap_orig = (a->r_cap + a_rev->r_cap)/2;
		eflow = (a_rev->r_cap - a->r_cap)/2;
		if (eflow<0) eflow*=-1;		

		if (cap>ecap_orig)
		{
			a->r_cap += (cap - ecap_orig);
			a_rev->r_cap += (cap - ecap_orig);
		}
		else if (cap<ecap_orig)
		{
			if (eflow<=cap)
			{
				a->r_cap -= (ecap_orig - cap);
				a_rev->r_cap -= (ecap_orig - cap);
			}
			else
			{
				excess = eflow - cap;

				if (a->r_cap < a->sister->r_cap)
				{

					a->r_cap = 0;
					a_rev->r_cap = 2*cap;

					f_node->tr_cap += excess;
					to_node->tr_cap -= excess;
				}
				else
				{
					a->r_cap = 2*cap;
					a_rev->r_cap = 0;

					to_node->tr_cap += excess;
					f_node->tr_cap -= excess;
				}				
			}
		}
	}
}


void Graph::edit_tweights_tree(node_id i, captype cap_source, captype cap_sink)
{
	node *f_node = (node*)i;
	if (f_node->t_cap != cap_source - cap_sink) 
	{
		if (f_node->t_cap>0) flow -= MIN(f_node->t_cap-f_node->tr_cap,f_node->t_cap);
		else flow -= -1*MAX(0,f_node->tr_cap);

		f_node->tr_cap = (cap_source - cap_sink) - (f_node->t_cap - f_node->tr_cap);
		f_node->t_cap = cap_source - cap_sink;

		if (f_node->t_cap>0) flow += MIN(f_node->t_cap-f_node->tr_cap,f_node->t_cap);
		else flow += -1*MAX(0,f_node->tr_cap);

		settle_terminal(f_node);
	}

	flow -= f_node->con_flow; 
	f_node -> con_flow = MIN(cap_source, cap_sink);
	flow += MIN(cap_source, cap_sink);
}


void Graph::edit_edge_tree(node_id from, node_id to, captype cap, captype rev_cap)
{
	arc *a, *a_rev;
	node *f_node = (node*)from;
	node *to_node = (node*)to;

	a=f_node->first;

	while((a!=NULL)&&(a!=a->next)&&(a->head!=to_node))
		a= a->next;

	if (a->head!=to_node) printf("Error: Specified edge doesn't exist");
	else
	{
		// Modifying flow value 

		if (f_node->t_cap>0) flow -= MIN(f_node->t_cap-f_node->tr_cap,f_node->t_cap);
		else flow -= -1*MAX(0,f_node->tr_cap);

		if (to_node->t_cap>0) flow -= MIN(to_node->t_cap-to_node->tr_cap,to_node->t_cap);
		else flow -= -1*MAX(0,to_node->tr_cap);




		captype ecap_orig, eflow, excess;
		a_rev = a->sister;
		eflow = a->e_cap - a->r_cap;

		if (eflow>0)
		{
			if (cap>a->e_cap)
			{
				if (eflow>=a->e_cap)
				{
					if ((f_node->parent!= NULL)&&(f_node->parent!= ORPHAN)) set_active(f_node);
					if ((to_node->parent!= NULL)&&(to_node->parent!= ORPHAN)) set_active(to_node);
				}
				a->r_cap += (cap - a->e_cap);
				a_rev->r_cap += (rev_cap - a_rev->e_cap);
			}
			else if (cap<a->e_cap)
			{
				if (eflow<=cap)
				{
					a->r_cap -= (a->e_cap - cap);
					a_rev->r_cap -= (a_rev->e_cap - rev_cap);
					
					if (eflow == cap)
					{
						if (f_node->parent==a) make_orphan(f_node);
						else if (to_node->parent==a) make_orphan(to_node);
					}
				}
				else
				{
					excess = eflow - cap;	
					a->r_cap = 0;
					a_rev->r_cap = rev_cap + cap;


					f_node->tr_cap += excess;
					to_node->tr_cap -= excess;

					if (f_node->tr_cap!=0) settle_terminal(f_node);
					else if (f_node->parent==a) make_orphan(f_node);

					if (to_node->tr_cap!=0) settle_terminal(to_node);
					else if (to_node->parent==a) make_orphan(to_node);
					
				}
			}
		}
		else 
		{
			eflow *=-1;
			if (rev_cap>a_rev->e_cap)
			{
				if (eflow>=a_rev->e_cap)
				{
					if ((f_node->parent!= NULL)&&(f_node->parent!= ORPHAN)) set_active(f_node);
					if ((to_node->parent!= NULL)&&(to_node->parent!= ORPHAN)) set_active(to_node);
				}				
				a->r_cap += (cap - a->e_cap);
				a_rev->r_cap += (rev_cap - a_rev->e_cap);
			}
			else if (rev_cap<a_rev->e_cap)
			{
				if (eflow<=rev_cap)
				{
					a->r_cap -= (a->e_cap - cap);
					a_rev->r_cap -= (a_rev->e_cap - rev_cap);

					if (eflow == cap)
					{
						if (f_node->parent==a_rev) make_orphan(f_node);
						else if (to_node->parent==a_rev) make_orphan(to_node);
					}
				}
				else
				{
					excess = eflow - rev_cap;	
					a_rev->r_cap = 0;
					a->r_cap = rev_cap + cap;

					f_node->tr_cap -= excess;
					to_node->tr_cap += excess;

					if (f_node->tr_cap!=0) settle_terminal(f_node);
					else if (f_node->parent==a) make_orphan(f_node);

					if (to_node->tr_cap!=0) settle_terminal(to_node);
					else if (to_node->parent==a) make_orphan(to_node);
				}
			}
		}
		a->e_cap = cap;
		a_rev->e_cap = rev_cap;


		// Modifying flow value 

		if (f_node->t_cap>0) flow += MIN(f_node->t_cap-f_node->tr_cap,f_node->t_cap);
		else flow += -1*MAX(0,f_node->tr_cap);

		if (to_node->t_cap>0) flow += MIN(to_node->t_cap-to_node->tr_cap,to_node->t_cap);
		else flow += -1*MAX(0,to_node->tr_cap);


	}

}


void Graph::settle_terminal(node *f_node)
{ 
	arc *a;
		
	
	if ((f_node->tr_cap==0)&&(f_node->parent==TERMINAL)&&(f_node->parent!=ORPHAN)) make_orphan(f_node);
	else if (f_node->is_sink==1)
	{
		if(f_node->tr_cap>0)
		{
			f_node->parent = TERMINAL;
			f_node->is_sink = 0;
			f_node->TS = TIME;
			f_node->DIST=1;
			set_active(f_node);

			for (a = f_node->first; a!=NULL; a = a->next)
				if ((a->head->parent!=NULL)&&(a->head->parent!=TERMINAL)&&(a->head->parent!=ORPHAN)&&(a->head->parent->head == f_node)) make_orphan(a->head);
		}
		else if ((f_node->tr_cap<0)&&(f_node->parent!=TERMINAL)) 
		{
			f_node->is_sink = 1;
			f_node->parent = TERMINAL;
			f_node->TS = TIME;
			f_node->DIST=1;
			set_active(f_node);
			
		}
	}
	else
	{
		if(f_node->tr_cap<0)
		{
			f_node->parent = TERMINAL;
			f_node->is_sink = 1;
			f_node->TS = TIME;
			f_node->DIST=1;
			set_active(f_node);

			for (a = f_node->first; a!=NULL; a = a->next)
				if ((a->head->parent!=NULL)&&(a->head->parent!=TERMINAL)&&(a->head->parent!=ORPHAN)&&(a->head->parent->head == f_node)) make_orphan(a->head);
		}
		else if ((f_node->tr_cap>0)&&(f_node->parent!=TERMINAL)) 
		{
			f_node->is_sink = 0;
			f_node->parent = TERMINAL;
			f_node->TS = TIME;
			f_node->DIST=1;
			set_active(f_node);
		}
	}
}


Graph::flowtype Graph::getflow()
{
	Graph::flowtype newflow=0;

	node *i;

	for (i=node_block->ScanFirst(); i; i=node_block->ScanNext())
	{
		if (i->t_cap>0) newflow += MIN(i->t_cap-i->tr_cap,i->t_cap);
		else newflow += -1*MAX(0,i->tr_cap);
	}

	return newflow;
}

/* 	special constants for node->parent */

//			#define TERMINAL ( (arc *) 1 )		/* to terminal */			definition shifted to graph.h 
//			#define ORPHAN   ( (arc *) 2 )		/* orphan */

#define INFINITE_D 1000000000		/* infinite distance to the terminal */

/***********************************************************************/

/*
	Functions for processing active list. 	i->next points to the next node in the list
	(or to i, if i is the last node in the list). If i->next is NULL iff i is not in the list.

	There are two queues. Active nodes are added to the end of the second queue and read from
	the front of the first queue. If the first queue is empty, it is replaced by the second queue
	(and the second queue becomes empty).
*/

inline void Graph::set_active(node *i)
{
	if (!i->next)
	{
		/* it's not in the list yet */
		if (queue_last[1]) queue_last[1] -> next = i;
		else               queue_first[1]        = i;
		queue_last[1] = i;
		i -> next = i;
	}
}

//	Returns the next active node. If it is connected to the sink, it stays in the list, otherwise it is removed from the list

inline Graph::node * Graph::next_active()
{
	node *i;

	while ( 1 )
	{
		if (!(i=queue_first[0]))
		{
			queue_first[0] = i = queue_first[1];
			queue_last[0]  = queue_last[1];
			queue_first[1] = NULL;
			queue_last[1]  = NULL;
			if (!i) return NULL;
		}

		/* remove it from the active list */
		if (i->next == i) queue_first[0] = queue_last[0] = NULL;
		else              queue_first[0] = i -> next;
		i -> next = NULL;

		/* a node in the list is active iff it has a parent */
		if (i->parent) return i;
	}
}


/* Functions for processing orphan list */

void Graph::make_orphan(node *i)
{
	nodeptr *np;
	if (i->parent!=ORPHAN)
	{
		i->parent = ORPHAN;
		np = nodeptr_block -> New();
		np->ptr = i;
		np->next = orphan_first;
		orphan_first = np;
	}
}


/***********************************************************************/

void Graph::maxflow_init()
{
	node *i;

	queue_first[0] = queue_last[0] = NULL;
	queue_first[1] = queue_last[1] = NULL;
	orphan_first = NULL;

	for (i=node_block->ScanFirst(); i; i=node_block->ScanNext())
	{
		i -> next = NULL;
		i -> TS = 0;
		if (i->tr_cap > 0)
		{
			/* i is connected to the source */
			i -> is_sink = 0;
			i -> parent = TERMINAL;
			set_active(i);
			i -> TS = 0;
			i -> DIST = 1;
		}
		else if (i->tr_cap < 0)
		{
			/* i is connected to the sink */
			i -> is_sink = 1;
			i -> parent = TERMINAL;
			set_active(i);
			i -> TS = 0;
			i -> DIST = 1;
		}
		else
		{
			i -> parent = NULL;
		}
	}
	TIME = 0;
}

/***********************************************************************/

void Graph::augment(arc *middle_arc)
{
	node *i;
	arc *a;
	captype bottleneck;
	nodeptr *np;


	/* 1. Finding bottleneck capacity */
	/* 1a - the source tree */
	bottleneck = middle_arc -> r_cap;
	for (i=middle_arc->sister->head; ; i=a->head)
	{
		a = i -> parent;
		if (a == TERMINAL) break;
		if (bottleneck > a->sister->r_cap) bottleneck = a -> sister -> r_cap;
	}
	if (bottleneck > i->tr_cap) bottleneck = i -> tr_cap;
	/* 1b - the sink tree */
	for (i=middle_arc->head; ; i=a->head)
	{
		a = i -> parent;
		if (a == TERMINAL) break;
		if (bottleneck > a->r_cap) bottleneck = a -> r_cap;
	}
	if (bottleneck > - i->tr_cap) bottleneck = - i -> tr_cap;


	/* 2. Augmenting */
	/* 2a - the source tree */
	middle_arc -> sister -> r_cap += bottleneck;
	middle_arc -> r_cap -= bottleneck;
	for (i=middle_arc->sister->head; ; i=a->head)
	{
		a = i -> parent;
		if (a == TERMINAL) break;
		a -> r_cap += bottleneck;
		a -> sister -> r_cap -= bottleneck;
		if (!a->sister->r_cap)
		{
			/* add i to the adoption list */
			i -> parent = ORPHAN;
			np = nodeptr_block -> New();
			np -> ptr = i;
			np -> next = orphan_first;
			orphan_first = np;
		}
	}
	i -> tr_cap -= bottleneck;
	if (!i->tr_cap)
	{
		/* add i to the adoption list */
		i -> parent = ORPHAN;
		np = nodeptr_block -> New();
		np -> ptr = i;
		np -> next = orphan_first;
		orphan_first = np;
	}
	/* 2b - the sink tree */
	for (i=middle_arc->head; ; i=a->head)
	{
		a = i -> parent;
		if (a == TERMINAL) break;
		a -> sister -> r_cap += bottleneck;
		a -> r_cap -= bottleneck;
		if (!a->r_cap)
		{
			/* add i to the adoption list */
			i -> parent = ORPHAN;
			np = nodeptr_block -> New();
			np -> ptr = i;
			np -> next = orphan_first;
			orphan_first = np;
		}
	}
	i -> tr_cap += bottleneck;
	if (!i->tr_cap)
	{
		/* add i to the adoption list */
		i -> parent = ORPHAN;
		np = nodeptr_block -> New();
		np -> ptr = i;
		np -> next = orphan_first;
		orphan_first = np;
	}

	flow += bottleneck;
}

/***********************************************************************/

void Graph::process_source_orphan(node *i)
{
	node *j;
	arc *a0, *a0_min = NULL, *a;
	nodeptr *np;
	int d, d_min = INFINITE_D;

	if (i->parent!=ORPHAN) return;

	/* trying to find a new parent */
	for (a0=i->first; a0; a0=a0->next)
	if (a0->sister->r_cap)
	{
		j = a0 -> head;
		a=j->parent;

		if (!j->is_sink && (a!=NULL) && (a!=ORPHAN))
		{
			/* checking the origin of j */
			d = 0;
			while ( 1 )
			{
				if (j->TS == TIME)
				{
					d += j -> DIST;
					break;
				}
				a = j -> parent;
				d ++;
				if (a==TERMINAL)
				{
					j -> TS = TIME;
					j -> DIST = 1;
					break;
				}
				if (a==ORPHAN) { d = INFINITE_D; break; }
				j = a -> head;
			}
			if (d<INFINITE_D) /* j originates from the source - done */
			{
				if (d<d_min)
				{
					a0_min = a0;
					d_min = d;
				}
				/* set marks along the path */
				for (j=a0->head; j->TS!=TIME; j=j->parent->head)
				{
					j -> TS = TIME;
					j -> DIST = d --;
				}
			}
		}
	}

	if (i->parent = a0_min)
	{
		i -> TS = TIME;
		i -> DIST = d_min + 1;