track.cpp

人头跟踪算法

	fread(&tmp, sizeof(int), 1, fp);
	if (tmp != N_STATE_VARIABLES)
		AfxMessageBox("Error: pixel list file contains wrong # of state variables!!", MB_OK|MB_ICONSTOP, 0);
	fread(&(ss->n_states_to_check), sizeof(int), 1, fp);
	if (ss->n_states_to_check != (2*msx+1)*(2*msy+1)*3)
		AfxMessageBox("Warning: pixel list file contains questionable # of states to check!!", MB_OK|MB_ICONEXCLAMATION, 0);
	for (i=0 ; i<ss->n_states_to_check ; i++) {
		fread(ptr, sizeof(searchLocType), 1, fp);  ptr++;
		fread(ptr, sizeof(searchLocType), 1, fp);  ptr++;
		fread(ptr, sizeof(searchLocType), 1, fp);  ptr++;
	}

	// Allocate memory for pixel list array
	PixelList2DArrayAlloc(lists, ss->n_states_to_check);

	// Read pixel list
	for (sz=MIN_OUTLINE_SZ ; sz<=MAX_OUTLINE_SZ ; sz++) {
		for (i=0 ; i<ss->n_states_to_check ; i++) {
			fread(&n_pix, sizeof(int), 1, fp);
			(*lists)[sz][i].n_pixels = n_pix;
			if (n_pix>=0) {
				(*lists)[sz][i].pixels = (pixelListType *) malloc(2*n_pix*sizeof(pixelListType));
				tmp = fread((*lists)[sz][i].pixels, sizeof(pixelListType), 2*n_pix, fp);	
				if (feof(fp))
					AfxMessageBox("END OF FILE ENCOUNTERED!!", MB_OK|MB_ICONSTOP, 0);
				if (tmp != 2*n_pix) 
					AfxMessageBox("Error reading pixel list!!", MB_OK|MB_ICONSTOP, 0);
			}
		}
	}
	fclose(fp);
}
#undef PIXELLIST_HEADER_LENGTH

void CompareDifferentialPixelLists(PixelList **lists1,
								   PixelList **lists2,
								   SearchStrategy *ss1,
								   SearchStrategy *ss2) 
{
	int i, j, sz;
	int n_pix1, n_pix2;
	pixelListType *ptr1, *ptr2;

	// Compare search strategies
	if (ss1->n_states_to_check != ss2->n_states_to_check)
		AfxMessageBox("Error: 'n_states_to_check's are different!!", MB_OK|MB_ICONSTOP, 0);
	for (i=0 ; i<N_STATE_VARIABLES * ss1->n_states_to_check ; i++)
		if (ss1->ds[i] != ss2->ds[i])
			AfxMessageBox("Error: Search strategies are different!!", MB_OK|MB_ICONSTOP, 0);

	// Compare pixel lists
	for (sz=MIN_OUTLINE_SZ ; sz<MAX_OUTLINE_SZ ; sz++) {
		for (i=0 ; i<ss1->n_states_to_check ; i++) {
			n_pix1 = lists1[sz][i].n_pixels;
			n_pix2 = lists2[sz][i].n_pixels;
			if (n_pix1 != n_pix2) 
				AfxMessageBox("Error: PixelLists are different!!", MB_OK|MB_ICONSTOP, 0);
			ptr1 = lists1[sz][i].pixels;
			ptr2 = lists2[sz][i].pixels;
			for (j=0 ; j<2*n_pix1 ; j++) {
				if (*ptr1++ != *ptr2++)
				AfxMessageBox("Error: PixelLists are different!!", MB_OK|MB_ICONSTOP, 0);
			}
		}
	}
}

