humantrackingview.cpp

opencv实现的人体运动跟踪源码

// HumanTrackingView.cpp : implementation of the CHumanTrackingView class
//

#include "stdafx.h"
#include "HumanTracking.h"

#include "HumanTrackingDoc.h"
#include "HumanTrackingView.h"
#include "Image.h"
#include "SnakeImage.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// CHumanTrackingView

IMPLEMENT_DYNCREATE(CHumanTrackingView, CView)

BEGIN_MESSAGE_MAP(CHumanTrackingView, CView)
	//{{AFX_MSG_MAP(CHumanTrackingView)
	ON_COMMAND(ID_ReadVideo, OnReadVideo)
	ON_WM_LBUTTONDOWN()
	ON_COMMAND(ID_DisplayVideo, OnDisplayVideo)
	ON_WM_RBUTTONDOWN()
	ON_COMMAND(ID_TrackOneFrame, OnTrackOneFrame)
	//}}AFX_MSG_MAP
	// Standard printing commands
	ON_COMMAND(ID_FILE_PRINT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_DIRECT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_PREVIEW, CView::OnFilePrintPreview)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CHumanTrackingView construction/destruction

CHumanTrackingView::CHumanTrackingView()
{
	// TODO: add construction code here
	DisImage = NULL;
    DisImage =  new CImage;
	markflag = false;
	meanpointx = new double ; 
	meanpointy = new double ;
	memset (meanpointx , 0 ,sizeof (*meanpointx) );
	memset( meanpointy , 0 ,sizeof (*meanpointy) );	
	Kalmanx = new CKalman( 2 , 1 , 0 );
	Kalmany = new CKalman( 2 , 1 , 0 );
}

CHumanTrackingView::~CHumanTrackingView()
{  delete DisImage;
   DisImage = NULL; 
   delete meanpointx;
   meanpointx = NULL;
   delete meanpointy;
   meanpointy = NULL;
   delete Kalmanx;
   delete Kalmany;
   Kalmanx = NULL;
   Kalmany = NULL;
}

BOOL CHumanTrackingView::PreCreateWindow(CREATESTRUCT& cs)
{
	// TODO: Modify the Window class or styles here by modifying
	//  the CREATESTRUCT cs

	return CView::PreCreateWindow(cs);
}

/////////////////////////////////////////////////////////////////////////////
// CHumanTrackingView drawing

void CHumanTrackingView::OnDraw(CDC* pDC)
{
	CHumanTrackingDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	// TODO: add draw code for native data here

}

/////////////////////////////////////////////////////////////////////////////
// CHumanTrackingView printing

BOOL CHumanTrackingView::OnPreparePrinting(CPrintInfo* pInfo)
{
	// default preparation
	return DoPreparePrinting(pInfo);
}

void CHumanTrackingView::OnBeginPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add extra initialization before printing
}

void CHumanTrackingView::OnEndPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add cleanup after printing
}

/////////////////////////////////////////////////////////////////////////////
// CHumanTrackingView diagnostics

#ifdef _DEBUG
void CHumanTrackingView::AssertValid() const
{
	CView::AssertValid();
}

void CHumanTrackingView::Dump(CDumpContext& dc) const
{
	CView::Dump(dc);
}

CHumanTrackingDoc* CHumanTrackingView::GetDocument() // non-debug version is inline
{
	ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CHumanTrackingDoc)));
	return (CHumanTrackingDoc*)m_pDocument;
}
#endif //_DEBUG

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

// CHumanTrackingView message handlers

// initial qsort
static int str_compare( const void *arg1, const void *arg2 ) {   

   return _stricmp( LPCTSTR ( *(CString*)arg1), LPCTSTR ( *(CString*)arg2) );
}

// read image names
std::vector<CString> ReadImgNames(const CString &path, const CString &extension)
{

	WIN32_FIND_DATA fd;
	HANDLE h;
	CString searchPath;
	int nbFiles;
	std::vector<CString> vFilenames;

    // add terminatin backslash (if needed)
    CString pathBS = path + '\\';
	
	// build and sort list of filenames
    searchPath = pathBS + CString("*.") + extension;
	h = FindFirstFile( searchPath, &fd );
	if (h==INVALID_HANDLE_VALUE) return 1;	// path does not exist
	vFilenames.push_back( pathBS+CString(fd.cFileName) );
	nbFiles=1;
	while( FindNextFile( h, &fd ) ) {

		vFilenames.push_back( pathBS+CString(fd.cFileName) );
		nbFiles++;
	}	

	// sort the filenames
	qsort(	(void *)&(vFilenames[0]), (size_t)nbFiles,
			sizeof( CString ), str_compare );

	// return
	return vFilenames;
}


