lktrack.cpp

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

#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 <GL/gl.h>
#include <GL/glu.h>
#include <GL/glaux.h>

#endif




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


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 = 0;
int night_mode = 0;
int flags = 0;
int add_remove_pt = 0;
int pause = 0;


CvPoint pt;
OpenGL *pgl;
Vector3f  vt1,vt2;
void CALLBACK display3()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	
	glLoadIdentity();
		pgl->clear ();
	glColor3f(0.4f,0.5f,0.7f);
		//auxSolidCylinder(0.2,1.0);
		//pgl->draw2point (vt1,vt2);
		pgl->display ();
	//glFlush();

}

void opengl_test()
{
	pgl = new OpenGL();
	pgl->Init();

	  vt1.x = 200;
	  vt1.y = 200;
	  vt1.z = 200;
	  vt2.x = 0;
	  vt2.y = 0;
	  vt2.z = 0;
	// auxMainLoop(display3);

	for(;;) display3();
	delete pgl;


}



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

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

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



int cvmain( int argc, char** argv )
{
    //opengl_test();
	//exit(0);

	skelecton *ske = new skelecton();
	skelectons *skes = new skelectons();
	kalman *bone_kalmans = new kalman[MAX_COUNT];
	
	CvCapture* capture = 0;
    
    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
        //capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
		capture = cvCaptureFromFile("e:\\MyFirstPoser.AVI");
    else if( argc == 2 )
        capture = cvCaptureFromAVI( argv[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( "MotionTrackDemo", 0 );
    cvSetMouseCallback( "LkDemo", on_mouse );

    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 = cvCreateImage( cvGetSize(frame), 8, 3 );
            image->origin = frame->origin;
			
			motion_track_img = cvCreateImage( cvGetSize(frame), 8, 3 );
			motion_track_img->origin = frame->origin;
            grey = cvCreateImage( cvGetSize(frame), 8, 1 );
            prev_grey = cvCreateImage( cvGetSize(frame), 8, 1 );
            pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
            prev_pyramid = cvCreateImage( cvGetSize(frame), 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;
        }

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

        if( night_mode )
            cvZero( image );
        
        if( need_to_init )
        {
            /* automatic initialization */
            IplImage* eig = cvCreateImage( cvGetSize(grey), 32, 1 );
            IplImage* temp = cvCreateImage( cvGetSize(grey), 32, 1 );
            double quality = 0.01;
            double min_distance = 10;

            count = MAX_COUNT;
            cvGoodFeaturesToTrack( grey, eig, temp, points[1], &count,
                                   quality, min_distance, 0 );
            cvFindCornerSubPix( grey, points[1], count,
                cvSize(win_size,win_size), cvSize(-1,-1),
                cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
            cvReleaseImage( &eig );
            cvReleaseImage( &temp );

            add_remove_pt = 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];
					switch(i)
						{
						case 0:							
							ske->CorrectPosition ("neck",points[1][i],bone_kalmans[i]);		
							break;
						case 1:							
							ske->CorrectPosition ("head",points[1][i],bone_kalmans[i]);	
							break;
						case 2:							
							ske->CorrectPosition ("rShoulder",points[1][i],bone_kalmans[i]);	
							break;
						case 3:
							ske->CorrectPosition ("lShoulder",points[1][i],bone_kalmans[i]);								
							break;
						case 4:							
							ske->CorrectPosition ("rElbow",points[1][i],bone_kalmans[i]);
							break;
						case 5:
							ske->CorrectPosition ("lElbow",points[1][i],bone_kalmans[i]);
							break;
						case 6:
							ske->CorrectPosition ("rWrist",points[1][i],bone_kalmans[i]);
							break;
						case 7:
							ske->CorrectPosition ("lWrist",points[1][i],bone_kalmans[i]);
							break;
						case 8:
							ske->CorrectPosition ("Waist",points[1][i],bone_kalmans[i]);
							break;
						case 9:
							ske->CorrectPosition ("rKnee",points[1][i],bone_kalmans[i]);
							break;
						case 10:
							ske->CorrectPosition ("lKnee",points[1][i],bone_kalmans[i]);
							break;
						case 11:
							ske->CorrectPosition ("rAnkle",points[1][i],bone_kalmans[i]);
							break;
						case 12:
							ske->CorrectPosition ("lAnkle",points[1][i],bone_kalmans[i]);
							break;
						}
                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->draw3D(motion_track_img); 
		ske->drawOpenGL3D(image); 
		skes->add (ske);  //不需要每次都保存，应该隔单位时间保存一次
		//skes->test ();

        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 == 'q' )
            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:
            ;
        }

    }

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