//////////////////////////////////////////////////////////////////////
// smoothImage
// 
// Smooths an image by convolving with a 5x5 Gaussian, using
// two fixed one-dimensional kernels:  [1 4 6 4 1].  After
// the full convolution, the result is divided by 8, so that the
// maximum pixel value is 8192.
// 
// INPUTS
// imagebuf:  grey-scale image
// ncols, nrows:  size of image
// 
// OUTPUTS
// gaussbuf:  smoothed image
// 
// NOTES
// Sets the two-pixel out-of-bounds border to zero.
//
// THIS FUNCTION WAS NOT PART OF THE ORIGINAL CODE.  IT HAS BEEN
// COPIED OVER FROM A RELATED PIECE OF CODE SIMPLY TO MAKE IT EASIER
// FOR THE USER TO REPLACE THE PROPRIETARY LIBRARY CALL 
// SmoothByConvolvingWithGaussian().

void smoothImage(unsigned char *imagebuf, unsigned short *gaussbuf,
			  int ncols, int nrows)
{
	unsigned char *cptr;
	unsigned short *sptr;
	int i, j, indx;

	// Zero top border
	sptr = gaussbuf;
	for (i = 0 ; i < 2*ncols + 2 ; i++)  
		*sptr++ = 0;

	// Convolve middle with horizontal [1 4 6 4 1] mask
	cptr = imagebuf;
	sptr = gaussbuf + 2*ncols + 2;
	for (i = 2 ; i < (nrows-2) ; i++)  {
		for (j = 2 ; j < ncols - 2 ; j++)  {
			indx = i*ncols+j;
			*sptr++ = cptr[indx-2] + 4 * cptr[indx-1] + 6 * cptr[indx]
					+ 4 * cptr[indx+1] + cptr[indx+2];
 		}
		for (j = 1 ; j < 5 ; j++)
			*sptr++ = 0;
	}

	// Convolve middle with vertical [1 4 6 4 1]^T mask
	cptr = imagebuf;
	sptr = gaussbuf + 2*ncols + 2;
	for (i = 2 ; i < (nrows-2) ; i++)  {
		for (j = 2 ; j < ncols - 2 ; j++)  {
			indx = i*ncols+j;
			*sptr += cptr[indx-2*ncols] + 4 * cptr[indx-1*ncols] 
					+ 6 * cptr[indx]
					+ 4 * cptr[indx+1*ncols] + cptr[indx+2*ncols];
			*sptr++ /= 8;
 		}
		sptr += 4;
	}

	// Zero bottom border
	cptr = imagebuf + (nrows-2)*ncols + 2;
	sptr = gaussbuf + (nrows-2)*ncols + 2;
	for (i = (nrows-2)*ncols + 2 ; i < ncols*nrows ; i++)
		*sptr++ = 0;
}


//////////////////////////////////////////////////////////////////////
// computeGradient
// 
// Takes the absolute value of the derivative of an image by
// convolving the image with two one-dimensional filters ([-1 0 1]
// and [-1 0 1]^T), taking the absolute values, and summing.
//  
// INPUTS
// gauss:  smoothed image, as returned by smoothImage()
// ncols, nrows:  size of image
// 
// OUTPUTS
// grad:  absolute derivative
// 
// NOTES
// Sets the two-pixel out-of-bounds border to zero.
// Saturates the values above thresh_absderiv; that is, any value
// above thresh_absderiv becomes 255.
//
// THIS FUNCTION WAS NOT PART OF THE ORIGINAL CODE.  IT HAS BEEN
// COPIED OVER FROM A RELATED PIECE OF CODE SIMPLY TO MAKE IT EASIER
// FOR THE USER TO REPLACE THE PROPRIETARY LIBRARY CALLS 
// GradientMagnitude(), GradientMagnitudeX(), and GradientMagnitudeY().

