alg.c

motion motion

/*	alg.c
 *
 *	Detect changes in a video stream.
 *	Copyright 2001 by Jeroen Vreeken ([e-mail])
 *	This software is distributed under the GNU public license version 2
 *	See also the file 'COPYING'.
 *
 */
#include "motion.h"
#include "alg.h"

#ifdef __MMX__
#define HAVE_MMX
#include "mmx.h"
#endif

#define MAX2(x, y) ((x) > (y) ? (x) : (y))
#define MAX3(x, y, z) ((x) > (y) ? ((x) > (z) ? (x) : (z)) : ((y) > (z) ? (y) : (z)))

/* locate the center and size of the movement. */
void alg_locate_center_size(struct images *imgs, int width, int height, struct coord *cent)
{
	unsigned char *out=imgs->out;
	int *labels=imgs->labels;
	int x, y, centc=0, xdist=0, ydist=0;

	cent->x=0;
	cent->y=0;
	cent->maxx=0;
	cent->maxy=0;
	cent->minx=width;
	cent->miny=height;

	/* If Labeling enabled - locate center of largest labelgroup */
	if (imgs->labelsize_max) {
		/* Locate largest labelgroup */
		for (y=0; y<height; y++) {
			for (x=0; x<width; x++) {
				if (*(labels++)&32768) {
					cent->x += x;
					cent->y += y;
					centc++;
				}
			}
		}
	} else {
		/* Locate movement */
		for (y=0; y<height; y++) {
			for (x=0; x<width; x++) {
				if (*(out++)) {
					cent->x += x;
					cent->y += y;
					centc++;
				}
			}
		}
	}
	if (centc) {
		cent->x=cent->x/centc;
		cent->y=cent->y/centc;
	}
	
	/* Now we find the size of the Motion */

	/* First reset pointers back to initial value */
	centc=0;
	labels=imgs->labels;
	out=imgs->out;

	/* If Labeling then we find the area around largest labelgroup instead */
	if (imgs->labelsize_max) {
		for (y=0; y<height; y++) {
			for (x=0; x<width; x++) {
				if (*(labels++)&32768) {
					if (x > cent->x)
						xdist += x - cent->x;
					else if (x < cent->x)
						xdist += cent->x - x;
					if (y > cent->y)
						ydist += y - cent->y;
					else if (y < cent->y)
						ydist += cent->y - y;
					centc++;
				}
			}	
		}
	} else {
		for (y=0; y<height; y++) {
			for (x=0; x<width; x++) {
				if (*(out++)) {
					if (x > cent->x)
						xdist += x - cent->x;
					else if (x < cent->x)
						xdist += cent->x - x;
					if (y > cent->y)
						ydist += y - cent->y;
					else if (y < cent->y)
						ydist += cent->y - y;
					centc++;
				}
			}	
		}
	}
	
	if (centc) {
		cent->minx = cent->x - xdist/centc*2;
		cent->maxx = cent->x + xdist/centc*2;
		/* Make the box a little bigger in y direction to make sure the
		   heads fit in so we multiply by 3 instead of 2 which seems to
		   to work well in practical */
		cent->miny = cent->y - ydist/centc*3;
		cent->maxy = cent->y + ydist/centc*2;
	}
	if (cent->maxx > width - 1)
		cent->maxx = width - 1;
	else if (cent->maxx < 0)
		cent->maxx = 0;
	if (cent->maxy > height - 1)
		cent->maxy = height - 1;
	else if (cent->maxy < 0)
		cent->maxy = 0;
	if (cent->minx > width - 1)
		cent->minx = width - 1;
	else if (cent->minx < 0)
		cent->minx = 0;
	if (cent->miny > height - 1)
		cent->miny = height - 1;
	else if (cent->miny < 0)
		cent->miny = 0;
	
	cent->width = cent->maxx - cent->minx;
	cent->height = cent->maxy - cent->miny;
	
	/* We want to center Y coordinate to be the center of the action.
	   The head of a person is important so we correct the cent.y coordinate
	   to match the correction to include a persons head that we just did above */
	cent->y = (cent->miny + cent->maxy)/2;
	
}


