headtrackerdlg.cpp

人头跟踪算法

// HeadtrackerDlg.cpp : implementation file
//
#include "stdafx.h"
// #include "string.h"
#include "Headtracker.h"
#include "HeadtrackerDlg.h"
#include <sys/timeb.h>	// time()
#include <time.h>		// time()

#include "digitizer.h"
#include "track.h"
#include "base.h" // ISIZE[XY]
#include "camera.h"
#include "color_histogram.h"	// BGRToColorBins

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#define LEFT_MARGIN 10
#define TOP_MARGIN 10
#define MAX_STRING_LENGTH 80
#define DIALOG_BACKGROUND_COLOR 192 // This is the dialog background color
#define RECORDING_DOT_X 236		// location of red dot indicating recording
#define RECORDING_DOT_Y 445
#define TRACKING_DOT_X 50		// location of red dot indicating tracker locked
#define TRACKING_DOT_Y 373
#define TIME_DOT_X 595			// location of red dot indicating sequence is being timed
#define TIME_DOT_Y 473
#define TIMER_ID	1
#define BYTES_PER_PIXEL_IN_SEQUENCE 3 // keep this at 3 for now
#define N_DISPLAY_SPEEDS 8
#define INITIAL_DISPLAY_SPEED 4

Digitizer_Params dparams;	
HANDLE imageready;
HANDLE VideoThread;
CDC *my_cdc;
HDC my_hdc;
enum {LIVE_VIDEO, IMAGE_SEQUENCE};
enum {DIG_BUSTER = 0, DIG_WINNOV = 1};
BOOL recording_sequence = FALSE;
enum {PLAY, STOP, FORWARD, BACKWARD};
int playmode = STOP;
int numfiles;
CHeadtrackerDlg *headtrackerDlg;
ImageBGR24 Image(ISIZEX, ISIZEY);
ImageBGR24 img_reddot(8,8);
Image8 img_blankdot(8,8);
char fname[MAX_STRING_LENGTH];
char filename_model[] = "model_stanb.dat";

extern Image8 img_gray;
extern Image16 img_gauss;
extern Image8 img_grad;
extern Image16 img_gradx;
extern Image16 img_grady;
extern Image8 img_gradx8;
extern Image8 img_grady8;
extern Image16 img_tmp16;
extern ImageBGR24 img_outline;
extern ImageBGR24 img_outline_grad;
extern ImageBGR24 img_outline_color;
extern Image8 img_outline8;
extern Image8 img_outline_grad8;
extern Image8 img_outline_color8;
extern Image8 img_mask;
extern Image8 img_color1;
extern Image8 img_color2;
extern Image8 img_color3;
extern Image8 img_color1ref;
extern Image8 img_color2ref;
extern Image8 img_color3ref;
extern Image8 img_color1q;
extern Image8 img_color2q;
extern Image8 img_color3q;
extern Image8 img_likelihood;
extern Image8 img_backprojection;
extern Image8 img_lights;
extern Image8 img_bin;
static Image8 img_tmp8(ISIZEX,ISIZEY);
static ImageBGR24 img_tmp24(ISIZEX,ISIZEY);
//extern Image8 img_ch_model;
//extern Image8 img_ch_new;
extern int tmp1, tmp2;
extern int tmp3;
Image8 blankimg(ISIZEX, 256);
BOOL building_model = FALSE;
static char msg[80];
BOOL use_gradient, use_color; // evidence used by tracker
BOOL sum_gradient_magnitude;
BOOL zoom_control;
BOOL adapt_histogram;
BOOL show_details;
unsigned char *recorded_images;
int *ellipse_states;
EllipseState curr_state;	// The current state (x,y,sz) of the ellipse
char fname_ellipse_states[] = "ellipse_states.txt";
static int display_speeds[N_DISPLAY_SPEEDS] = {1, 6, 10, 15, 30, 60, 90, 1000};
static int display_speed;

/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// Dialog Data
	//{{AFX_DATA(CAboutDlg)
	enum { IDD = IDD_ABOUTBOX };
	//}}AFX_DATA

	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CAboutDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
	//}}AFX_VIRTUAL