void computeGradient(unsigned short *gauss, unsigned char *grad,
				 int ncols, int nrows)
{
	register unsigned short *ptrL, *ptrR, *ptrU, *ptrD;
	register unsigned char *ptro;
	register unsigned char *end;
	register int val, row;


	// Zero top border
	for (ptro = grad ; ptro < grad+3*ncols+3 ; *ptro++ = 0) ;

	end = grad + ncols*(nrows-3) - 3;
	for (ptrL = gauss+3*ncols+2, ptrR = gauss+3*ncols+4,
		 ptrU = gauss+2*ncols+3, ptrD = gauss+4*ncols+3 ;
		 ptro < end; )  {

		val = abs(*ptrL++ - *ptrR++) + abs(*ptrU++ - *ptrD++);
		if (val > thresh_absderiv)  val = 255;

		// Clear left and right three-pixel borders
		row = (ptro - grad) % ncols;
		if (row <= 2 || row >= ncols - 3)  val = 0;

		assert(ptro >= grad);
		assert(ptro < grad + ncols*nrows*sizeof(char));
		*ptro++ = val;
	}

	// Zero bottom border
	for ( ; ptro < grad+ncols*nrows ; *ptro++ = 0) ;
}

//////////////////////////////////////////////////////////////////////
// searchForHead
//
// INPUTS
// s_old:  old state (x,y,sz)
//
// OUTPUTS
// s_new:  new state (x,y,sz)
int grad_scores[MAX_N_STATES_TO_CHECK];
int color_scores[MAX_N_STATES_TO_CHECK];

