skelecton.cpp

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

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


#include "skelecton.h"
#include "kalman.h"
#include <string.h>
#include <math.h>
#include "vector3f.h"
#include <assert.h>

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
int coordinator_X,coordinator_Y=0; //图像中心坐标
const double MaxMinZero=0.0000001;  //小于这个值认为是0
const double f=200;  //pin-hole的有效焦距
const double StardardLength=500;  //对应现实世界的人体的高度，实际上是对应的象素
const double StardardLengthNoiseRatio = 0.3 ; //测量误差占整个测量的比率
double arc_y= 0;//y轴旋转15度
double arc_z= 0;//z轴旋转15度


skelecton::skelecton()
{
	bone *pbone = new bone (1,"neck",0);
	bonevec.push_back (*pbone);	

	pbone = new bone (2,"head",1);	
	pbone->start_arc_x = 30.0;   //角度定义为右手坐标系
	pbone->end_arc_x = -30.0;
	pbone->start_arc_z = 10.0;
	pbone->end_arc_z = -80.0;
	bonevec.push_back (*pbone);	

	pbone = new bone (3,"rShoulder",1);	
	bonevec.push_back (*pbone);	

	pbone = new bone (4,"lShoulder",1);	
	bonevec.push_back (*pbone);	

	pbone = new bone (5,"rElbow",3);	
	pbone->start_arc_y = -90;
	pbone->end_arc_y = 90;
	pbone->start_arc_z = 90;
	pbone->end_arc_z = -60.0;
	pbone->CID = 7;
	bonevec.push_back (*pbone);	

	pbone = new bone  (6,"lElbow",4);	
	pbone->start_arc_y = -90;
	pbone->end_arc_y = 90;
	pbone->start_arc_z = -90;
	pbone->end_arc_z = 60.0;
	pbone->LengthRatio = 0.15;
	pbone->CID = 8;
	bonevec.push_back (*pbone);	

	pbone = new bone (7,"rWrist",5);	
	bonevec.push_back (*pbone);	

	pbone = new bone (8,"lWrist",6);	
	bonevec.push_back (*pbone);	

	bonevec.push_back (bone (9,"Waist",1));

	pbone = new bone (10,"rKnee",9);	
	pbone->CID = 12;
	bonevec.push_back (*pbone);	

	pbone = new bone(11,"lKnee",9);	
	pbone->CID = 13;
	bonevec.push_back (*pbone);	

	bonevec.push_back (bone (12,"rAnkle",10));
	bonevec.push_back (bone (13,"lAnkle",11));

	delete pbone; 
	pbone=NULL;

	pre_ske=NULL;
	//pgl = m_OpenGL;
	//pgl->Init();
	
}

skelecton::~skelecton()
{	
  //这里释放出错，需要再次检查是否有循环引用
	/*
	if(pre_ske!=NULL) 
	{
		delete pre_ske;
		pre_ske = NULL;
	}

	if(pgl!=NULL) 
	{
		delete pgl;
		pgl = NULL;
	}
*/
	bonevec.empty(); 

}

int skelecton::draw2D(CvArr* motion_track_img)
{
	bone_vector::iterator theIterator;
	bone *pbone = 0;
	bone *ppbone = 0;
		
    for (theIterator = bonevec.begin(); theIterator != bonevec.end();
         theIterator++)
    {
		  pbone = theIterator;		   
		  cvCircle( motion_track_img, cvPoint(pbone->x,pbone->y),10, CV_RGB(0,255,0),3, CV_AA, 0 );
		  if ( 0 != pbone->PID )  
		  {
			  ppbone = get_bone_by_ID(pbone->PID); 
			  cvLine( motion_track_img, cvPoint(pbone->x,pbone->y),cvPoint(ppbone->x,ppbone->y),CV_RGB(0,255,0),2,CV_AA, 0 );
		  }

	} 
	return 0;
}

bone* skelecton::get_bone_by_name(char *name)
{
	bone_vector::iterator theIterator;
	bone_vector::iterator theIterator2;
	theIterator2  = 0;
	
	
    for (theIterator = bonevec.begin(); theIterator != bonevec.end();
         theIterator++)
    {
			 if (0 == strncmp(theIterator->name, name , 4 ))
			 {
				 theIterator2=theIterator;
				 break;
			 }                                         
	} 
	return (theIterator2);
}

