track.cpp

人头跟踪算法

#include "stdafx.h"	// TRACE(), AfxMessageBox(), max(), min()
#include "assert.h"

#include "base.h" // ISIZE[XY]
#define VARSIZE // prevents "filters.h" from defining ISIZEX & ISIZEY
#include "filters.h"
#include "draw.h"
#include "gradient_sum.h"
#include "color_histogram.h"
#include "outline.h"
#include "search.h" // EllipseState
#include "track.h"

// #define COMPUTE_PIXEL_LISTS   // forces computation rather than reading precomputed data

// images
Image8 img_gray(ISIZEX,ISIZEY), img_gray_old(ISIZEX,ISIZEY);
Image16 img_gauss(ISIZEX,ISIZEY);
Image8 img_grad(ISIZEX,ISIZEY);
Image16 img_gradx(ISIZEX,ISIZEY);
Image16 img_grady(ISIZEX,ISIZEY);
Image8 img_gradx8(ISIZEX,ISIZEY);
Image8 img_grady8(ISIZEX,ISIZEY);
ImageFloat img_gradxf(ISIZEX,ISIZEY);
ImageFloat img_gradyf(ISIZEX,ISIZEY);
Image16 img_tmp16(ISIZEX,ISIZEY);
ImageBGR24 img_outline(ISIZEX,ISIZEY);
ImageBGR24 img_outline_grad(ISIZEX,ISIZEY);
ImageBGR24 img_outline_color(ISIZEX,ISIZEY);
Image8 img_outline8(ISIZEX,ISIZEY);
Image8 img_outline_grad8(ISIZEX,ISIZEY);
Image8 img_outline_color8(ISIZEX,ISIZEY);
Image8 img_mask(ISIZEX,ISIZEY);
Image8 img_color1(ISIZEX,ISIZEY);
Image8 img_color2(ISIZEX,ISIZEY);
Image8 img_color3(ISIZEX,ISIZEY);
Image8 img_color1ref(ISIZEX,ISIZEY);
Image8 img_color2ref(ISIZEX,ISIZEY);
Image8 img_color3ref(ISIZEX,ISIZEY);
Image8 img_color1q(ISIZEX,ISIZEY);
Image8 img_color2q(ISIZEX,ISIZEY);
Image8 img_color3q(ISIZEX,ISIZEY);
Image8 img_likelihood(ISIZEX,ISIZEY);
Image8 img_backprojection(ISIZEX,ISIZEY);
Image8 img_lights(ISIZEX,ISIZEY);
Image8 img_bin(ISIZEX,ISIZEY);

// external variables
extern BOOL building_model;
extern HDC my_hdc;  // for debugging, to allow displaying of images
extern CDC *my_cdc;  // for debugging, to allow TextOut()
extern BOOL use_gradient;
extern BOOL use_color;
extern BOOL adapt_histogram;
extern BOOL show_details;
extern BOOL sum_gradient_magnitude;

// Other variables
ColorHistogram ch_model;
ColorHistogram ch_ref;
ColorHistogram ch_background;
ColorHistogram ch_curr;
int n_images_in_model;
int color_bin_of_interest=0;
static char msg[80];
int tmp1, tmp2;
int tmp3;

// Outlines
OUTLINE outlines[MAX_OUTLINE_SZ + 1];

// Search strategy
SearchStrategy search_strategy;
SearchStrategy ss_tmp;

// Search range is +/- {msx, msy} pixels
int msx = 16, msy = 4;

// Skip szinc pixels to find the smaller and larger scale
int szinc = 1;


// just for debugging and display
void Add32768(Image16 *img)
{
	short *ptri = (short*) img->GetImagePtr();
	unsigned short *ptro = img->GetImagePtr();
	int val;
	int i;

	for (i=0 ; i<ISIZEX*ISIZEY ; i++) {
		val = *ptri++;
		*ptro++ = val + 32768;
	}
}