// read pictures 
void CHumanTrackingView::OnReadVideo() 
{
    CString strPathName = "E:\\跟踪程序\\Tracking Object using Haar Features\\Tracking Object using Haar Features\\experiment_people";
	//CString strPathName = "C:\\downvideo\\car1";
	CString strExtension = "bmp";
	m_vecImgNames = ReadImgNames(strPathName, strExtension );
	::SetCurrentDirectory(strPathName);
	//CString temp;
	//temp.Format("%d images has been loaded", m_vecImgNames.size());
	//AfxMessageBox(temp);

	DisImage->ReadFromFile(m_vecImgNames[0]);
	DisImage->CreateGreyDib();
	if(DisImage){
		CClientDC dc(this);
		DisImage->Dib();			
		DisImage->Display(&dc,CPoint(0,0));
		DisImage->Data();
	}	
	AfxMessageBox("请标注初始化轮廓点：");
    markflag = true;	
}


void CHumanTrackingView::OnLButtonDown(UINT nFlags, CPoint point) 
{
	// TODO: Add your message handler code here and/or call default
	// snake initial points
    if( markflag == true){
		m_points.push_back(point);
		CPen myPen;
		myPen.CreatePen(PS_SOLID, 2, RGB(255, 0, 0));
		CClientDC dc(this);
		//  set pixel color
		dc.SetPixel(point,RGB(255,255,0)); //debug used
		dc.SelectObject(myPen);
		int count = m_points.size();	
		if(count!= 1){
			dc.MoveTo(m_points[count - 2].x,m_points[count - 2].y);
			dc.LineTo(point.x,point.y);
		}
		myPen.DeleteObject ();
		CView::OnLButtonDown(nFlags, point);
	}
}


void CHumanTrackingView::OnRButtonDown(UINT nFlags, CPoint point) 
{
	// TODO: Add your message handler code here and/or call default
	if( markflag == true){
		m_points.push_back(point);

		CPen myPen;
		myPen.CreatePen(PS_SOLID, 3, RGB(255, 0, 0));
		CClientDC dc(this);
		//  set pixel color
		dc.SetPixel(point,RGB(255,255,0)); //debug used
		dc.SelectObject(myPen);
		int count = m_points.size();	
		dc.MoveTo(m_points[count - 2].x,m_points[count - 2].y);
		dc.LineTo(point.x,point.y);
		dc.LineTo(m_points[0].x , m_points[0].y);
		myPen.DeleteObject ();
		CView::OnRButtonDown(nFlags, point);
		markflag = false;
	}
}


