humanmotion.cpp

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

				//	cvWaitKey(0);
					SegmentBody(contour);
					FindBody();
					ClearBodyJoints();

				}
			}

		}

        
		return 0;

}

int HumanMotion::SegmentBody(CvSeq* contour)
{
	const float StandBodyLength = 1.0;
	const float StandArc = 90;
	const float nStandArc = 90;
	float l1,l2=0;
	CvPoint pt1,pt2;
	double total_arc = 0; //内旋角度
	double rarc12= 0;
	double arc12= 0;	
	bool Haslkvec = false; //是否存在先前记录的矢量
	bool HasFoundedJoint = false; //是否找到拐点
	

	CvSeqReader reader;
	CvSeqReader subReader;
	int N = contour->total;
	int i;
	CvPoint pt;
	CvPoint pre_pt;
	CvPoint pre_pre_pt;

	ujoints_count=0;
	njoints_count=0;


	cvStartReadSeq(contour, &reader);

	for (i= 0; i < N; i++)
	{
		CV_READ_SEQ_ELEM(pt, reader);
		pre_pt = pre_pre_pt = pt;
		//printf("in sub %d,%d \n",pt.x,pt.y);
		 
		//计算两点之间的距离
		subReader = reader ;

		Haslkvec = false;
		total_arc =0;


		for(int j=0;j<3;j++)
		{		
			
			l1=calc_2Point_Magnitude(pt,pre_pt);
			if (l1>StandBodyLength)
			{
				//是否存在先前记录的点，如果有，检查角度范围
				if(Haslkvec)
				{
					
					Vector3f vt1 =  Vector3f (pt2.x-pt1.x,pt2.y-pt1.y,0);
					Vector3f vt2 =  Vector3f (pt1.x-pt.x,pt1.y-pt.y,0);
					arc12 = AngleBetweenVectors(vt1,vt2);
					rarc12 = 180*arc12/pi;
					Vector3f vtnor=Cross(vt1,vt2); //使用叉集检查矢量是内旋还是外旋。
					if(vtnor.z <0) total_arc -= rarc12; 
					else total_arc += rarc12; 

					//检查角度范围合理U -200 -160
					if (abs(total_arc+180)<StandArc)  
					{
			
						//绘出U形的位置
						//draw_2Points(pt,pre_pt,pt1,pt2,CV_RGB(255,255,250));
						save_Points(pt,pre_pt,pt1,pt2,U_TYPE);
						HasFoundedJoint = true ;

					}

					//检查角度范围N 150 210
					if (abs(total_arc-180)<nStandArc)  
					{
						//记录n形点的位置
						//draw_2Points(pt,pre_pt,pt1,pt2,CV_RGB(230,255,0));
						save_Points(pt,pre_pt,pt1,pt2,N_TYPE);
						HasFoundedJoint = true ;

					}


					if(HasFoundedJoint)
					{

						//记录两个矢量，清空标记
						Haslkvec = false;
						total_arc = 0;	
						HasFoundedJoint = false;
						break;
					}


				}
				else
				{
					//记录两个点
					pt1=pt;
					pt2=pre_pt;
					Haslkvec = true;
				}
				
			}


			pre_pre_pt = pre_pt;
			pre_pt= pt;
			CV_READ_SEQ_ELEM(pt, subReader);
			

		}

	}


	return 0;
}



float HumanMotion::calc_2Point_Magnitude(CvPoint pt1,CvPoint pt2)
{
	return (float)sqrt( pow((pt1.x-pt2.x),2) + pow((pt1.y-pt2.y),2) );
	
}

int HumanMotion::draw_2Points(CvPoint pt1,CvPoint pt2,CvPoint pt3,CvPoint pt4,CvScalar color)
{

	
		cvLine( dst,pt1,pt2,color,1, CV_AA, 0);
		cvLine( dst,pt3,pt4,color,1, CV_AA, 0);
		cvLine( dst,pt1,pt4,color,1, CV_AA, 0);
		cvLine( dst,pt2,pt3,color,1, CV_AA, 0);

		return 0;
}