// Implementation
protected:
	//{{AFX_MSG(CAboutDlg)
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
	//{{AFX_DATA_INIT(CAboutDlg)
	//}}AFX_DATA_INIT
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CAboutDlg)
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
	//{{AFX_MSG_MAP(CAboutDlg)
		// No message handlers
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CHeadtrackerDlg dialog

CHeadtrackerDlg::CHeadtrackerDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CHeadtrackerDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CHeadtrackerDlg)
	m_input_mode = -1;
	m_enable_recording = FALSE;
	m_last_filenum = 0;
	m_basename = _T("");
	m_first_filenum = 0;
	m_tracking = FALSE;
	m_building_model = FALSE;
	m_use_camera = -1;
	m_input_file_bw_instead_of_color = -1;
	m_filenum = 0;
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CHeadtrackerDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CHeadtrackerDlg)
	DDX_CBIndex(pDX, IDC_INPUT_MODE, m_input_mode);
	DDX_Check(pDX, IDC_ENABLE_RECORDING, m_enable_recording);
	DDX_Text(pDX, IDC_LAST, m_last_filenum);
	DDV_MinMaxInt(pDX, m_last_filenum, 0, 9999);
	DDX_Text(pDX, IDC_BASENAME, m_basename);
	DDX_Text(pDX, IDC_FIRST, m_first_filenum);
	DDV_MinMaxInt(pDX, m_first_filenum, 0, 9999);
	DDX_Check(pDX, IDC_CHECK_TRACKING, m_tracking);
	DDX_Check(pDX, IDC_BUILDING_MODEL, m_building_model);
	DDX_Radio(pDX, IDC_CAMERA_OFF, m_use_camera);
	DDX_Radio(pDX, IDC_INPUT_FILE_COLOR, m_input_file_bw_instead_of_color);
	DDX_Text(pDX, IDC_FRAME_NUMBER, m_filenum);
	DDV_MinMaxInt(pDX, m_filenum, 0, 9999);
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CHeadtrackerDlg, CDialog)
	//{{AFX_MSG_MAP(CHeadtrackerDlg)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_WM_DESTROY()
	ON_CBN_SELCHANGE(IDC_INPUT_MODE, OnSelchangeInputMode)
	ON_BN_CLICKED(IDC_CHECK_TRACKING, OnCheckTracking)
	ON_BN_CLICKED(IDC_RECORD_SEQ, OnRecordSeq)
	ON_WM_TIMER()
	ON_BN_CLICKED(IDC_PLAY_SEQ, OnPlaySeq)
	ON_BN_CLICKED(IDC_FORWARD, OnForward)
	ON_BN_CLICKED(IDC_BACKWARD, OnBackward)
	ON_BN_CLICKED(IDC_STOP_PLAYING, OnStopPlaying)
	ON_EN_CHANGE(IDC_BASENAME, OnChangeBasename)
	ON_WM_LBUTTONDOWN()
	ON_BN_CLICKED(IDC_SAVE_MODEL, OnSaveModel)
	ON_BN_CLICKED(IDC_BUILDING_MODEL, OnBuildingModel)
	ON_BN_CLICKED(IDC_SHOW_PIXEL_LISTS, OnShowPixelLists)
	ON_BN_CLICKED(IDC_CAMERA_HOME, OnCameraHome)
	ON_BN_CLICKED(IDC_LOCK_WHITE_BALANCE, OnLockWhiteBalance)
	ON_BN_CLICKED(IDC_USE_GRADIENT, OnUseGradient)
	ON_BN_CLICKED(IDC_USE_COLOR, OnUseColor)
	ON_BN_CLICKED(IDC_ZOOM, OnZoom)
	ON_BN_CLICKED(IDC_ADAPT_HISTOGRAM, OnAdaptHistogram)
	ON_BN_CLICKED(IDC_CAMERA_OFF, OnCameraOff)
	ON_BN_CLICKED(IDC_CAMERA_ON, OnCameraOn)
	ON_BN_CLICKED(IDC_GRADIENT_MAG, OnGradientMag)
	ON_BN_CLICKED(IDC_GRADIENT_DOT, OnGradientDot)
	ON_BN_CLICKED(IDC_TIME_SEQUENCE, OnTimeSequence)
	ON_BN_CLICKED(IDC_SAVE_ERROR_FUNCTION, OnSaveErrorFunction)
	ON_BN_CLICKED(IDC_OPEN_MANUAL, OnOpenManual)
	ON_BN_CLICKED(IDC_CLOSE_MANUAL, OnCloseManual)
	ON_BN_CLICKED(IDC_SHOW_DETAILS, OnShowDetails)
	ON_BN_CLICKED(IDC_OVERLAY_PRECOMPUTED_ELLIPSES, OnOverlayPrecomputedEllipses)
	ON_BN_CLICKED(IDC_CONTINUE_PLAYING, OnContinuePlaying)
	ON_WM_HSCROLL()
	ON_BN_CLICKED(IDC_INPUT_FILE_BW, OnInputFileBw)
	ON_BN_CLICKED(IDC_INPUT_FILE_COLOR, OnInputFileColor)
	ON_EN_KILLFOCUS(IDC_LAST, OnKillfocusLast)
	ON_EN_KILLFOCUS(IDC_FIRST, OnKillfocusFirst)
	ON_EN_KILLFOCUS(IDC_BASENAME, OnKillfocusBasename)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()