/* draw a box around the movement */
void alg_draw_location(struct coord *cent, struct images *imgs, int width, unsigned char *new, int mode)
{
	unsigned char *out=imgs->out;
	int x, y;

	out=imgs->out;

	/* Draw a box around the movement */
	if (mode == LOCATE_BOTH){ /* both normal and motion image gets a box */
		int width_miny = width*cent->miny;
		int width_maxy = width*cent->maxy;
		for (x=cent->minx; x<=cent->maxx; x++) {
			int width_miny_x = x+width_miny;
			int width_maxy_x = x+width_maxy;
			new[width_miny_x]=~new[width_miny_x];
			new[width_maxy_x]=~new[width_maxy_x];
			out[width_miny_x]=~out[width_miny_x];
			out[width_maxy_x]=~out[width_maxy_x];
		}
		for (y=cent->miny; y<=cent->maxy; y++) {
			int width_minx_y = cent->minx+y*width; 
			int width_maxx_y = cent->maxx+y*width;
			new[width_minx_y]=~new[width_minx_y];
			new[width_maxx_y]=~new[width_maxx_y];
			out[width_minx_y]=~out[width_minx_y];
			out[width_maxx_y]=~out[width_maxx_y];
		}
	}
	else{ /* normal image only (e.g. preview shot) */
		int width_miny = width*cent->miny;
		int width_maxy = width*cent->maxy;
		for (x=cent->minx; x<=cent->maxx; x++) {
			int width_miny_x = width_miny+x;
			int width_maxy_x = width_maxy+x;
			new[width_miny_x]=~new[width_miny_x];
			new[width_maxy_x]=~new[width_maxy_x];
		}
		for (y=cent->miny; y<=cent->maxy; y++) {
			int minx_y = cent->minx+y*width;
			int maxx_y = cent->maxx+y*width;
			new[minx_y]=~new[minx_y];
			new[maxx_y]=~new[maxx_y];
		}
	}
}



#define NORM               100
#define ABS(x)             ((x)<0 ? -(x) : (x))
#define DIFF(x, y)         (ABS((x)-(y)))
#define NDIFF(x, y)        (ABS(x)*NORM/(ABS(x)+2*DIFF(x,y)))


void alg_noise_tune(struct context *cnt, unsigned char *new)
{
	struct images *imgs=&cnt->imgs;
	int i;
	unsigned char *ref=imgs->ref;
	int diff, sum=0, count=0;
	unsigned char *mask=imgs->mask;
	unsigned char *smartmask=imgs->smartmask_final;

	i=imgs->motionsize;
			
	for (; i>0; i--) {
		diff = ABS(*ref - *new);
		if (mask)
			diff = ((diff * *mask++)/255);
		if (*smartmask){
			sum += diff + 1;
			count++;
		}
		ref++;
		new++;
		smartmask++;
	}
	if (count > 3) { /* avoid divide by zero */
		sum /= count / 3;
	}
	cnt->noise = 4 + (cnt->noise + sum) / 2;  /* 5: safe, 4: regular, 3: more sensitive */
}

void alg_threshold_tune(struct context *cnt, int diffs, int motion)
{
	int i;
	int sum = 0, top = diffs;

	if (!diffs)
		return;

	if (motion)
		diffs = cnt->threshold / 4;

	for (i = 0; i < THRESHOLD_TUNE_LENGTH - 1; i++)
	{
		sum += cnt->diffs_last[i];
		if (cnt->diffs_last[i+1] && !motion)
			cnt->diffs_last[i] = cnt->diffs_last[i+1];
		else
			cnt->diffs_last[i] = cnt->threshold / 4;
		if (cnt->diffs_last[i] > top)
			top = cnt->diffs_last[i];
	}
	sum += cnt->diffs_last[i];
	cnt->diffs_last[i] = diffs;

	sum /= THRESHOLD_TUNE_LENGTH / 4;
	if (sum < top * 2)
		sum = top * 2;
	if (sum < cnt->conf.max_changes)
		cnt->threshold = (cnt->threshold + sum) / 2;
}

