track.cpp

人头跟踪算法

		}
	}

	// Compute best and worst scores for grad and color alone
	for (i = 0 ; i < ss->n_states_to_check ; i++) {
		if (use_gradient) {
			curr_score = grad_scores[i];
			if (curr_score > best_score_grad) {
				best_score_grad = curr_score;
				sgrad->x  = s_old->x  + ss->ds[N_STATE_VARIABLES*i];
				sgrad->y  = s_old->y  + ss->ds[N_STATE_VARIABLES*i+1];
				sgrad->sz = s_old->sz + ss->ds[N_STATE_VARIABLES*i+2];
			}
			if (curr_score < worst_score_grad) {
				worst_score_grad = curr_score;
			}
		}
		if (use_color) {
			curr_score = color_scores[i];
			if (curr_score > best_score_color) {
				best_score_color = curr_score;
				scolor->x  = s_old->x  + ss->ds[N_STATE_VARIABLES*i];
				scolor->y  = s_old->y  + ss->ds[N_STATE_VARIABLES*i+1];
				scolor->sz = s_old->sz + ss->ds[N_STATE_VARIABLES*i+2];
			}
			if (curr_score < worst_score_color) {
				worst_score_color = curr_score;
			}
		}
	}

	// Select best state
	float grad_fact  = 1000.0f/(best_score_grad  - worst_score_grad);
	float color_fact = 1000.0f/(best_score_color - worst_score_color);
	if (!use_gradient) grad_fact = 0.0f;
	if (!use_color) color_fact = 0.0f;
//	if (n_saturated_pixels >= 500)  {tmp1=n_saturated_pixels; color_fact *= 0.2f;} else {tmp1=0;}
	for (i = 0 ; i < ss->n_states_to_check ; i++) {
		curr_score = (building_model ? grad_scores[i] : 
			(int) ((grad_scores[i]  - worst_score_grad)  * grad_fact +
			       (color_scores[i] - worst_score_color) * color_fact));
		if (curr_score > best_score) {
			best_score = curr_score;
			s_new->x  = s_old->x  + ss->ds[N_STATE_VARIABLES*i];
			s_new->y  = s_old->y  + ss->ds[N_STATE_VARIABLES*i+1];
			s_new->sz = s_old->sz + ss->ds[N_STATE_VARIABLES*i+2];
		}
	}

//	sprintf(msg, "using grad %d %s, using color %d    ",
//		use_gradient, sum_gradient_magnitude?"MAG":"DOT", use_color);
//	my_cdc->TextOut(5, 225, msg);

//	sprintf(msg, "grad: <%4d,%4d>, color: <%4d,%4d>       ", 
//		worst_score_grad, best_score_grad, worst_score_color, best_score_color);
//	my_cdc->TextOut(5, 225, msg);

	// Build model, if desired
	// Model is always of size CH_MODEL_SZ
	if (building_model || adapt_histogram) {
		ComputeColorHistogram(&img_color1, &img_color2, &img_color3, s_new->x, s_new->y,
							&outlines[s_new->sz], &ch_new);
		assert(SizeOfColorHistogram(&ch_new) == outlines[s_new->sz].area);
		NormalizeColorHistogram(&ch_new, outlines[CH_MODEL_SZ].area,
								outlines[s_new->sz].area, &ch_tmp);
	}
	if (building_model) {
		AddColorHistograms(&ch_model, &ch_tmp);
		memcpy(ch_ref.val, ch_model.val, N_BINS_TOT*sizeof(int));
		n_images_in_model++;
	}
	if (adapt_histogram) {
		memset(ch_ref.val, 0, N_BINS_TOT*sizeof(int));
		AddColorHistograms(&ch_ref, &ch_model);
		AddColorHistograms(&ch_ref, &ch_tmp);		
		NormalizeColorHistogram(&ch_ref, 1, 2, &ch_ref);
	}

	// When user clicks the mouse, statistics on current histogram can be displayed.
	if (show_details) {
		ComputeColorHistogram(&img_color1, &img_color2, &img_color3, s_new->x, s_new->y,
							&outlines[s_new->sz], &ch_curr);
		DrawPixelsInCertainColorBin(img, &ch_curr, color_bin_of_interest, &img_bin);
	}
}