void CHumanTrackingView::OnDisplayVideo() 
{
	// TODO: Add your command handler code here
	double  s0x , s0y ,  s1x , s1y ;
	s0x  = *meanpointx ;  s0y = *meanpointy;
    DisImage->ImageClear();
	for(int i=1; i<m_vecImgNames.size(); i++){
		DWORD time1 = GetTickCount();
		DisImage->ReadFromFile(m_vecImgNames[i]);
		DisImage->CreateGreyDib();
		if(DisImage){
			CClientDC dc(this);
			DisImage->Dib();			
			DisImage->Display(&dc,CPoint(0,0));
			DisImage->Data();
		}
        Sleep(100);
		if(i >= 2){
			int offsetx , offsety ;
			//Kalman predict
			Kalmanx->KalmanPredict( NULL );
			Kalmany->KalmanPredict( NULL );
   
			
			//calcute differ vector
			offsetx  = *Kalmanx->state_pre - *meanpointx;
			offsety  = *Kalmany->state_pre - *meanpointy;
			for( int j = 0 ;j < m_points.size(); j++){
				m_points[j].x += offsetx;
				m_points[j].y += offsety;
			}
		
			//snake track
			OnTrackOneFrame();

			//kalman correct
			Kalmanx->KalmanCorrect( meanpointx );
			Kalmany->KalmanCorrect( meanpointy );
		}
		else if( i == 1 ){
			OnTrackOneFrame();
			s1x  = *meanpointx ;  s1y = *meanpointy;
			time1 = GetTickCount() - time1;
			
			// test
           // CString temp;
			//temp.Format("%u" ,time1);
			//AfxMessageBox(temp);

			//initial Kalman pridictor paramer
			/*   A  = { 1 , T 
				        0 , 1 };
				 H = {  1 , 0  };
				 R = {  1  };
				 Q = {  0 , 0 ,
				        0 , 1};
				S(2) = [ s2 , ( s2 - s1 ) / T  ];
				p(2) = [ 1        ,  1/ T       
				         1/T      ,  2/(T*T)+1];
			*/
           double T1 = (double)time1 ;
		   double T2 = (double)T1 * T1;

		   Kalmanx->transition_matrix[0] = Kalmany->transition_matrix[0] = 1;
		   Kalmanx->transition_matrix[1] = Kalmany->transition_matrix[1] = T1;
		   Kalmanx->transition_matrix[2] = Kalmany->transition_matrix[2] = 0;
		   Kalmanx->transition_matrix[3] = Kalmany->transition_matrix[3] = 1;
		   

		   Kalmanx->measurement_matrix[0] = Kalmany->measurement_matrix[0] = 1;
		   Kalmanx->measurement_matrix[1] = Kalmany->measurement_matrix[1] = 0;
           
		   Kalmanx->measurement_noise_cov[0] = Kalmany->measurement_noise_cov[0] = 1;

		   Kalmanx->process_noise_cov[0] = Kalmany->process_noise_cov[0] = 0;
		   Kalmanx->process_noise_cov[1] = Kalmany->process_noise_cov[1] = 0;
		   Kalmanx->process_noise_cov[2] = Kalmany->process_noise_cov[2] = 0;
		   Kalmanx->process_noise_cov[3] = Kalmany->process_noise_cov[3] = 1;

		   Kalmanx->state_post[0] = s1x;                Kalmany->state_post[0] = s1y;
		   Kalmanx->state_post[1] = (s1x-s0x)/T1;       Kalmany->state_post[1] = (s1y-s0y)/T1;
		   
		   Kalmanx->error_cov_post[0] = Kalmany->error_cov_post[0] = 1;              
		   Kalmanx->error_cov_post[1] = Kalmany->error_cov_post[1] = 1/T1;              
		   Kalmanx->error_cov_post[2] = Kalmany->error_cov_post[2] = 1/T1;              
		   Kalmanx->error_cov_post[3] = Kalmany->error_cov_post[3] = 2/T2 + 1;              
          // AfxMessageBox("Kalman initial finished!");
		}	
		DisImage->ImageClear();
// 		if(i == 49 || i == 69 || i == 89 || i == 99){
// 			CString tmp;
// 			tmp.Format("第%d帧",i+1);
// 			AfxMessageBox(tmp);
// 		}
 		if(i == 389 || i == 19 || i == 29 || i == 39){
 			CString tmp;
 			tmp.Format("第%d帧",i+1);
 			AfxMessageBox(tmp);
 		}
	}
	AfxMessageBox("OK!");	
}

void CHumanTrackingView::OnTrackOneFrame() 
{
	// TODO: Add your command handler code here
	BOOL flag = SnakeImage(   DisImage, &m_points[0], m_points.size(),
                              0.03 , 0.005 , 1, CSize(7,7));
	if(flag == true){
		CPen myPen;
		myPen.CreatePen(PS_SOLID, 3, RGB(0, 255, 0));
		CClientDC dc(this);
		dc.SelectObject(myPen);
		int count = m_points.size();	
		dc.MoveTo(m_points[0].x,m_points[0].y);
		
		*meanpointx = m_points[0].x;
		*meanpointy = m_points[0].y;
	
		for(int i = 1 ; i < count ; i++){
		    dc.LineTo(m_points[i].x , m_points[i].y);
			dc.MoveTo(m_points[i].x , m_points[i].y);
			*meanpointx += m_points[i].x;
			*meanpointy += m_points[i].y;	
		}
		*meanpointx /= ( double )count;
		*meanpointy /= ( double )count;
		dc.LineTo(m_points[0].x , m_points[0].y);
		myPen.DeleteObject();
	}
	else{
		AfxMessageBox("Snake Track bad!");
	}
}

// test T 
//          DWORD time1 = GetTickCount();
// 			time1 = GetTickCount() - time1;
// 			
// 			// test
//          CString temp;
// 			temp.Format("%u" ,time1);
// 			AfxMessageBox(temp);