headtrackerdlg.cpp

人头跟踪算法

	if (!show_details) {
		blankimg.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
		blankimg.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
		blankimg.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
		blankimg.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
		blankimg.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	}

}


/////////////////////////////////////////////////////////////////////////////
// Image Sequence functions

FILE *fp_ellipses = NULL;

void CHeadtrackerDlg::OnInputFileBw() { UpdateData(TRUE); }
void CHeadtrackerDlg::OnInputFileColor() { UpdateData(TRUE); }

void CHeadtrackerDlg::OnRecordSeq() 
{
//	int can_allocate_memory = TRUE;

	UpdateData(TRUE);
	TRACE("1 %d %d %d\n", m_enable_recording, m_first_filenum, m_last_filenum);
	if (m_enable_recording && m_first_filenum <= m_last_filenum) {
		TRACE("2\n");
		numfiles = m_last_filenum - m_first_filenum + 1;
		recorded_images = (unsigned char *) 
			malloc(numfiles*BYTES_PER_PIXEL_IN_SEQUENCE*ISIZEX*ISIZEY*sizeof(unsigned char));
		if (recorded_images == NULL) {
			AfxMessageBox("Unable to allocate memory for image sequence",MB_OK|MB_ICONSTOP,0);
			return;
//			can_allocate_memory = FALSE;
		}
		ellipse_states = (int *) malloc(numfiles*3*sizeof(int));
		if (ellipse_states == NULL) {
			AfxMessageBox("Unable to allocate memory for ellipse states",MB_OK|MB_ICONSTOP,0);
			return;
//			can_allocate_memory = FALSE;
		}
		m_filenum = m_first_filenum;
//		for (i = 0 ; i < numfiles ; i++)  {
//			image_sequence_Y[i] = new Image8(ISIZEX, ISIZEY);
//			image_sequence_U[i] = new Image8(ISIZEX, ISIZEY);
//			image_sequence_V[i] = new Image8(ISIZEX, ISIZEY);
//		}
//		if (can_allocate_memory) {
		img_reddot.Display(my_hdc,RECORDING_DOT_X, RECORDING_DOT_Y);
		recording_sequence = TRUE;
//		}
	}
}

void CHeadtrackerDlg::OnTimer(UINT nIDEvent) 
{
//	CDialog::OnTimer(nIDEvent);
//	char msg[80];

	if (m_input_mode == IMAGE_SEQUENCE)  {
		if (playmode != STOP)  {
			sprintf(fname, m_basename, m_filenum);
			if (!m_input_file_bw_instead_of_color && !Image.Read24BitBMP(fname)) { // color
				playmode = STOP;
				AfxMessageBox("Error reading BMP file (Perhaps image is B/W, not Color)",MB_OK|MB_ICONSTOP,0);
			} else if (m_input_file_bw_instead_of_color && !img_gray.Read8BitBMP(fname)) { // b/w
				playmode = STOP;
				AfxMessageBox("Error reading BMP file (Perhaps image is Color, not B/W)",MB_OK|MB_ICONSTOP,0);
			} else {
				unsigned char *ptr = img_gray.GetImagePtr();
				if (m_input_file_bw_instead_of_color) Image.FillWithRGB(ptr, ptr, ptr); // b/w
				Image.Display(my_hdc,LEFT_MARGIN,TOP_MARGIN);			
				PrintEditNumber4D(IDC_FRAME_NUMBER, m_filenum);
//				sprintf(msg, "%4d", m_filenum);
//				GetDlgItem(IDC_FRAME_NUMBER)->SetWindowText(msg);
				if (headtrackerDlg->m_tracking)  {
					TrackAndDisplay();
					if (fp_ellipses) 
						fprintf(fp_ellipses, "%3d %3d %3d %3d ; ...\n", 
							m_filenum, curr_state.x, curr_state.y, curr_state.sz);
//					sprintf(fname, "../../seq_results/overlay%03d.bmp", m_filenum);
//					if (!img_outline8.Write8BitBMP(fname)) {
//						AfxMessageBox("Error writing BMP file",MB_OK|MB_ICONSTOP,0);
//					}
				}
			}
		}
		if (playmode == PLAY)  {
			if (m_filenum == m_last_filenum) {
				fprintf(fp_ellipses, "];\n");
				fclose(fp_ellipses);
				playmode = STOP;
				img_reddot.Display(my_hdc,TRACKING_DOT_X, TRACKING_DOT_Y);
			}
			else m_filenum++;
		}
		else playmode = STOP;
	}
}