bone* skelecton::get_bone_by_ID(int ID)
{

	return (&bonevec[ID-1]);	
}

int skelecton::reset()
{
	bone_vector::iterator theIterator;
	bone *pbone = 0;
	
		
    for (theIterator = bonevec.begin(); theIterator != bonevec.end();
         theIterator++)
    {
		  pbone = theIterator;		   
		  pbone->x =0 ;
		  pbone->y =0 ;	
		  pbone->is_marked = 0;
		 
	} 
	return 0;
}

int skelecton::draw3D(CvArr *motion_track_img)
{
	//2d坐标转化为3d坐标
	bone_vector::iterator theIterator;
	bone *pbone = 0;
	bone *ppbone = 0;	
	
	ppbone = get_bone_by_name("head"); 
	ppbone->r_z =0;  //初始化head的坐标为0
	ppbone->r_x=ppbone->x - coordinator_X;
	ppbone->r_y=ppbone->y - coordinator_Y;

	
	CvSize ImageSize; 
	ImageSize = cvGetSize(motion_track_img);
	
	coordinator_X = (int)ImageSize.width/2;
	coordinator_Y = (int)ImageSize.height/2;
	
	CalcBonePosition();

	//绘制3d坐标系
	if(ske_pause) arc_y+=5;
	const double pi = 3.1415926535;
	double cosx = cos(arc_y/90*pi);
	double sinx = sin(arc_y/90*pi);
	double cosx_z = cos(arc_z/90*pi);
	double sinx_z = sin(arc_z/90*pi);

	const float ZX[] = { 1, 0, 0, 0,
						0,cosx,sinx,0,					
						0,-sinx,cosx,0,
						0,0,0,1
						};

	const float ZY[] = { cosx,0,sinx,0,	
						 0,1,0,0,
						-sinx,0,cosx,0,
						0,0,0,1
						};

	const float ZZ[] = { cosx_z,sinx_z, 0, 0,
						-sinx_z,cosx_z,0,0,					
						0,0,1,0,
						0,0,0,1
						};

	const float Zoom[] = { 
						1,0,0,0,
						0,1,0,0,					
						0,0,1,0,
						coordinator_X,coordinator_Y,coordinator_X,1
						};  //缩放比率,并且平移100
	CvMat* RotateMatrix = cvCreateMat( 4, 4, CV_32FC1 ); //旋转矩阵
	memcpy( RotateMatrix->data.fl, ZY, sizeof(ZY));      
	CvMat* tempMatrix = cvCreateMat( 4, 4, CV_32FC1 );
	memcpy( tempMatrix->data.fl, ZZ, sizeof(ZZ));
	cvMatMul( RotateMatrix, tempMatrix, RotateMatrix);	
	memcpy( tempMatrix->data.fl, Zoom, sizeof(Zoom));
	cvMatMul( RotateMatrix, tempMatrix, RotateMatrix);  

	CvMat* PointVector = cvCreateMat( 1, 4, CV_32FC1 ); //点矢量矩阵
	float pt[] = { 0,0, 0, 1};
	int ox,oy=0; //原点
	memcpy( PointVector->data.fl, pt, sizeof(pt));
	cvMatMul(PointVector,RotateMatrix,PointVector);	
	ox = PointVector->data.fl[0];
	oy = PointVector->data.fl[1];

	pt[0]=300 ;
	pt[1]=0 ;
	pt[2]=0;
	memcpy( PointVector->data.fl, pt, sizeof(pt));
	cvMatMul(PointVector,RotateMatrix,PointVector);	
	cvLine( motion_track_img, cvPoint(ox,oy),cvPoint(PointVector->data.fl[0],PointVector->data.fl[1]),CV_RGB(255,255,255),2,CV_AA, 0 );
	//printf( "PointVector->data.fl[0,1]: %f,%f \n",PointVector->data.fl[0],PointVector->data.fl[1]);
	
	pt[0]=0 ;
	pt[1]=300 ;
	pt[2]=0;
	memcpy( PointVector->data.fl, pt, sizeof(pt));
	cvMatMul(PointVector,RotateMatrix,PointVector);	
	cvLine( motion_track_img, cvPoint(ox,oy),cvPoint(PointVector->data.fl[0],PointVector->data.fl[1]),CV_RGB(0,255,255),2,CV_AA, 0 );
	//printf( "PointVector->data.fl[0,1]: %f,%f \n",PointVector->data.fl[0],PointVector->data.fl[1]);
	
	pt[0]=0 ;
	pt[1]=0 ;
	pt[2]=300;
	memcpy( PointVector->data.fl, pt, sizeof(pt));
	cvMatMul(PointVector,RotateMatrix,PointVector);
	cvLine( motion_track_img, cvPoint(ox,oy),cvPoint(PointVector->data.fl[0],PointVector->data.fl[1]),CV_RGB(255,255,0),2,CV_AA, 0 );
	//printf( "PointVector->data.fl[0,1]: %f,%f \n",PointVector->data.fl[0],PointVector->data.fl[1]);


		
    for (theIterator = bonevec.begin(); theIterator != bonevec.end();
         theIterator++)
    {
		  pbone = theIterator;	
		  
			pt[0]=pbone->r_x ;
			pt[1]=pbone->r_y ;
			pt[2]=pbone->r_z ;
			memcpy( PointVector->data.fl, pt, sizeof(pt));
			cvMatMul(PointVector,RotateMatrix,PointVector);
			pbone->rotated_X = PointVector->data.fl[0];
			pbone->rotated_Y = PointVector->data.fl[1];		
			
			if(pbone->is_marked==0)  continue; //未标记则不画
/*
			printf( "%s,r_x,r_y,r_z: %d,%d,%d \n",pbone->name,pbone->r_x,pbone->r_y,pbone->r_z);
			printf( "rotated_X,Y: %d,%d \n",pbone->rotated_X,pbone->rotated_Y);
			printf( "---------------------------------------------------\n");
*/
			cvCircle( motion_track_img, cvPoint(pbone->rotated_X,pbone->rotated_Y),5, CV_RGB(0,255,0),2, CV_AA, 0 );
			  if ( 0 != pbone->PID )  
			  {
				  ppbone = get_bone_by_ID(pbone->PID); 
				  cvLine( motion_track_img, cvPoint(pbone->rotated_X,pbone->rotated_Y),cvPoint(ppbone->rotated_X,ppbone->rotated_Y),CV_RGB(0,255,0),2,CV_AA, 0 );
		  
			  }

	} 
	
	pgl->display();
    return 0;
}