//检查平行线之间构成四边形是否在同一轴线上，正方形相似度判断
//方法如下，取两个矢量中心点连线，取连线与较大矢量的夹角，如果夹角在90度偏差20度内认为是正方形。
bool HumanMotion::Is_Closed_Vectors(CvPoint pt1,CvPoint pt2,CvPoint pt3,CvPoint pt4)
{
	
	CvPoint pt5,pt6;
	const float StandSqArc = 40;
	pt5.x= (pt1.x + pt2.x ) /2;
	pt5.y= (pt1.y + pt2.y ) /2;
	pt6.x= (pt3.x + pt4.x ) /2;
	pt6.y= (pt3.y + pt4.y ) /2;

	Vector3f vt1 = Vector3f (pt1.x-pt2.x,pt1.y-pt2.y,0);
	Vector3f vt2 = Vector3f (pt3.x-pt4.x,pt3.y-pt4.y,0);
	Vector3f vtc = Vector3f (pt6.x-pt5.x,pt6.y-pt5.y,0);

	Vector3f vtmax;
	if(Magnitude(vt1) > Magnitude(vt2)) 
		vtmax= vt1;
	else
		vtmax= vt2;

	double arc12 = AngleBetweenVectors(vtc,vtmax);
	double rarc12 = 180*arc12/pi;
	if (abs(abs(rarc12)-90)<StandSqArc) 
		return true;
	else
		return false;
}

//排除的第二种情况
//1.两矢量长度不等，差距相差4倍时，认为无效
//2.两矢量错位，不协调，即对角线长度大于两矢量长度的两倍。
bool HumanMotion::Is_Apar_Vectors(CvPoint pt1,CvPoint pt2,CvPoint pt3,CvPoint pt4)
{
	bool result = false;
	const float StandLenRatio = 4.0;  
	const float StandCrossLenRatio = 2.0;  

	Vector3f vt1 = Vector3f (pt1.x-pt2.x,pt1.y-pt2.y,0);
	Vector3f vt2 = Vector3f (pt3.x-pt4.x,pt3.y-pt4.y,0);


	//1.两矢量长度不等，差距相差4倍时，认为无效
	double lenvct1,lenvct2,raido12;
	lenvct1 = Magnitude(vt1);
	lenvct2 = Magnitude(vt2);
	raido12 = lenvct1/lenvct2;
	if( raido12<StandLenRatio && raido12 > 1/StandLenRatio )
		result = true ;

	//2.两矢量错位，不协调，即对角线长度大于两矢量长度的两倍。
	if(result)
	{
		Vector3f vtc1 = Vector3f (pt1.x-pt3.x,pt1.y-pt3.y,0); //对角线1
		Vector3f vtc2 = Vector3f (pt1.x-pt3.x,pt1.y-pt3.y,0); //对角线2
		double c1,c2,ratio12;
		double vct_min,cross_vct_max;
		c1 = Magnitude(vtc1);
		c2 = Magnitude(vtc2);
		if (lenvct1>lenvct2) 
			vct_min = lenvct2; 
		else
			vct_min = lenvct1; 
		if(c1>c2)
			cross_vct_max = c1;
		else
			cross_vct_max = c2;
		ratio12 = cross_vct_max/vct_min;
		if (ratio12<StandCrossLenRatio) 
			result = true ;
		else
			result = false;
	}

	return result;
}