void KeepStateInBounds(EllipseState *s)
{
	s->sz = max(s->sz, MIN_OUTLINE_SZ+1);
	s->sz = min(s->sz, MAX_OUTLINE_SZ-1);
	s->x  = max(s->x, msx + outlines[s->sz+1].halfsize_x);
	s->x  = min(s->x, ISIZEX-1 - msx - outlines[s->sz+1].halfsize_x);
	s->y  = max(s->y, msy + outlines[s->sz+1].halfsize_y);
	s->y  = min(s->y, ISIZEY-1 - msy - outlines[s->sz+1].halfsize_y);
}

PixelList **add_lists = NULL;
PixelList **sub_lists = NULL;
PixelList **add_lists_tmp = NULL;
PixelList **sub_lists_tmp = NULL;

void InitTracker(EllipseState *curr_state)
{
	static BOOL first_time = TRUE;
	int i;

	if (!first_time) 
		AfxMessageBox("Warning:  InitTracker called more than once!!", MB_OK|MB_ICONEXCLAMATION, 0);	
	first_time = FALSE;

	// Initialize elliptical outlines
	for (i = MIN_OUTLINE_SZ ; i <= MAX_OUTLINE_SZ ; i++)  {
		MakeEllipticalOutline(&outlines[i], i, (int) (1.2 * i));
	}

	/*
	DrawMask(64, 48, &outlines[15], &img_mask); img_mask.Write8BitBMP("img15.bmp");
	DrawMask(64, 48, &outlines[16], &img_mask); img_mask.Write8BitBMP("img16.bmp");
	DrawMask(64, 48, &outlines[17], &img_mask); img_mask.Write8BitBMP("img17.bmp");
	DrawMask(64, 48, &outlines[18], &img_mask); img_mask.Write8BitBMP("img18.bmp");
	DrawMask(64, 48, &outlines[19], &img_mask); img_mask.Write8BitBMP("img19.bmp");
	DrawMask(64, 48, &outlines[20], &img_mask); img_mask.Write8BitBMP("img20.bmp");
	DrawMask(64, 48, &outlines[21], &img_mask); img_mask.Write8BitBMP("img21.bmp");
	DrawMask(64, 48, &outlines[22], &img_mask); img_mask.Write8BitBMP("img22.bmp");
	DrawMask(64, 48, &outlines[23], &img_mask); img_mask.Write8BitBMP("img23.bmp");
	DrawMask(64, 48, &outlines[24], &img_mask); img_mask.Write8BitBMP("img24.bmp");
	DrawMask(64, 48, &outlines[25], &img_mask); img_mask.Write8BitBMP("img25.bmp");
	img_lights = 0;
	curr_state->x = 64;	curr_state->y = 48;
	curr_state->sz = 15;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline15.bmp");
	curr_state->sz = 16;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline16.bmp");
	curr_state->sz = 17;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline17.bmp");
	curr_state->sz = 18;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline18.bmp");
	curr_state->sz = 19;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline19.bmp");
	curr_state->sz = 20;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline20.bmp");
	curr_state->sz = 21;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline21.bmp");
	curr_state->sz = 22;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline22.bmp");
	curr_state->sz = 23;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline23.bmp");
	curr_state->sz = 24;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline24.bmp");
	curr_state->sz = 25;	OverlayOutline8(&img_lights, curr_state, 255, &img_mask);  img_mask.Write8BitBMP("outline25.bmp");
	*/

#ifdef COMPUTE_PIXEL_LISTS
	MakeSearchStrategy(&search_strategy);
	PixelList2DArrayAlloc(&add_lists, search_strategy.n_states_to_check);
	PixelList2DArrayAlloc(&sub_lists, search_strategy.n_states_to_check);
	for (i = MIN_OUTLINE_SZ ; i <= MAX_OUTLINE_SZ ; i++)  {
		ComputeDifferentialPixelLists(i, &search_strategy, add_lists[i], sub_lists[i]); 
	}
	WriteDifferentialPixelLists("lists_add.dat", add_lists, &search_strategy);
	WriteDifferentialPixelLists("lists_sub.dat", sub_lists, &search_strategy);
#else
	if (msx==8 && msy==8) {
		ReadDifferentialPixelLists("lists_add8x8.dat", &add_lists, &search_strategy);
		ReadDifferentialPixelLists("lists_sub8x8.dat", &sub_lists, &search_strategy);
	} else if (msx==4 && msy==4) {
		ReadDifferentialPixelLists("lists_add4x4.dat", &add_lists, &search_strategy);
		ReadDifferentialPixelLists("lists_sub4x4.dat", &sub_lists, &search_strategy);
	} else if (msx==16 && msy==16) {
		ReadDifferentialPixelLists("lists_add16x16.dat", &add_lists, &search_strategy);
		ReadDifferentialPixelLists("lists_sub16x16.dat", &sub_lists, &search_strategy);
	} else if (msx==16 && msy==4) {
		ReadDifferentialPixelLists("lists_add16x4.dat", &add_lists, &search_strategy);
		ReadDifferentialPixelLists("lists_sub16x4.dat", &sub_lists, &search_strategy);
	} else if (msx==2 && msy==2) {
		ReadDifferentialPixelLists("lists_add2x2.dat", &add_lists, &search_strategy);
		ReadDifferentialPixelLists("lists_sub2x2.dat", &sub_lists, &search_strategy);
	} else {
		AfxMessageBox("msx and msy are invalid!!", MB_OK|MB_ICONSTOP, 0);	
		// Read anyway, just so we don't get any memory faults
		ReadDifferentialPixelLists("lists_add8.dat", &add_lists, &search_strategy);
		ReadDifferentialPixelLists("lists_sub8.dat", &sub_lists, &search_strategy);
	}

#endif
//	ReadDifferentialPixelLists("lists_add4x4.dat", add_lists_tmp, &ss_tmp);
//	ReadDifferentialPixelLists("lists_sub4x4.dat", sub_lists_tmp, &ss_tmp);
//	CompareDifferentialPixelLists(add_lists, add_lists_tmp, &search_strategy, &ss_tmp);
//	CompareDifferentialPixelLists(sub_lists, sub_lists_tmp, &search_strategy, &ss_tmp);
//	pixelListType tmpadd[31], tmpsub[37];
//	memcpy(tmpadd, add_lists[24][0].pixels, 31*sizeof(pixelListType));
//	memcpy(tmpsub, sub_lists[24][0].pixels, 37*sizeof(pixelListType));

	curr_state->x = ISIZEX/2;
	curr_state->y = ISIZEY/2;
	curr_state->sz = MAX_OUTLINE_SZ-1;
}