/*
Labeling by Joerg Weber. Based on an idea from Hubert Mara.
Floodfill enhanced by Ian McConnel based on code from
http://www.acm.org/pubs/tog/GraphicsGems/
http://www.codeproject.com/gdi/QuickFill.asp
*/

/*
 * Filled horizontal segment of scanline y for xl<=x<=xr.
 * Parent segment was on line y-dy.  dy=1 or -1
 */

#define MAXS 10000               /* max depth of stack */

#define PUSH(Y, XL, XR, DY)     /* push new segment on stack */ \
        if (sp<stack+MAXS && Y+(DY) >= 0 && Y+(DY) < height) \
        {sp->y = Y; sp->xl = XL; sp->xr = XR; sp->dy = DY; sp++;}

#define POP(Y, XL, XR, DY)      /* pop segment off stack */ \
        {sp--; Y = sp->y+(DY = sp->dy); XL = sp->xl; XR = sp->xr;}

typedef struct {short y, xl, xr, dy;} Segment;


static int iflood(int x, int y,       
                  int width, int height, unsigned char *out, int *labels, int newvalue, int oldvalue)
{
	int l, x1, x2, dy;
	Segment stack[MAXS], *sp = stack;    /* stack of filled segments */
	int count = 0;

	if (x < 0 || x >= width || y < 0 || y >= height)
		return 0;

	PUSH(y, x, x, 1);             /* needed in some cases */
	PUSH(y+1, x, x, -1);          /* seed segment (popped 1st) */

	while (sp > stack) {
		/* pop segment off stack and fill a neighboring scan line */
		POP(y, x1, x2, dy);
		/*
		 * segment of scan line y-dy for x1<=x<=x2 was previously filled,
		 * now explore adjacent pixels in scan line y
		 */
		for (x = x1; x >= 0 && out[y*width+x] != 0 && labels[y*width+x] == oldvalue; x--) {
			labels[y*width+x] = newvalue;
			count++;
		}
		
		if (x >= x1)
			goto skip;
		
		l = x + 1;
		
		if (l < x1)
			PUSH(y, l, x1-1, -dy);  /* leak on left? */
		
		x = x1 + 1;
		
		do {
			for (; x < width && out[y*width+x] != 0 && labels[y*width+x]==oldvalue; x++) {
				labels[y*width+x] = newvalue;
				count++;
			}
			
			PUSH(y, l, x-1, dy);
			
			if (x > x2+1)
				PUSH(y, x2+1, x-1, -dy);  /* leak on right? */
			
			skip:
			
			for (x++; x <= x2 && !(out[y*width+x] != 0 && labels[y*width+x]==oldvalue); x++);
			
			l = x;
		} while (x <= x2);
	}
	return count;
}

static int alg_labeling(struct context *cnt)
{
	struct images *imgs=&cnt->imgs;
	unsigned char *out=imgs->out;
	int *labels=imgs->labels;
	int ix, iy, pixelpos;
	int width=imgs->width;
	int height=imgs->height;
	int labelsize=0;
	int current_label=2;
	cnt->current_image->total_labels=0;
	imgs->labelsize_max=0;
	/* ALL labels above threshold are counted as labelgroup */
	imgs->labelgroup_max=0;
	imgs->labels_above=0;

	/* init: 0 means no label set / not checked */
	memset(labels, 0, width*height*sizeof(labels));
	pixelpos = 0;
	for( iy=0; iy<height-1; iy++ ) {
		for( ix=0; ix<width-1; ix++, pixelpos++ ) {
			/* no motion - no label */
			if( out[pixelpos] == 0 ) {
				labels[pixelpos]=1;
				continue;
			}
			
			/* already visited by iflood */
			if (labels[pixelpos] > 0)
				continue;
			labelsize=iflood(ix, iy, width, height, out, labels, current_label, 0);
			
			if( labelsize > 0 ) {
				//printf( "Label: %i (%i) Size: %i (%i,%i)\n", current_label, cnt->current_image->total_labels, labelsize, ix, iy );
				/* Label above threshold? Mark it again (add 32768 to labelnumber) */
				if (labelsize > cnt->threshold){