int HumanMotion::FindBody()
{
	init_OK = false;
	if(ujoints_count==5 && njoints_count==3)
	{
	


		CvPoint pt1,pt2,pt3,pthead;
		//右腋下
		pt1 = njoints[0].pt;

		//裆部
		pt2 = njoints[1].pt;
	
		//左腋下
		pt3 = njoints[2].pt;

		//寻找head 节点
		int Head_i;
		int temp = 0;
		for(int i= 0; i<5 ;i++)
		{
			pthead.y = ujoints[i].pt.y ;
			if (pthead.y>temp) Head_i =i;
		}
		pthead=ujoints[Head_i].pt ;


		CvPoint pt_neck;
		pt_neck.x = ( pthead.x - pt2.x )*0.75 + pt2.x; 
		pt_neck.y = ( pthead.y - pt2.y )*0.75 + pt2.y;

		CvPoint pt_waist;
		pt_waist.x = ( pthead.x - pt2.x )*0.3 + pt2.x; 
		pt_waist.y = ( pthead.y - pt2.y )*0.3 + pt2.y;

		CvPoint pt_lhand;
		pt_lhand = ujoints[3].pt ;

		CvPoint pt_rhand;
		pt_rhand = ujoints[0].pt ;



		CvPoint pt_rAnkle;
		pt_rAnkle = ujoints[1].pt ;

		CvPoint pt_lAnkle;
		pt_lAnkle = ujoints[2].pt ;

		CvPoint pt_rknee;
		pt_rknee.x  =( pt_waist.x +  pt_rAnkle.x ) /2;
		pt_rknee.y  =( pt_waist.y +  pt_rAnkle.y ) /2;

		CvPoint pt_lknee;
		pt_lknee.x  =( pt_waist.x +  pt_lAnkle.x ) /2;
		pt_lknee.y  =( pt_waist.y +  pt_lAnkle.y ) /2;

		CvPoint pt_lshoulder;
		//pt_lshoulder.x = ( pthead.x - pt_lhand.x )*(pt_neck.y - pt_lhand.y)/(pthead.y - pt_lhand.y) + pt_lhand.x; 
		//pt_lshoulder.y = pt_neck.y;

		CvPoint pt_rshoulder;
		//pt_rshoulder.x =( pthead.x - pt_rhand.x )*(pt_neck.y - pt_rhand.y)/(pthead.y - pt_rhand.y) + pt_rhand.x; 
		//pt_rshoulder.y = pt_neck.y;

		CvPoint pt_relbow;
		pt_relbow.x  =( pt_rshoulder.x +  pt_rhand.x ) /2;
		pt_relbow.y  =( pt_rshoulder.y +  pt_rhand.y ) /2;

		CvPoint pt_lelbow;
		pt_lelbow.x  =( pt_lshoulder.x +  pt_lhand.x ) /2;
		pt_lelbow.y  =( pt_lshoulder.y +  pt_lhand.y ) /2;



		ZoomLine(pthead,pt_neck);
		
		ZoomLine(pt_neck,pt_waist);
		ZoomLine(pt_neck,pt_rshoulder);
		ZoomLine(pt_neck,pt_lshoulder);
		ZoomLine(pt_rshoulder,pt_relbow);
		ZoomLine(pt_lshoulder,pt_lelbow);
		ZoomLine(pt_relbow,pt_rhand);
		ZoomLine(pt_lelbow,pt_lhand);
		ZoomLine(pt_waist,pt_rAnkle);
		ZoomLine(pt_waist,pt_lAnkle);

		points[1][0] = Point_to_32f(pt_neck) ;
		points[1][1] = Point_to_32f(pthead) ;
		points[1][2] = Point_to_32f(pt_rshoulder) ;
		points[1][3] = Point_to_32f(pt_lshoulder) ;
		points[1][4] = Point_to_32f(pt_relbow) ;
		points[1][5] = Point_to_32f(pt_lelbow) ;
		points[1][6] = Point_to_32f(pt_rhand) ;
		points[1][7] = Point_to_32f(pt_lhand) ;
		points[1][8] = Point_to_32f(pt_waist) ;
		points[1][9] = Point_to_32f(pt_rknee) ;
		points[1][10] = Point_to_32f(pt_lknee) ;
		points[1][11] = Point_to_32f(pt_rAnkle) ;
		points[1][12] = Point_to_32f(pt_lAnkle) ;


		need_to_init = 0 ;
		init_OK = true ;
		cvShowImage( "LkDemo", image );
		cvWaitKey(0);


	}

	return 0;
}

int  HumanMotion::ClearBodyJoints()
{
	for(int i= 0; i<5 ;i++) ujoints[i].HasSet = false;
	for(i= 0; i<3 ;i++) njoints[i].HasSet = false;
	ujoints_count = 0;
	njoints_count = 0;
	return 0;
}
int HumanMotion::save_Points(CvPoint pt1,CvPoint pt2,CvPoint pt3,CvPoint pt4,JointType upoint_type)
{
	if(upoint_type == U_TYPE)
	{
		ujoints_count = ujoints_count % 5;
		ujoints[ujoints_count].next_pt = pt1;
		ujoints[ujoints_count].pt = pt2;
		ujoints[ujoints_count].pre_pt = pt4;
		ujoints[ujoints_count].HasSet = true;
		ujoints_count ++;
	}
	
	if(upoint_type == N_TYPE)
	{
		njoints_count = njoints_count % 3;
		njoints[njoints_count].next_pt = pt1;
		njoints[njoints_count].pt = pt2;
		njoints[njoints_count].pre_pt = pt4;
		njoints[njoints_count].HasSet = true;
		njoints_count ++;
	}

	return 0;
}


int HumanMotion::ZoomLine(CvPoint pt1,CvPoint pt2)
{
	CvPoint pts1,pts2;
	int zoom = 1;
	pts1.x = pt1.x * zoom ;
	pts1.y = pt1.y * zoom ;
	pts2.x = pt2.x * zoom ;
	pts2.y = pt2.y * zoom ;

	cvCircle( image,pts1,5, CV_RGB(0,233,250),3, CV_AA, 0 );	
	cvCircle( image,pts2,5, CV_RGB(0,233,250),3, CV_AA, 0 );	
	cvLine( image,pts1,pts2,CV_RGB(240,210,0),1, CV_AA, 0);

	cvShowImage( "LkDemo", image );
		cvWaitKey(0);


	return 0;

}

CvPoint2D32f HumanMotion::Point_to_32f(CvPoint pt)
{
	CvPoint2D32f pt32;
	int zoom = 1;
	pt32.x = pt.x * zoom;
	pt32.y = pt.y * zoom;
	return pt32;
}