void CHeadtrackerDlg::OnPlaySeq() 
{
	img_blankdot.Display(my_hdc,TRACKING_DOT_X, TRACKING_DOT_Y);
	if (m_first_filenum <= m_last_filenum) {
		if (fp_ellipses != NULL) fclose(fp_ellipses);
		fp_ellipses = fopen("../../seq_results/loc.m", "w");
		if (fp_ellipses == NULL) {
			AfxMessageBox("Error opening 'loc.m' file", MB_OK|MB_ICONSTOP,0);
		} else {
			fprintf(fp_ellipses, "loc = [...\n");
			m_filenum = m_first_filenum;
			playmode = PLAY;
		}
	}
}

void CHeadtrackerDlg::OnContinuePlaying() 
{
	if (m_first_filenum <= m_last_filenum) {
		if (playmode==STOP || playmode==BACKWARD) m_filenum++;
		if (m_filenum >= m_first_filenum && m_filenum <= m_last_filenum) playmode = PLAY;
	}
}

void CHeadtrackerDlg::OnForward() 
{
	if (m_filenum < m_last_filenum)  {
		m_filenum++;
		playmode = FORWARD;
	}
}

void CHeadtrackerDlg::OnBackward() 
{
	if (m_filenum > m_first_filenum)  {
		m_filenum--;
		playmode = BACKWARD;
	}
}

void CHeadtrackerDlg::OnStopPlaying() 
{
	if (m_input_mode==LIVE_VIDEO && recording_sequence) {
		m_filenum = headtrackerDlg->m_last_filenum - 1;
	} else if (m_input_mode==IMAGE_SEQUENCE) {
		if (playmode==PLAY || playmode==FORWARD) m_filenum--;
		else if (playmode==BACKWARD) m_filenum++;
		playmode = STOP;
	}
}

void CHeadtrackerDlg::OnChangeBasename() { }//UpdateData(TRUE); }
void CHeadtrackerDlg::OnKillfocusBasename() { PrintEditString(IDC_BASENAME, (char *) ((LPCTSTR) m_basename)); }
void CHeadtrackerDlg::OnKillfocusFirst() { PrintEditNumber4D(IDC_FIRST, m_first_filenum); }
void CHeadtrackerDlg::OnKillfocusLast() { PrintEditNumber4D(IDC_LAST, m_last_filenum); }

void CHeadtrackerDlg::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar* pSB) 
{
	int n;

	if (pSB->GetDlgCtrlID() == IDC_SCROLLBAR_SPEED) {

		n = pSB->GetScrollPos();
		switch(nSBCode) {
		case SB_THUMBPOSITION:
			n = nPos;
			break;
		case SB_LINELEFT: // left arrow button
			n = max(n-1, 0);
			break;
		case SB_LINERIGHT: // right arrow button
			n = min(n+1, N_DISPLAY_SPEEDS-1);
			break;
		case SB_ENDSCROLL:
			// For some reason, the function is called twice after the user clicks --
			// the second invocation is with SB_ENDSCROLL and must be ignored.
			return;
			break;
		}
		pSB->SetScrollPos(n);
		display_speed = display_speeds[n];
		PrintDisplaySpeed(display_speed);
		if (KillTimer(TIMER_ID) == 0)
			AfxMessageBox("Failed to Kill Timer",MB_OK|MB_ICONSTOP,0);
		else {
			if (SetTimer(TIMER_ID, 1000/display_speeds[n], NULL) == 0)
				AfxMessageBox("Failed to Set Timer",MB_OK|MB_ICONSTOP,0);
		}
	}
}


/////////////////////////////////////////////////////////////////////////////
// Camera functions