void MakeSearchStrategy(SearchStrategy *ss)
{
	int i = 0;
	int x, y, sz;
	BOOL left_to_right = TRUE;
	BOOL top_to_bottom = TRUE;

	assert(N_STATE_VARIABLES == 3);
	for (sz = -szinc ; sz <= szinc ; sz += szinc) {
		for (y = -msy ; y <= msy ; y++) {
			for (x = -msx ; x <= msx ; x++) {
				assert(i < N_STATE_VARIABLES * MAX_N_STATES_TO_CHECK - 2);
				assert(x>=-127 && x<=127);
				assert(y>=-127 && y<=127);
				assert(sz>=-127 && sz<=127);
				ss->ds[i++] = (searchLocType) (left_to_right ? x : -x);
				ss->ds[i++] = (searchLocType) (top_to_bottom ? y : -y);
				ss->ds[i++] = (searchLocType) sz;
//				TRACE("     check: dx dy ds %3d %3d %3d\n",
//					ss->ds[i-3], ss->ds[i-2], ss->ds[i-1]);
			}
			left_to_right = 1 - left_to_right;
		}
		top_to_bottom = 1 - top_to_bottom;
	}

	assert(i%N_STATE_VARIABLES==0);
	ss->n_states_to_check = i/N_STATE_VARIABLES;
}

// AndNotBitmap()
// Returns img1 && !(img2)
static void AndNotBitmap(Image8* img1, Image8* img2, Image8* output)
{
	unsigned char *ptr1, *ptr2, *ptrout;
	int i;

	ptr1 = img1->GetImagePtr();
	ptr2 = img2->GetImagePtr();
	ptrout = output->GetImagePtr();

	for (i = 0 ; i < ISIZEX*ISIZEY ; i++) {
		*ptrout++ = 255 * (*ptr1 && !(*ptr2));
		ptr1++; ptr2++;
	}
}

void ComputePixelList(Image8 *img1,
					  Image8 *img2,
					  int xcen, int ycen,
					  PixelList *pl
					  )
{
	Image8 img_diff(ISIZEX,ISIZEY);
	unsigned char *ptr;
	pixelListType *ptro;
	int x, y;
	int i=0;

	AndNotBitmap(img1, img2, &img_diff);

	// Count no. of nonzero pixels
	ptr = img_diff.GetImagePtr();
	for (y=0 ; y<ISIZEX*ISIZEY ; y++) {
		if (*ptr++)  i++;
	}
	pl->n_pixels = i;
	pl->pixels = (pixelListType *) malloc(pl->n_pixels * 2 * sizeof(pixelListType));
	
	// Copy nonzero pixels to list
	ptr = img_diff.GetImagePtr();
	ptro = pl->pixels;
	for (y=0 ; y<ISIZEY ; y++) {
		for (x=0 ; x<ISIZEX ; x++) {
			if (*ptr++) {
				assert(x-xcen>=-128 && x-xcen<=127);
				assert(y-ycen>=-128 && y-ycen<=127);
				*ptro++ = (pixelListType) (x - xcen);
				*ptro++ = (pixelListType) (y - ycen);
			}
		}
	}
	
}

void** createArray2D(int ncols, int nrows, int nbytes)
{
	char **tt;
	int i;

     tt = (char **) malloc(nrows * sizeof(void *) +
                           ncols * nrows * nbytes);
	 if (tt == NULL) AfxMessageBox("(createArray2D) out of memory!!", MB_OK|MB_ICONSTOP, 0);	

     for (i = 0 ; i < nrows ; i++)
          tt[i] = ((char *) tt) + (nrows * sizeof(void *) + 
							i * ncols * nbytes);

	return((void **) tt);
}

void PixelList2DArrayAlloc(PixelList ***ptr, int n_states_to_check)
{
	*ptr = (PixelList **) createArray2D(n_states_to_check, MAX_OUTLINE_SZ+1, sizeof(PixelList));
}

