humanmotion.cpp

基于Opencv的人体运动检测系统

// HumanMotion.cpp: implementation of the HumanMotion class.
//
//////////////////////////////////////////////////////////////////////



#ifdef _CH_
#pragma package <opencv>
#endif


#ifndef _EiC
#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>
#include "skelecton.h"
#include "skeletons.h"
#include "kalman.h"
#include "OpenGL.h"
#include "stdafx.h"
#include "HumanMotion.h"
#endif


IplImage *image = 0, *grey = 0, *prev_grey = 0,
 *pyramid = 0, *prev_pyramid = 0,*motion_track_img = 0, *swap_temp;

IplImage* background_image = 0;

int win_size = 10;
const int MAX_COUNT = 13;  //骨架节点数
CvPoint2D32f* points[2] = {0,0}, *swap_points;
char* status = 0;
int count = 0;
int need_to_init = 1;
int night_mode = 0;
int flags = 0;
int add_remove_pt = 0;
int pause = 0;
bool init_OK = false;

CvPoint pt;
CvSize image_sz;
OpenGL *pgl;



//五个U形点
U_Joint ujoints[5];
int ujoints_count = 0;

//三个n形点，即手臂与身体两个，裆部一个
U_Joint njoints[3];
int njoints_count = 0;

const float pi = 3.1415926f;

CvMemStorage* storage = 0; // temporary storage

IplImage* abs_image = 0;
IplImage* add_abs_image = 0;
IplImage* add_abs_image2 = 0;

IplImage* dst = 0;
IplImage* src = 0;
CvSeq* contour = 0;
IplConvKernel* element = 0;


int frame_count =0;


void on_mouse( int event, int x, int y, int flags, void *param )
{
	
	if( !image )
        return;

    if( image->origin )
        y = image->height - y;

    if( event == CV_EVENT_LBUTTONDOWN )
    {
        pt = cvPoint(x,y);
        add_remove_pt = 1;
    } 
}

HumanMotion::HumanMotion()
{

}

HumanMotion::~HumanMotion()
{

}

int HumanMotion::Start()
{

	main_loop(1);
	return 0;
}