void CHeadtrackerDlg::OnCameraOff() { UpdateData(TRUE);	StopCamera(); }
void CHeadtrackerDlg::OnCameraOn() { UpdateData(TRUE); }
void CHeadtrackerDlg::OnZoom() { NegateBoolean(&zoom_control); }
void CHeadtrackerDlg::OnCameraHome() { SendCameraHome(); }
void CHeadtrackerDlg::OnLockWhiteBalance() { CameraLockWhiteBalance(); }


/////////////////////////////////////////////////////////////////////////////
// Monitoring functions

FILE *fp_manual = NULL;

void CHeadtrackerDlg::OnShowPixelLists() { ShowPixelListsAndOutlines(); }
void CHeadtrackerDlg::OnSaveErrorFunction() { SaveErrorFunctions(); }

void CHeadtrackerDlg::OnLButtonDown(UINT nFlags, CPoint point) 
{
	int skipx = LEFT_MARGIN+ISIZEX, skipy = TOP_MARGIN+ISIZEY;
	int x, y;
	unsigned char *ptr;
	int col1, col2, col3;

	x = point.x - skipx * (point.x / skipx) - LEFT_MARGIN;
	y = point.y - skipy * (point.y / skipy) - TOP_MARGIN;
	ptr = Image.GetImagePtr();
	ptr += 3*(y*ISIZEX+x);
	sprintf(msg, "(%3d,%3d)", x, y);
	GetDlgItem(IDC_XY)->SetWindowText(msg);
	sprintf(msg, "(%3d,%3d,%3d)", *ptr, *(ptr+1), *(ptr+2));
	GetDlgItem(IDC_BGR)->SetWindowText(msg);
	PrintBinContents(*ptr, *(ptr+1), *(ptr+2));
	BGRToColorBins(*ptr, *(ptr+1), *(ptr+2), &col1, &col2, &col3);
	sprintf(msg, "(%3d,%3d,%3d)", col1, col2, col3);
	GetDlgItem(IDC_COLOR_BINS)->SetWindowText(msg);

	if (fp_manual) 
		fprintf(fp_manual, "%3d %3d %3d ; ...\n", m_filenum, x, y);
}

void CHeadtrackerDlg::OnTimeSequence() 
{
	struct _timeb timebuffer1, timebuffer2;
	unsigned char *images;
	int i_rel;
	int niter = 0;
	int i;
	int j;

	if (m_input_mode == IMAGE_SEQUENCE)  {
		img_reddot.Display(my_hdc,TIME_DOT_X, TIME_DOT_Y);

		// Allocate memory and read images
		numfiles = m_last_filenum - m_first_filenum + 1;
		images = (unsigned char *) 
			malloc(numfiles*BYTES_PER_PIXEL_IN_SEQUENCE*ISIZEX*ISIZEY*sizeof(unsigned char));
		if (images == NULL) {
			AfxMessageBox("Unable to allocate memory for image sequence",MB_OK|MB_ICONSTOP,0);
			return;
		}
		for (i=m_first_filenum ; i<=m_last_filenum ; i++) {
			sprintf(fname, m_basename, i);
			if (!Image.Read24BitBMP(fname)) {
				AfxMessageBox("Error reading BMP file",MB_OK|MB_ICONSTOP,0);
				break;
			} else {
				i_rel = i - m_first_filenum;
				memcpy(images+i_rel*BYTES_PER_PIXEL_IN_SEQUENCE*ISIZEX*ISIZEY,
					Image.GetImagePtr(), BYTES_PER_PIXEL_IN_SEQUENCE*ISIZEX*ISIZEY);
			}
		}

		// Track throughout sequence while timing
		_ftime( &timebuffer1 );
		for (j=0; j<3;j++) {
			for (i=m_first_filenum ; i<=m_last_filenum ; i++) {
				i_rel = i - m_first_filenum;
				memcpy(Image.GetImagePtr(), 
					images+i_rel*BYTES_PER_PIXEL_IN_SEQUENCE*ISIZEX*ISIZEY, BYTES_PER_PIXEL_IN_SEQUENCE*ISIZEX*ISIZEY);
//				Image.Display(my_hdc,LEFT_MARGIN,TOP_MARGIN);			
//				sprintf(msg, "%4d", i);
//				GetDlgItem(IDC_FRAME_NUMBER)->SetWindowText(msg);
				if (headtrackerDlg->m_tracking)  {
					TrackAndDisplay();
				}
				niter++;
			}
		}
		_ftime( &timebuffer2 );
		img_blankdot.Display(my_hdc,TIME_DOT_X, TIME_DOT_Y);
		float timediff = (timebuffer2.time - timebuffer1.time) + (timebuffer2.millitm - timebuffer1.millitm)/1000.0f;
		sprintf(msg, "Elapsed time: %5.1f seconds.  Result: %3d ms = %5.1f Hz", timediff, (int) (timediff*1000.0f/niter), niter / timediff);
		AfxMessageBox(msg, MB_OK|MB_ICONINFORMATION,0);
		free(images);
	}
}