//\假设pin hole模型
int skelecton::CalcBoneRealWorldPosition(char *pname, char *ppName,double LengthRatio)
{

	bone *ppbone = 0;
	bone *pbone = 0;
	int cx0,cy0=0; //校正后的坐标
	int cx,cy=0; //校正后的坐标
	double A,B,C,det,sqdet,x1,x2=0;

	ppbone = get_bone_by_name(pname); 
	pbone = get_bone_by_name(ppName); 
	cx0= ppbone->r_x;
	cy0= ppbone->r_y;	
	cx= pbone->x - coordinator_X ;
	cy= pbone->y - coordinator_Y ;
	
	//'求解二元一次方程

	A = pow(cx/f,2)  + pow(cy/f,2) + 1;
	B = -2*(cx/f)*ppbone ->r_x -2*(cy/f)*ppbone ->r_y - 2*ppbone->r_z;
	C = pow(ppbone->r_x,2) + pow(ppbone->r_y,2) + pow(ppbone->r_z,2) - pow(LengthRatio*StardardLength,2);
		//'B^2-4AC
		det = pow(B,2) - 4*A*C;
		//'det>0 两个解
		if (det>MaxMinZero)
		{
			//需要舍弃一个无意义的解
			sqdet = sqrt(det);
			x1=(-B+sqdet)/(2*A);
			x2=(-B-sqdet)/(2*A);
		}
		//'det==0 单解
		else if (det<MaxMinZero && det>0)
		{
			x1=x2=(-B)/(2*A);
		} 
		//'det<0 无解
		else if (det<0)
		{
			//需要再次探索
			x1=x2=0;
		}	
	pbone->r_z =x1;
	pbone ->r_x = pbone->x * pbone->r_z / f;
	pbone ->r_y = pbone->y * pbone->r_z / f;

	/*
	printf( "A: %f \n",A);
	printf( "B: %f \n",B);
	printf( "C: %f \n",C);	
	
	*/
	
	return 0;

}

//\假设平行光模型
int skelecton::CalcBoneRealWorldPosition_parallel(char *pname, char *ppName)
{

	bone *ppbone = 0;
	bone *pbone = 0;
	int cx0,cy0=0; //校正后的坐标