int HumanMotion::main_loop( int useCAM)
{


	skelecton *ske = new skelecton();
	skelectons *skes = new skelectons();
	kalman *bone_kalmans = new kalman[MAX_COUNT];
	
	CvCapture* capture = 0;
    
    if( useCAM == 1)
		capture = cvCaptureFromFile("e:\\mse\\movie\\capture4.avi");
	
    else if( useCAM == 2 )
        capture = cvCaptureFromCAM(-1);

    if( !capture )
    {
        fprintf(stderr,"Could not initialize capturing...\n");
        return -1;
    }

    printf( "Hot keys: \n"
            "\tESC - quit the program\n"
            "\tr - auto-initialize tracking\n"
            "\tc - delete all the points\n"
            "\tn - switch the \"night\" mode on/off\n"
			"\tp - pause on/off\n"
            "\n" );

    cvNamedWindow( "LkDemo", 1 );
	cvNamedWindow( "Contour", 1 );
    cvSetMouseCallback( "LkDemo", on_mouse, 0 );

    for(;;)
    {
        IplImage* frame = 0;
		
        int i, k, c;

        if(pause)
			frame = image ;		
		else 
			frame = cvQueryFrame( capture );

        if( !frame ) break;

        if( !image )
        {
            /* allocate all the buffers */
			image_sz = cvGetSize(frame) ;
            image = cvCreateImage(image_sz , 8, 3 );
            image->origin = frame->origin;
			
			motion_track_img = cvCreateImage( image_sz, 8, 3 );
			motion_track_img->origin = frame->origin; 
            grey = cvCreateImage( image_sz, 8, 1 );
            prev_grey = cvCreateImage( image_sz, 8, 1 );
            pyramid = cvCreateImage( image_sz, 8, 1 );
            prev_pyramid = cvCreateImage( image_sz, 8, 1 );
            points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
            points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
            status = (char*)cvAlloc(MAX_COUNT);
            flags = 0;

			CvSize sz = cvSize( image->width/2 & -1, image->height/2 & -1 );
			CvSize sz2 = cvSize( sz.width/2 & -1, sz.height/2 & -1 );			
			abs_image = cvCreateImage( cvGetSize(image), 8, 1 );
			add_abs_image =cvCreateImage(sz, 8, 1 ); 
			add_abs_image2 =cvCreateImage(sz2, 8, 1 ); 
			dst = cvCreateImage( image_sz, 8, 3 );
			dst->origin = frame->origin;

			background_image = cvCreateImage( image_sz, 8, 1 );
			background_image->origin = frame->origin;
			
			storage = cvCreateMemStorage(0);

        }

        cvCopy( frame, image, 0 );
        cvCvtColor( image, grey, CV_BGR2GRAY );
		cvZero( motion_track_img );

        if( night_mode )
            cvZero( image );
        
        if( need_to_init )
        {	
			//分析轮廓，开始自动标定
			analyze_contour(grey);
			if(init_OK) 
			{
				count = MAX_COUNT;
				cvFindCornerSubPix( grey, points[1], count,
									cvSize(win_size,win_size), cvSize(-1,-1),
 									cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
				
				for(int s=0;s<count;s++) 
				{ 
					 ske->CalcLengthRatio (s+1,points[1][s]);
					 bone_kalmans[s].init_kalman (points[1][s].x,0,points[1][s].y,0);
				}
			}

        }
        else if( count > 0 )
        {
            cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
                points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0,
                cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );
            flags |= CV_LKFLOW_PYR_A_READY;
            for( i = k = 0; i < count; i++ )
            {
                if( add_remove_pt )
                {
                    double dx = pt.x - points[1][i].x;
                    double dy = pt.y - points[1][i].y;

                    if( dx*dx + dy*dy <= 25 )
                    {
                        add_remove_pt = 0;
                        continue;
                    }
                }
                
                if( !status[i] )
                    continue;
                
                points[1][k++] = points[1][i];
				ske->CorrectPosition (i+1,points[1][i],bone_kalmans[i],grey);	
                cvCircle( image, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0);
            }
            count = k;
        }
		
		//			
		skelecton *pske;
		pske = skes->get_previous_skelecton ();
		if(pske!=NULL) 	ske->set_previous_skelecton (pske);
		ske->pgl = pgl;
		ske->drawOpenGL3D(image); 
		//ske->draw2D(image); 
		skes->add (ske);  //to do list 不需要每次都保存，应该隔单位时间保存一次
	

        if( add_remove_pt && count < MAX_COUNT )
        {
            points[1][count++] = cvPointTo32f(pt);
            cvFindCornerSubPix( grey, points[1] + count - 1, 1,
                cvSize(win_size,win_size), cvSize(-1,-1),
                cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
            add_remove_pt = 0;
			bone_kalmans[count-1].init_kalman (points[1][count-1].x,0,points[1][count-1].y,0);
			ske->CalcLengthRatio (count,points[1][count-1]);
        }

        CV_SWAP( prev_grey, grey, swap_temp );
        CV_SWAP( prev_pyramid, pyramid, swap_temp );
        CV_SWAP( points[0], points[1], swap_points );
        //need_to_init = 0;
        cvShowImage( "LkDemo", image );
		//cvShowImage( "MotionTrackDemo", motion_track_img );

        c = cvWaitKey(10);
		//printf("%c,%d",c,c);
        if( c == 27 )
            break;
        switch( c )
        {
        case 'r':
            need_to_init = 1;
            break;
        case 'c':
            count = 0;
			ske->reset();
            break;
        case 'n':
            night_mode ^= 1;
            break;
		case 'p':
            pause ^= 1;
            break;
		case 'o':            
			if(ske->ske_pause)  ske->ske_pause= false;
			else ske->ske_pause= true;
            break;
		case 'i':            
			ske->view_up();
			break;
		case 'k':            
			ske->view_down();
			break;
		case 'j':            
			ske->view_left();
			break;
		case 'l':            
			ske->view_right();
			break;
        default:
            ;
        }
		frame_count++;

    }

    cvReleaseCapture( &capture );
    cvDestroyWindow("LkDemo");
	cvDestroyWindow("Contour");
	
	
	delete ske;
	ske = NULL;
	delete skes;
	skes = NULL;
	delete []bone_kalmans;
	
    return 0;
}


int HumanMotion::analyze_contour(IplImage* grey_image)
{

	
	getContour(grey_image);
	cvShowImage( "Contour", dst );
	return 0;
}


int HumanMotion::getContour( IplImage* grey)
{
		double d_count = frame_count;
	
		cvAddWeighted(background_image, d_count/(d_count+1),
					  grey, 1/(d_count+1),
                      0,background_image);

		cvAbsDiff( grey,background_image, abs_image );
        cvThreshold( abs_image, abs_image, 50, 255, CV_THRESH_BINARY );		

		//cvPyrDown(abs_image, add_abs_image, 7);
		//cvPyrDown(add_abs_image, add_abs_image2, 7);
		//cvCanny(add_abs_image2, add_abs_image2, 20.0, 20.0*3, 3);
		
		

		int pos = 4;
		const int element_shape = CV_SHAPE_RECT;
		element = cvCreateStructuringElementEx( pos*2+1, pos*2+1, pos, pos, element_shape, 0 );		
		cvDilate(abs_image,abs_image,element,1);	
		cvErode(abs_image,abs_image,element,1);
		cvReleaseStructuringElement(&element);
		
		

        cvFindContours( abs_image, storage, &contour, sizeof(CvContour),
						CV_RETR_TREE , CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );

		cvZero( dst );

		if(contour != NULL)
		{
			//int ApproxCount = image_sz.height / 30; //估算的简化度，待完善。
			int ApproxCount = 14; 

			contour = cvApproxPoly( contour, sizeof(CvContour), storage, CV_POLY_APPROX_DP,ApproxCount, 1 );
			
			CvSeq* contour2 = contour;

			int N = contour->total;

			
			CvSeqReader reader;			
			int i;
			CvPoint pt;
			CvPoint pre_pt;

			//for (; contour != NULL; contour = contour->h_next)
			//{
				cvStartReadSeq(contour, &reader);				
				CV_READ_SEQ_ELEM(pt, reader);
				pre_pt= pt;

				for (i = 1; i < N+1; i++)
				{
					
					CV_READ_SEQ_ELEM(pt, reader);
					cvLine( dst, pt,pre_pt,CV_RGB(0,255,0),2,CV_AA, 0 );
					pre_pt= pt;

				}
			//}
			
			contour = contour2;

			if(contour != NULL) 
			{
				//简化模型中只有8个节点
				if(N==8) 
				{
					
				//	cvShowImage( "Contour", dst );