void track(ImageBGR24* img, EllipseState *curr_state)
{
	EllipseState grad_state, color_state, new_state;
	static int xvel = 0, yvel = 0;	// velocities (for prediction)

	curr_state->x += xvel;
	curr_state->y += yvel;
	KeepStateInBounds(curr_state);
	SearchForHead(curr_state, 
				  img, 
				  &search_strategy, 
				  add_lists[curr_state->sz],
				  sub_lists[curr_state->sz],
				  &grad_state,
				  &color_state,
				  &new_state);
//	DrawMask(new_state.x, new_state.y, &outlines[new_state.sz], &img_mask);
	xvel = new_state.x - curr_state->x;
	yvel = new_state.y - curr_state->y;
	curr_state->x = new_state.x;
	curr_state->y = new_state.y;
	curr_state->sz = new_state.sz;
	OverlayOutlineBGR24(&img_gray, curr_state, 255, 0, 0, &img_outline);
	if (show_details) {
		OverlayOutline8(&img_gray, curr_state, 255, &img_outline8);
		if (use_gradient) {
			OverlayOutlineBGR24(&img_gray, &grad_state, 0, 255, 0, &img_outline_grad);
			OverlayOutline8(&img_gray, &grad_state, 255, &img_outline_grad8);
		}
		if (use_color) { 
			OverlayOutlineBGR24(&img_gray, &color_state, 0, 0, 255, &img_outline_color);
			OverlayOutline8(&img_gray, &color_state, 255, &img_outline_color8);
		}
	}
}