/////////////////////////////////////////////////////////////////////////////
// 

void PrintDisplaySpeed(int speed)
{
	sprintf(msg, (speed<0 ?		"                         " : 
								"Speed:  %4d Hz  "), speed);
	my_cdc->TextOut(465, 520, msg);
}

void PrintEditNumber4D(int id, int num)
{
	sprintf(msg, "%4d", num);
	headtrackerDlg->GetDlgItem(id)->SetWindowText(msg);
}

void PrintEditString(int id, char *str)
{
	headtrackerDlg->GetDlgItem(id)->SetWindowText(str);
}

void CHeadtrackerDlg::EnableDisableWindows() 
{
	switch (m_input_mode) {
	case LIVE_VIDEO:
		GetDlgItem(IDC_STATIC_CAMERA)->EnableWindow(TRUE);
		GetDlgItem(IDC_CAMERA_ON)->EnableWindow(TRUE);
		GetDlgItem(IDC_CAMERA_OFF)->EnableWindow(TRUE);
		GetDlgItem(IDC_ZOOM)->EnableWindow(TRUE);
		GetDlgItem(IDC_CAMERA_HOME)->EnableWindow(TRUE);
		GetDlgItem(IDC_LOCK_WHITE_BALANCE)->EnableWindow(TRUE);
		GetDlgItem(IDC_RECORD_SEQ)->EnableWindow(TRUE);
		GetDlgItem(IDC_ENABLE_RECORDING)->EnableWindow(TRUE);
		GetDlgItem(IDC_INPUT_FILE_COLOR)->EnableWindow(FALSE);
		GetDlgItem(IDC_INPUT_FILE_BW)->EnableWindow(FALSE);
		GetDlgItem(IDC_PLAY_SEQ)->EnableWindow(FALSE);
		GetDlgItem(IDC_FORWARD)->EnableWindow(FALSE);
		GetDlgItem(IDC_BACKWARD)->EnableWindow(FALSE);
		GetDlgItem(IDC_STOP_PLAYING)->EnableWindow(TRUE);
		GetDlgItem(IDC_STATIC_FRAME_NUMBER)->EnableWindow(FALSE);
		GetDlgItem(IDC_FRAME_NUMBER)->EnableWindow(FALSE);
		GetDlgItem(IDC_SCROLLBAR_SPEED)->EnableWindow(FALSE);
		GetDlgItem(IDC_TIME_SEQUENCE)->EnableWindow(FALSE);
		GetDlgItem(IDC_OVERLAY_PRECOMPUTED_ELLIPSES)->EnableWindow(FALSE);
		PrintDisplaySpeed(-1);
		break;
	case IMAGE_SEQUENCE:
		GetDlgItem(IDC_STATIC_CAMERA)->EnableWindow(FALSE);
		GetDlgItem(IDC_CAMERA_ON)->EnableWindow(FALSE);
		GetDlgItem(IDC_CAMERA_OFF)->EnableWindow(FALSE);
		GetDlgItem(IDC_ZOOM)->EnableWindow(FALSE);
		GetDlgItem(IDC_CAMERA_HOME)->EnableWindow(FALSE);
		GetDlgItem(IDC_LOCK_WHITE_BALANCE)->EnableWindow(FALSE);
		GetDlgItem(IDC_RECORD_SEQ)->EnableWindow(FALSE);
		GetDlgItem(IDC_ENABLE_RECORDING)->EnableWindow(FALSE);
		GetDlgItem(IDC_INPUT_FILE_COLOR)->EnableWindow(TRUE);
		GetDlgItem(IDC_INPUT_FILE_BW)->EnableWindow(TRUE);
		GetDlgItem(IDC_PLAY_SEQ)->EnableWindow(TRUE);
		GetDlgItem(IDC_FORWARD)->EnableWindow(TRUE);
		GetDlgItem(IDC_BACKWARD)->EnableWindow(TRUE);
		GetDlgItem(IDC_STOP_PLAYING)->EnableWindow(TRUE);
		GetDlgItem(IDC_STATIC_FRAME_NUMBER)->EnableWindow(TRUE);
		GetDlgItem(IDC_FRAME_NUMBER)->EnableWindow(TRUE);
		GetDlgItem(IDC_SCROLLBAR_SPEED)->EnableWindow(TRUE);
		GetDlgItem(IDC_TIME_SEQUENCE)->EnableWindow(TRUE);
		GetDlgItem(IDC_OVERLAY_PRECOMPUTED_ELLIPSES)->EnableWindow(TRUE);
		PrintDisplaySpeed(display_speed);
		break;
	}

}