/////////////////////////////////////////////////////////////////////////////
// ComputeDifferentialPixelLists()
// Given an ellipse size and a search strategy, fills two arrays
// of pixel lists.
//   add_list contains all the pixels in img2 but not in img1, while
//   sub_list contains all the pixels in img1 but not in img2.
void ComputeDifferentialPixelLists(int sz, 
								   SearchStrategy *ss,
								   PixelList add_list[],
								   PixelList sub_list[]
								   ) 
{
	Image8 img1(ISIZEX,ISIZEY), img2(ISIZEX,ISIZEY);
	searchLocType *ptr = ss->ds;
	int xcen=ISIZEX/2, ycen=ISIZEY/2;
	int x1=xcen, y1=ycen, sz1=sz, x2=xcen, y2=ycen, sz2=sz;
	int i;

	assert(N_STATE_VARIABLES == 3);
	for (i=0 ; i<ss->n_states_to_check ; i++) {
		x2  = xcen + *ptr++;
		y2  = ycen + *ptr++;
		sz2 = sz   + *ptr++;

		// Do not perform computation when sz is out of bounds; this 
		// incompleteness is okay because it will never be needed at
		// runtime.
		if (sz1>=MIN_OUTLINE_SZ && sz1<=MAX_OUTLINE_SZ &&
			sz2>=MIN_OUTLINE_SZ && sz2<=MAX_OUTLINE_SZ) {

			// Center ellipses onto image, then draw
			DrawMask(x1, y1, &outlines[sz1], &img1);
			DrawMask(x2, y2, &outlines[sz2], &img2);

			// Compute two pixel lists
			ComputePixelList(&img2, &img1, xcen, ycen, &add_list[i]);
			ComputePixelList(&img1, &img2, xcen, ycen, &sub_list[i]);
		} else {
			add_list[i].n_pixels = -1;
			sub_list[i].n_pixels = -1;
		}

		x1=x2;  y1=y2;  sz1=sz2;
	}

}

#define PIXELLIST_HEADER_LENGTH 3
char pixellist_header[PIXELLIST_HEADER_LENGTH+1] = "PL1";

void WriteDifferentialPixelLists(char *fname, PixelList **lists,
								 SearchStrategy *ss) 
{
	searchLocType *ptr = ss->ds;
	int i, sz;
	int n_pix;
	int tmp;
	FILE *fp;

	// Open file for writing
	fp = fopen(fname, "wb");
	if (fp == NULL)
		AfxMessageBox("Error: could not open pixel list file for writing!!", MB_OK|MB_ICONSTOP, 0);

	// Write header
	fwrite(pixellist_header, sizeof(char), PIXELLIST_HEADER_LENGTH, fp);

	// Write search strategy
	tmp = N_STATE_VARIABLES;
	fwrite(&tmp, sizeof(int), 1, fp);
	fwrite(&(ss->n_states_to_check), sizeof(int), 1, fp);
	for (i=0 ; i<ss->n_states_to_check ; i++) {
		fwrite(ptr, sizeof(searchLocType), 1, fp);  ptr++;
		fwrite(ptr, sizeof(searchLocType), 1, fp);  ptr++;
		fwrite(ptr, sizeof(searchLocType), 1, fp);  ptr++;
	}

	// Write pixel list
	for (sz=MIN_OUTLINE_SZ ; sz<=MAX_OUTLINE_SZ ; sz++) {
		for (i=0 ; i<ss->n_states_to_check ; i++) {
			n_pix = lists[sz][i].n_pixels;
			fwrite(&n_pix, sizeof(int), 1, fp);
			if (n_pix>=0) {
				tmp = fwrite(lists[sz][i].pixels, sizeof(pixelListType), 2*n_pix, fp);	
				if (tmp != 2*n_pix) 
					AfxMessageBox("Error writing pixel list!!", MB_OK|MB_ICONSTOP, 0);
			}
		}
	}
	fclose(fp);
}

// Reads data and allocates memory
void ReadDifferentialPixelLists(char *fname, 
								PixelList ***lists,
								SearchStrategy *ss) 
{
	char strtmp[PIXELLIST_HEADER_LENGTH+1];
	searchLocType *ptr = ss->ds;
	int i, sz;
	int n_pix;
	int tmp;
	FILE *fp;

	if (*lists != NULL)
		AfxMessageBox("Warning: trying to read pixel list into non-empty array!!", MB_OK|MB_ICONEXCLAMATION, 0);

	// Open file for reading
	fp = fopen(fname, "rb");
	if (fp == NULL)
		AfxMessageBox("Error: could not open pixel list file for reading!!", MB_OK|MB_ICONSTOP, 0);

	// Read header
	fread(strtmp, sizeof(char), PIXELLIST_HEADER_LENGTH, fp);
	strtmp[PIXELLIST_HEADER_LENGTH] = '\0';
	if (strcmp(strtmp, pixellist_header) != 0)
		AfxMessageBox("Error: pixel list file has invalid header!!", MB_OK|MB_ICONSTOP, 0);

	// Read search strategy