void SearchForHead(EllipseState *s_old, ImageBGR24 *img, 
				   SearchStrategy *ss, 
				   PixelList addlist[],
				   PixelList sublist[],
				   EllipseState *sgrad,
				   EllipseState *scolor,
				   EllipseState *s_new)
{
	int x;
	int y;
	int sz;
	int sz_curr;
	searchLocType *ptr = ss->ds;
	int best_score_grad = -1, best_score_color = -1, best_score = -1;
	int worst_score_grad = 999999, worst_score_color = 999999;
	ColorHistogram ch_ref_scaled, ch_new, ch_tmp;
	int i;
//	int tmp;
	int curr_score;
//	int n_saturated_pixels;

	assert(s_old->sz>MIN_OUTLINE_SZ && s_old->sz<MAX_OUTLINE_SZ);
	assert(s_old->x >= msx + outlines[s_old->sz+1].halfsize_x);
	assert(s_old->x <= ISIZEX-1 - msx - outlines[s_old->sz+1].halfsize_x);
	assert(s_old->y >= msy + outlines[s_old->sz+1].halfsize_y);
	assert(s_old->y <= ISIZEY-1 - msy - outlines[s_old->sz+1].halfsize_y);
	assert(ss->n_states_to_check <= MAX_N_STATES_TO_CHECK);

	// Preprocessing
	img->ConvertToImage8(&img_gray);
	if (use_gradient) {
		SmoothByConvolvingWithGaussian(&img_gray, &img_gauss, 5.0f);
		if (sum_gradient_magnitude) {
			GradientMagnitude(&img_gauss, &img_tmp16);
			img_tmp16.ConvertToImage8(8, &img_grad);
		} else {
			GradientMagnitudeX(&img_gauss, &img_gradx);
			GradientMagnitudeY(&img_gauss, &img_grady);
			img_gradx.ConvertToImageFloat(FALSE, &img_gradxf); // FALSE indicates that img_gradx is 'short', as opposed to 'unsigned short'
			img_grady.ConvertToImageFloat(FALSE, &img_gradyf);
			if (show_details) {  // This is simply for debugging purposes
				Add32768(&img_gradx);
				Add32768(&img_grady);
				img_gradx.ConvertToImage8(8, &img_gradx8);
				img_grady.ConvertToImage8(8, &img_grady8);
			}
		}
	}
	if (use_color) {
		ExtractColorSpace(img, &img_color1, &img_color2, &img_color3);
		// QuantizeColorSpace(&img_color1, &img_color2, &img_color3, &img_color1q, &img_color2q, &img_color3q);
	}

	// Evaluate states based on normalized gradient
	if (use_gradient) {
		if (sum_gradient_magnitude) {
			for (i = 0 ; i < ss->n_states_to_check ; i++) {
				x  = s_old->x  + *ptr++;  y  = s_old->y  + *ptr++;  sz = s_old->sz + *ptr++;
				grad_scores[i]  = NormalizedSumOfGradientMagnitude(x, y, &outlines[sz], img_grad.GetImagePtr());
			}
		} else { // sum dot product
			for (i = 0 ; i < ss->n_states_to_check ; i++) {
				x  = s_old->x  + *ptr++;  y  = s_old->y  + *ptr++;  sz = s_old->sz + *ptr++;
				grad_scores[i]  = NormalizedSumOfGradientDotProduct(x, y, &outlines[sz], 
						img_gradxf.GetImagePtr(), img_gradyf.GetImagePtr());
			}
		}
	}

	// Display likelihood
	if (use_color && show_details) {
		DrawColorLikelihoodMap(img, &ch_ref, &img_likelihood);
		ComputeColorHistogramOfWholeImage(&img_color1, &img_color2, &img_color3, &ch_background);
		DrawHistogramBackprojection(img, &ch_ref, &ch_background, &img_backprojection);
	}
	
	// Evaluate states based on color histogram
	if (use_color) {
		x = s_old->x;  y = s_old->y;  sz = s_old->sz;
		sz_curr = sz+ss->ds[2];
		ComputeColorHistogram(&img_color1, &img_color2, &img_color3, x+ss->ds[0], y+ss->ds[1], 
							&outlines[sz_curr], &ch_new);
		assert(SizeOfColorHistogram(&ch_new) == outlines[sz_curr].area);
		NormalizeColorHistogram(&ch_ref, outlines[sz_curr].area, 
								outlines[CH_MODEL_SZ].area, &ch_ref_scaled);
		curr_score = ColorHistogramIntersection(&ch_ref_scaled, &ch_new);
		color_scores[0] = (curr_score * 10000) / outlines[sz_curr].area;

		for (i = 1 ; i < ss->n_states_to_check ; i++) {
			if (sz + ss->ds[N_STATE_VARIABLES*i+2] != sz_curr) {
				sz_curr = sz + ss->ds[N_STATE_VARIABLES*i+2];
				NormalizeColorHistogram(&ch_ref, outlines[sz_curr].area, 
								outlines[CH_MODEL_SZ].area, &ch_ref_scaled);
				UpdateColorHistogram(&img_color1, &img_color2, &img_color3, x, y, &addlist[i], &sublist[i], &ch_new);
				assert(SizeOfColorHistogram(&ch_new) == outlines[sz_curr].area);
				curr_score = ColorHistogramIntersection(&ch_ref_scaled, &ch_new);
				color_scores[i] = (curr_score * 10000) / outlines[sz_curr].area;
			} else {
				UpdateColorHistogramAndIntersection(&img_color1, &img_color2, &img_color3, x, y, 
								&addlist[i], &sublist[i], &ch_ref_scaled, &ch_new, &curr_score);
				assert(SizeOfColorHistogram(&ch_new) == outlines[sz_curr].area);
				color_scores[i] = (curr_score * 10000) / outlines[sz_curr].area;
			}

#if 0 // useful for debugging
			ComputeColorHistogram(&img_color1, &img_color2, &img_color3, x+ss->ds[N_STATE_VARIABLES*i], y+ss->ds[N_STATE_VARIABLES*i+1], 
							&outlines[sz_curr], &ch_tmp);
			tmp = AreColorHistogramsEqual(&ch_new, &ch_tmp);
			if (!tmp) {
				my_cdc->TextOut(200, 215, "Color Histograms are Not Equal!!");
			}
			tmp = ColorHistogramIntersection(&ch_ref_scaled, &ch_tmp);
			if (tmp != curr_score) {
				my_cdc->TextOut(200, 215, "Current score is incorrect!!");
			}
#endif