void CHeadtrackerDlg::EnableDisableMagDot(BOOL ug) 
{
	GetDlgItem(IDC_GRADIENT_MAG)->EnableWindow(ug);
	GetDlgItem(IDC_GRADIENT_DOT)->EnableWindow(ug);
}

void TrackAndDisplay()
{
	static BOOL tracker_locked_old = FALSE;
	int left = 1, top = 0;

//	Digitizer_Shutdown(0);
	track(&Image, &curr_state);
	if (headtrackerDlg->m_use_camera) MoveCamera(&curr_state);
//	track(&Image, use_camera);
//	sprintf(msg, "%3d %3d", tmp1, tmp2);
//	headtrackerDlg->GetDlgItem(IDC_XY)->SetWindowText(msg);
//	sprintf(msg, "%d", tmp3);
//	headtrackerDlg->GetDlgItem(IDC_BGR)->SetWindowText(msg);

//		img_reddot.Display(my_hdc,TRACKING_DOT_X, TRACKING_DOT_Y);
//		img_gray.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
//		img_gauss.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
		img_outline.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
		if (show_details) {
			img_outline_grad8.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
			img_outline_color8.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
			img_grad.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_bspline2.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
			left = 0; top++;
	//		img_tmp16.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_gradx.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_grady.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
			img_likelihood.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
			img_bin.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
			img_backprojection.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
			img_gradx8.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
			img_grady8.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_lights.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_mask.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_color1q.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_color2q.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_color3q.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_ch_model.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_ch_new.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_overlay.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
	//		img_overlay2.Display(my_hdc,(left+1)*LEFT_MARGIN+left*ISIZEX,(top+1)*TOP_MARGIN+top*ISIZEY); left++;
		}

//	sprintf(msg, "camera:  %4d        ", headtrackerDlg->m_use_camera);
//	sprintf(msg, "camera:  %4d        ", use_camera);
//	my_cdc->TextOut(5, 215, msg);
}

static int VideoLoop(int reserved)
{
	Image8 tmpimg(ISIZEX,ISIZEY);	
	DWORD timestamp;
//	int left, top;

	while (1)
	{
		if (WaitForSingleObject(imageready,3000) != WAIT_FAILED) //wait for framegrabber pulse