void CHeadtrackerDlg::OnOpenManual() 
{
	if (fp_manual) fclose(fp_manual);
	fp_manual = fopen("../../seq_results/locm.m", "w");
	if (fp_manual == NULL) {
		AfxMessageBox("Error opening 'locm.m' file", MB_OK|MB_ICONSTOP,0);
	} else {
		fprintf(fp_manual, "loc = [ ...\n");
	}
}

void CHeadtrackerDlg::OnCloseManual() 
{
	if (fp_manual != NULL) {
		fprintf(fp_manual, "];\n");
		fclose(fp_manual);
		fp_manual = NULL;
	}
}

void CHeadtrackerDlg::OnOverlayPrecomputedEllipses() 
{
	FILE *fp;
	EllipseState state, state2;
	BOOL error = FALSE;
	char fname_out[80];
	int i, frame;

	if (m_input_mode == IMAGE_SEQUENCE)  {
		sprintf(msg, "../../seq1/%s", fname_ellipse_states);
		fp = fopen(msg, "r");
		if (fp == NULL) {
			sprintf(msg, "Error opening '%s' file", fname_ellipse_states);
			AfxMessageBox(msg, MB_OK|MB_ICONSTOP,0);
		} else {
			fgets(msg, 80, fp);	// ignore first line
			img_blankdot.Display(my_hdc,TRACKING_DOT_X, TRACKING_DOT_Y);
			for (i=m_first_filenum ; i<=m_last_filenum && !error ; i++) {
				sprintf(fname, m_basename, i);
				if (!Image.Read24BitBMP(fname)) {
					AfxMessageBox("Error reading BMP file",MB_OK|MB_ICONSTOP,0);
					break;
				} else {
					fscanf(fp, "%d %d %d %d\n", &frame, &(state.x), &(state.y), &(state.sz));
					if (frame != i)  {
						AfxMessageBox("Something wrong with ellipse_states file (1)",MB_OK|MB_ICONSTOP,0);
						error = TRUE;
					}
					Image.Display(my_hdc,LEFT_MARGIN,TOP_MARGIN);			
					sprintf(msg, "%4d", i);
					GetDlgItem(IDC_FRAME_NUMBER)->SetWindowText(msg);
					state2.x = state.x;  state2.y = state.y;  state2.sz = state.sz; 
					KeepStateInBounds(&state);
					if (state2.x != state.x || state2.y != state.y || state2.sz != state.sz) {
						AfxMessageBox("Something wrong with ellipse_states file (2)",MB_OK|MB_ICONSTOP,0);
						error = TRUE;
					}
					Image.ConvertToImage8(&img_gray);
#if 1 // overlay white outline => 8-bit BMP image
					OverlayOutline8(&img_gray, &state, 255, &img_outline8);
					sprintf(fname_out, "../../seq_results/overlay%03d.bmp", i);
					if (!img_outline8.Write8BitBMP(fname_out)) {
#else // overlay red outline => 24-bit BMP image
					OverlayOutlineBGR24(&img_gray, &state, 255, 0, 0, &img_outline);
					sprintf(fname_out, "../../seq_results/overlay%03d.bmp", i);
					if (!img_outline.Write24BitBMP(fname_out)) {
#endif
						AfxMessageBox("Error writing BMP file",MB_OK|MB_ICONSTOP,0);
						break;
					};
				}
			}
			img_reddot.Display(my_hdc,TRACKING_DOT_X, TRACKING_DOT_Y);
			fclose(fp);
		}
	}	
}