track.cpp

利用web camera对目标进行特征跟踪的程序 对于初学机器视觉的有些帮助

	if(!pTemplateImage || !pTargetImage) return false;

	DWORD dwSize = nTargetHeight*nTargetWidth;
	int position = 0;
	int position_l = 0;

	//分配内存,初始化	  
    double b[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
	//设置当前b值
    double b_next[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
	  
	bool bConverge = FALSE;
	int nCount = 	0;
	double delt_delt = delt*delt;

	double temp_b0_1,temp_b0_2,temp_b1_1,temp_b1_2,temp_b2_1,temp_b2_2,temp_b3_1,temp_b3_2,temp_b4_1,temp_b4_2,temp_b5_1,temp_b5_2;
	double iCenter_x,iCenter_y;
    iCenter_x = ((double)nTargetWidth-1)/2;
	iCenter_y = ((double)nTargetHeight-1)/2;
	  
	double temp_delt_05 = delt_delt*0.5;
	double temp_position;
	double temp_position_dsize;
	double ii,jj,temp_x;
	double temp,tempi,tempj,temp1,temp2;
	int i,j;
	bool bFirst = TRUE;
	  
	  while((nCount<2)&&(!bConverge))
	  {
		  temp_b0_2 = temp_b1_2 = temp_b2_2 = temp_b3_2 = temp_b4_2 = temp_b5_2 = 0;

		  //以下的参数只需要计算一次就行了:
		  if(bFirst)
			temp_b0_1 = temp_b1_1 = temp_b2_1 = temp_b3_1 = temp_b4_1 = temp_b5_1 = 0;
	
		  position  = 0;
		  for(j=0; j<nTargetHeight; j++)
		  {
			  for(i=0; i<nTargetWidth; i++) 
			  {
			  //对于小背景可以通过设置阈值滤掉,但是对于复杂背景设置应将阈值放大
	//		 if((type==1)||((type==4)&&((level && m_pTargetMatricSecond[position])||(!level && m_pTargetMatric[position]))))
			  {
				  ii = (double)i-iCenter_x;
				  jj = (double)j-iCenter_y;
				  //temp_position 为在position处的差分值:x
				  temp_position = pTargetDifference[position];
				  temp_position_dsize = pTargetDifference[position+dwSize];
				  //temp为x的平方
				  temp  = temp_position * temp_position;
				  //横纵坐标的平方
				  tempi = ii*ii;
				  tempj = jj*jj;
				  
				  //求fai(x,del);
				  temp_x = ((temp_position       * (b[0]+ii*b[1]+jj*b[2]) )+ 
					       (temp_position_dsize * (b[3]+ii*b[4]+jj*b[5]) )  )+
					       (pTargetImage[position] - pTemplateImage[position]);
				  temp_x = (2 * temp_x * delt_delt) / ((delt_delt + temp_x*temp_x)*(delt_delt + temp_x*temp_x));//temp_x*2;// 
				  
				  //最后的标志_1为计算分母的累加值,_2为计算分子的累加值
				  if(bFirst) temp_b0_1 += temp;
				  temp1      = temp_position  *  temp_x;
				  temp_b0_2 += temp1;
				  
				  if(bFirst) temp_b1_1 += temp * tempi;
				  temp_b1_2 += temp1 * ii; 

				  if(bFirst) temp_b2_1 += temp * tempj;
				  temp_b2_2 += temp1 * jj;

				  temp = temp_position_dsize * temp_position_dsize;

				  if(bFirst) temp_b3_1 += temp;
				  temp2      = temp_position_dsize  * temp_x;
				  temp_b3_2 += temp2;

				  if(bFirst) temp_b4_1 += temp * tempi;
				  temp_b4_2 += temp2 * ii;

				  if(bFirst) temp_b5_1 += temp * tempj;
				  temp_b5_2 += temp2 * jj;
			  }  
				  position ++;
			  }
			 
		  }
		  
		  if(bFirst)
		  {
			  temp_b0_1 = temp_delt_05 / temp_b0_1;
			  temp_b1_1 = temp_delt_05 / temp_b1_1;
			  temp_b2_1 = temp_delt_05 / temp_b2_1;
			  temp_b3_1 = temp_delt_05 / temp_b3_1;
			  temp_b4_1 = temp_delt_05 / temp_b4_1;
			  temp_b5_1 = temp_delt_05 / temp_b5_1;
		  }
		  bFirst = FALSE;
		  b_next[0] = temp_b0_2 * temp_b0_1;
		  b_next[1] = temp_b1_2 * temp_b1_1;
		  b_next[2] = temp_b2_2 * temp_b2_1;
		  b_next[3] = temp_b3_2 * temp_b3_1;
		  b_next[4] = temp_b4_2 * temp_b4_1;
		  b_next[5] = temp_b5_2 * temp_b5_1;
         

		  bConverge = TRUE;
		  if(b_next[0] >0.1 || b_next[0]<-0.1|| b_next[3]>0.1||b_next[3]<-0.1)
			  bConverge = FALSE;
		  if((bConverge)&&((b_next[1] > 0.005 || b_next[1]<-0.005)||(b_next[4] > 0.005 || b_next[4]<-0.005)||(b_next[5] > 0.005 || b_next[5]<-0.005))||(b_next[2] > 0.005 || b_next[2]<-0.005))
			  bConverge = FALSE;

		   for(i=0; i<6; i++)
			  b[i] -= b_next[i];

		  nCount ++;
	  }
	
	//拷贝结果:
	for(i=0; i<6; i++)
	   a[i] = b[i];

	return true;
}
/////////////////////////////////////////////////////////////////////////////////////////////////
//
//  计算当前金字塔层的I,到输入图像中抠取目标区域
//
/////////////////////////////////////////////////////////////////////////////////////////////////
bool CTrack::PyramidSampleTargetImageFromPictureAndCalculateDifferenceMatric(DWORD dHeight_now,DWORD dWidth_now,double* a,double* pInputImage,DWORD ImageWidth,DWORD ImageHeight,double *pTargetImage,double *pDifferenceMatric)
{
	double x,y,temp,x1,x2,x3,y1,y2,y3;
	double x_center = ((double)dWidth_now-1)/2;
	double y_center = ((double)dHeight_now-1)/2;
	double dMean    = 0;
	double dVariance = 0;
    DWORD position,i,j,position_l=0,dwObjectSize_now=dWidth_now*dHeight_now;
	double x_addition;
	double y_addition;
	
	
    //================有效性检测========================	
	if(a[1]*a[1]+a[4]*a[4] <0.001 || a[2]*a[2]+a[5]*a[5] <0.001)
	{
	//	AfxMessageBox("在采集当前选择框所覆盖的目标区时，区域瞬间变得太小！");
	//	return false;
	}
	if(a[1]*a[1]+a[4]*a[4] >9 || a[2]*a[2]+a[5]*a[5] >9)
	{
	//	AfxMessageBox("在采集当前选择框所覆盖的目标区时，区域瞬间变得太大！");
	//	return false;
	}
	//检测匹配框是否出界:(检测四个顶点是否出界)
	x  = a[0]-x_center*a[1]-y_center*a[2] -x_center ;
	x1 = a[0]+x_center*a[1]-y_center*a[2] +x_center ;
	x2 = a[0]-x_center*a[1]+y_center*a[2] -x_center ;
	x3 = a[0]+x_center*a[1]+y_center*a[2] +x_center ;
	y  = a[3]-x_center*a[4]-y_center*a[5] -y_center ;
	y1 = a[3]+x_center*a[4]-y_center*a[5] -y_center ;
	y2 = a[3]-x_center*a[4]+y_center*a[5] +y_center ;
	y3 = a[3]+x_center*a[4]+y_center*a[5] +y_center ;
	if(x<2||x1<2||x2<2||x3<2|| x>ImageWidth-2||x1>ImageWidth-2||x2>ImageWidth-2||x3>ImageWidth-2||  y<2||y1<2||y2<2||y3<2|| y1>ImageHeight-2||y2>ImageHeight-2||y3>ImageHeight-2||y>ImageHeight-2 ) 
	{
	//	AfxMessageBox("在采集当前选择框所覆盖的目标区时，发生越界！");
		return false;
	}

	double x_Left;
	double y_Left;
	//仿射变换,上中心偏移量
	x_Left = a[0] -x_center*a[1] -y_center*a[2] -x_center  ;
	y_Left = a[3] -x_center*a[4] -y_center*a[5] -y_center  ;

	x_addition = a[1]+1;
	y_addition = a[4];

	//采集目标图象
	for(j=0; j<dHeight_now; j++)
	{
		x = x_Left;
		y = y_Left;
		x_Left += a[2]; 
		y_Left += a[5]+1;
		for(i=0; i<dWidth_now; i++)
		{
			//计算当前点在输入图像中的位置
			position = (int)(y) * ImageWidth + (int)(x);

       //============采集该点处的灰度值================
			//抽样：在2*2临域内采用内插法：
			temp  = pInputImage[position] - (pInputImage[position] - pInputImage[position+1])* (x-(int)x);
			position += ImageWidth;
			temp -= (temp-(pInputImage[position] - (pInputImage[position] - pInputImage[position+1])* (x-(int)x)))*(y-(int)y);
			//插值结果赋给相应的标准化目标点:
			pTargetImage[position_l] = temp;
			dMean += temp;
			dVariance+= temp*temp;

	  //===========下面计算差分矩阵==================
			// 行差分
			position = (int)(y+0.5) * ImageWidth + (int)(x+0.5);
			temp = 2*(pInputImage[position+1])+pInputImage[position+1+ImageWidth]+pInputImage[position+1-ImageWidth]
				-2*(pInputImage[position-1])-pInputImage[position-1+ImageWidth]-pInputImage[position-1-ImageWidth];
			temp /= 8;
			pDifferenceMatric[position_l] = temp;
			// 列差分
			temp = 2*pInputImage[position+ImageWidth]+pInputImage[position+ImageWidth-1]+pInputImage[position+ImageWidth+1]
				-2*pInputImage[position-ImageWidth]-pInputImage[position-ImageWidth-1]-pInputImage[position-ImageWidth+1];
			temp /= 8;
			pDifferenceMatric[position_l+dwObjectSize_now] = temp;

			position_l ++;//计算下一个点
			x += x_addition;
			y += y_addition;
		}
	}

	// 0 均值化:
	position = dWidth_now * dHeight_now;
	dMean /= (double)position;

	m_dVarianceTarget = sqrt(dVariance/position - dMean*dMean);
	m_dGradationOffset = (BYTE) dMean;

	if(TrackType==TT_AFFINE)
	{
		for(i=0; i<position; i++)
			pTargetImage[i] -= dMean;//*30.0/dVariance;

	}

    return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  计算金字塔当前层
//  主要用于计算b参数，也就是计算一次a(n+1),来调整一次a
//
//////////////////////////////////////////////////////////////////////////////////////////////////////
bool CTrack::PyramidLevel(double* a,double delt,double* pInputImage,DWORD dwImageHeight,DWORD dwImageWidth,double*& pTemplate,double* pEigenSpace,DWORD dwTargetHeight,DWORD dwTargetWidth)
{
	  int i;
      //计算当前层的图像大小
	  DWORD dwSize   = dwTargetWidth * dwTargetHeight;

	  //设置目标图像和差分图像数组
	  double *pTargetImage      = (double*) new double [dwSize];
	  double *pTargetDifference = (double*) new double [dwSize*2];

//===========================抠取目标图象,并计算差分图像===============================================
	  if(!PyramidSampleTargetImageFromPictureAndCalculateDifferenceMatric(dwTargetHeight,dwTargetWidth,a,pInputImage,dwImageWidth,dwImageHeight,pTargetImage,pTargetDifference))
	  	  goto ERROR_PyramidLevel;
//==============计算重构图像====================================
	  if(TrackType==TT_SVD_AFFINE)
	  {
		  if(!ReconstructImageUseSVD(pTargetImage,dwSize,pEigenSpace,m_nNumberOfSelectedEigenVectors,pTemplate))
			  goto ERROR_PyramidLevel;
	  }else if(TrackType==TT_KL_AFFINE)
	  {
		  if(!ReconstructImageUseKL(pTargetImage,dwSize,pEigenSpace,m_nNumberOfSelectedEigenVectors,pTemplate))
			  goto ERROR_PyramidLevel;
	  }

///=============求解仿射参数=======================================
	double b[6];
	if(!CalculateParameter(pTemplate, dwTargetHeight, dwTargetWidth, pTargetImage, pTargetDifference, delt, b))
		goto ERROR_PyramidLevel;

    //在原来的基础上加上计算出来得增量,得到目标的最终姿态的仿射参数表示
	for(i=0;i<6;i++)
  	   a[i] += b[i];

	//释放无用空间;
	delete []pTargetImage;
	delete []pTargetDifference;
	return true;
	//错误返回：	
ERROR_PyramidLevel:
	delete []pTargetImage;
	delete []pTargetDifference;
	return false;
}
///////////////////////////////////////////////////////////////////////////////////
//
//  跟踪主函数，也是与外部的调用接口:
//  作用是：对于当前帧的输入图像做一次检测目标, 这只是一个面向外界的启动按钮函数
//
///////////////////////////////////////////////////////////////////////////////////
bool CTrack::Track(double* pBitmapInputSet)
{
	//将当前帧输入图像的指针保留下来：
	int j = m_dwBitmapInputWidth*m_dwBitmapInputHeight;
	if(m_pBitmapInput) delete m_pBitmapInput;
	m_pBitmapInput = new double [j];
	memcpy(m_pBitmapInput,pBitmapInputSet,j*sizeof(double));
	
	PyramidSampleImage(m_dwBitmapInputHeight,m_dwBitmapInputWidth,m_pBitmapInput,m_pBitmapInput_SecondLevel);

    //保留上一帧的参数,作为当前帧的初始值：
	for(int i=0; i<6; i++)	a[i] = a_last[i];

 	//计算当前参数下搜索的情况，根据返回值判断是否需要搜索：
	m_bSearchStatus = false;

	i = Search(0,m_bSearchStatus);
 	if(!i) goto TRACK_ERROR;
	if(i==2) //使用搜索：如果当前的模板和目标之间的误差较大，则采用搜索方法
	{
		//先将参数恢复为原来的值
		for(i=0; i<6; i++)	a[i] = a_last[i];
		//在搜索一次，如果仍然找不到，则返回错误!
		m_bSearchStatus= true;
		if(!Search(5,m_bSearchStatus))  goto TRACK_ERROR;
		m_bSearchStatus = false;
		if(!Search(0,1)) goto TRACK_ERROR;
	}
	
	//在屏幕中画出结果来(将标准化的目标图像 取出来)：
	if(!GetTargetImageByGivenAffineParameters(m_pBitmapTarget,a))
		goto TRACK_ERROR;
	
	if(TemplateUpdateType == TTUT_PWOER && TrackType == TT_AFFINE)
	{
		if(m_dTemplateUpdatePower > 0.00001)
			m_dTemplateUpdatePower = 0.0125;
		else
			m_dTemplateUpdatePower = 0;
	
		UpdateTemplate();
	}
	
	//参数预测
	for(i=0;i<6;i++)
	{
		a_add[i] = a_add[i]*0.5 + (a[i]-a_last[i])*0.5;
		a_last[i] = a[i];
	}
	return true;

TRACK_ERROR:
	for(i=0;i<6;i++)
		a[i] = 0;
	return false;
}
////////////////////////////////////////////////////////////////////////
//
//采用搜索机制，找出极值点  bFine:寻找为0，跟踪为1
////////////////////////////////////////////////////////////////////////
int CTrack::Search(int nScope,bool bFine)
{
	DWORD dwSize = m_dwTargetWidth*m_dwTargetHeight;  //目标大小
	DWORD dwErrorCounterMin,dwErrorCounter;
	double a_min[6];  //保存最小值
	double a_last[6]; //保存a
	int m,n,x,y;
	int nStep = bFine ? 1:2;
	nScope /= 2;