lgraph.cpp

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

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

//#include <stdio.h>
//#include <time.h>
#include "stdafx.h"
#include "lgraph.h"
LGraph::captype MAX(LGraph::captype a, LGraph::captype b)
{
  if (a>b) return a;
  else return b;
}

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


LGraph::LGraph(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;
}

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

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

void LGraph::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 LGraph::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 LGraph::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 LGraph::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 LGraph::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 LGraph::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 LGraph::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 LGraph::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);
		}
	}
}


LGraph::flowtype LGraph::getflow()
{
	LGraph::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 LGraph::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 LGraph::node * LGraph::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 LGraph::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 LGraph::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 LGraph::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 LGraph::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;