void InitBuildingOfModel()
{
//	AfxMessageBox("Model being initialized", MB_OK|MB_ICONSTOP, 0);	
	n_images_in_model = 0;
	memset(ch_model.val, 0, N_BINS_TOT * sizeof(int));
}

void CopyModelHistogramToRef()
{
	memcpy(ch_ref.val, ch_model.val, N_BINS_TOT*sizeof(int));
}

void StopBuildingOfModel()
{
	if (n_images_in_model>0)
		NormalizeColorHistogram(&ch_model, 1, n_images_in_model, &ch_model);
	n_images_in_model = 1;
	CopyModelHistogramToRef();
}

void WriteModel(char *fname)
{
	if (n_images_in_model>0)
		NormalizeColorHistogram(&ch_model, 1, n_images_in_model, &ch_model);
	n_images_in_model = 1;
	WriteColorHistogram(fname, &ch_model);
}

void ReadModel(char *fname)
{
	ReadColorHistogram(fname, &ch_model);
//	NormalizeColorHistogram(&ch_model, 15, 10, &ch_model);
	CopyModelHistogramToRef();
}

void ShowPixelListsAndOutlines()
{
	ShowPixelListsAndOutlines2(&search_strategy, add_lists, sub_lists);
}

void PrintBinContents(unsigned char b,
					  unsigned char g,
					  unsigned char r)
{
	int col1, col2, col3;
	int n_pixels;
	int indx;

	BGRToColorBins(b, g, r, &col1, &col2, &col3);
	indx = col3*N_BINS_COLOR1*N_BINS_COLOR2+col2*N_BINS_COLOR1 + col1;
	assert(indx >= 0 && indx < N_BINS_TOT);
	n_pixels = ch_ref.val[indx];
	color_bin_of_interest = indx;
//	sprintf(msg, "Pixels in reference histogram:  %4d/%4d   current: %4d/%4d        ", 
//		n_pixels, SizeOfColorHistogram(&ch_ref), ch_curr.val[indx], SizeOfColorHistogram(&ch_curr));
//	my_cdc->TextOut(5, 225, msg);
}

void SaveErrorFunctions()
{
	FILE *fpg, *fpc;
	int i;
	int x, y, sz;
	int newx, newy, newsz;
	int goodi;

	fpg = fopen("grad_err.m", "w");
	fpc = fopen("color_err.m", "w");
	fprintf(fpg, "grad_err = [ ...\n");
	fprintf(fpc, "color_err = [ ...\n");
	assert(N_STATE_VARIABLES == 3);
	sz = 0;
	for (y = -msy ; y <= msy ; y++) {
		for (x = -msx ; x <= msx ; x++) {
			for (i=0 ; i<search_strategy.n_states_to_check ;i++) {
				newx  = search_strategy.ds[N_STATE_VARIABLES*i];
				newy  = search_strategy.ds[N_STATE_VARIABLES*i+1];
				newsz = search_strategy.ds[N_STATE_VARIABLES*i+2];
				if (x==newx && y==newy && sz==newsz) {
					goodi = i;
				}
			}
			if (goodi>=0) {
				fprintf(fpg, "%3d ", grad_scores[goodi]);
				fprintf(fpc, "%3d ", color_scores[goodi]);
			}
		}
		fprintf(fpg, "; ...\n");
		fprintf(fpc, "; ...\n");
	}
	fprintf(fpg, "];\n");
	fprintf(fpc, "];\n");
	fclose(fpg);
	fclose(fpc);
}