track.cpp

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

	double threshold;  //对于不同的情况设置不同的提前退出门限:
	if(TrackType==TT_SVD_AFFINE || TrackType==TT_KL_AFFINE) threshold = 0.4;
	else if(TrackType==TT_AFFINE) threshold=0.2;
	
	//设置初始值
	for(int i=0; i<6; i++)	  a_last[i]=a[i];

	//计算初始点处的误差： 
	dwErrorCounterMin  = CalculateCurrentParameters(a,m_dDelt,bFine);  
	for(i=0; i<6; i++)  a_min[i] = a[i];
	if(dwErrorCounterMin<threshold*dwSize) 	goto EXIT_SEARCH;

	for( m=1;  m<nScope; m++ )
	for( x=-m; x<=m;     x++ )
	{
		y = m - abs(x);
		for( n=0; n<2; y=-y,n++ )
		{
			for(i=0; i<6; i++)	  a[i] = a_last[i];
			a[0] += x*nStep;
			a[3] += y*nStep;
			
			//计算在新的参数下的模板和实际得到的图象的误差
			dwErrorCounter = CalculateCurrentParameters(a,m_dDelt,bFine);
			if( dwErrorCounter<dwErrorCounterMin )
			{
				dwErrorCounterMin = dwErrorCounter;
				for(i=0; i<6; i++)  a_min[i] = a[i];
				if(dwErrorCounterMin<threshold*dwSize) 
					goto EXIT_SEARCH;  // 设定一个提前退出门限:
			}
		}
	}
	
EXIT_SEARCH:
	for(i=0;i<6;i++) 
		a[i] = a_min[i];

	//================有效性检测========================	
	if(dwErrorCounterMin<m_dwTargetWidth*m_dwTargetHeight) 
	{
		if(dwErrorCounterMin>(m_dwTargetWidth*m_dwTargetHeight*0.8)) 
			return 2;
		else
			return 1;
	}
	else 
	{
		AfxMessageBox("CTrack::Searching()目标不存在!");
		return 0;
	}
	return 1;
}
////////////////////////////////////////////////////////////////////////////////////
//
//  计算当前参数下的匹配结果：
//  本函数的主要目的是引起金字塔形运算
//
////////////////////////////////////////////////////////////////////////////////////
DWORD CTrack::CalculateCurrentParameters(double *a, double delt_now,bool bTypeCourseOrFine=TRUE)
{
	DWORD dwErrorCounter;
	int	  nCount_2=0;
	bool  bConverge = false;
	int   nLoopNumber = m_nMaxIterateTimes=8;
	int   i;

	double *pTemp=NULL;

	DWORD dwTargetHeight_now,dwTargetWidth_now;
	DWORD dwImageHeight_now,dwImageWidth_now;
	DWORD dwSize = m_dwTargetHeight * m_dwTargetWidth;
	int level ,nFactor;
	
	//准备数据:输入图象、模板图象:
	double* pInputImage[2];
	pInputImage[0] =  m_pBitmapInput;
	pInputImage[1] =  m_pBitmapInput_SecondLevel;
	
	//设置当前模板图象指针；
	double **pTemplate[2];
	//设置当前特征空间：
	double *pEigenSpace[2];

	//准备模板图象:2个金字塔层
	pTemplate[0] = &m_pBitmapTemplate;
	pTemplate[1] = &m_pBitmapTemplate_SecondLevel;

	if(TrackType==TT_AFFINE)
		pEigenSpace[0] = pEigenSpace[1] = NULL;
	else if(TrackType==TT_SVD_AFFINE || TrackType==TT_KL_AFFINE)
	{   //准备特征空间:2个金字塔层
		pEigenSpace[0] = m_pEigenSpace;
		pEigenSpace[1] = m_pEigenSpace_SecondLevel;
	}else{
		dwErrorCounter = dwSize +1;
		goto EXIT_CalculateCurrentParameters;
	}

	double a_back[6];
	for(i=0;i<6;i++)
		a_back[i] = a[i];
  
		
//通过降低Delt值,做下列循环
//   while (nCount<3)  在一般的算法中没有考虑这一步
	{
		//在当前Delt值处作15遍
	  while ((nCount_2<nLoopNumber)&&(!bConverge))
		{
		    //采用的金字塔层数:
		    level   = m_nPyramidLevel-1;
			nFactor = 1<<level;
		  
			//将仿射变换参数映射到最高一层
			if(level)
			{
				a[0] /= nFactor;	
				a[3] /= nFactor; 
			
				if((m_dwTargetHeight)%2) 	a[3] -= 0.5;
				else 				  	    a[3] -= 0.25;
				if((m_dwTargetWidth) %2)	a[0] -= 0.5;
				else 				    	a[0] -= 0.25;
			}

			//从金字塔顶到底做一遍:level从1->0
	    	while (level>=0)
			{
				 //需要将大小进行映射:
				nFactor = 1 << level;
				dwTargetHeight_now = m_dwTargetHeight / nFactor;
				dwTargetWidth_now  = m_dwTargetWidth  / nFactor;
				
				dwImageHeight_now = m_dwBitmapInputHeight / nFactor;
				dwImageWidth_now  = m_dwBitmapInputWidth  / nFactor;
				
				//如果出错，则返回一个比实际目标面积大的值
		    	if(!PyramidLevel(a,delt_now,pInputImage[level],dwImageHeight_now,dwImageWidth_now,*(pTemplate[level]),pEigenSpace[level],dwTargetHeight_now,dwTargetWidth_now)) 
				{
					dwErrorCounter = dwSize +1;
					goto EXIT_CalculateCurrentParameters;
				}

				//将仿射变换参数映射到底一层
				if(level)
				{	
					a[0] *= 2;		a[3] *= 2;	

					if((m_dwTargetHeight)%2) 	a[3] += 1;
					else 				  	    a[3] += 0.5;
					if((m_dwTargetWidth) %2)	a[0] += 1;
					else 				    	a[0] += 0.5;
				}
				//跳至下一层:
				level--; 
				
				//如果是搜索阶段，则只在目标的第二层金字塔中搜寻，所以跳开第0层搜索
				if(!bTypeCourseOrFine) level=-1;
			}
			//判断a是否收敛
			bConverge = true;
			if((a_back[0]-a[0] >0.1) || (a_back[0]-a[0]<-0.1) || (a_back[3]-a[3] >0.1) || (a_back[3]-a[3]<-0.1))			  bConverge = FALSE;
		    if((bConverge)&&((a_back[1]-a[1] > 0.005 || a_back[1]-a[1]<-0.005)||(a_back[4] -a[4]> 0.005 || a_back[4]-a[4]<-0.005)||(a_back[5]-a[5] > 0.005 || a_back[5]-a[5]<-0.005))||(a_back[2]-a[2] > 0.005 || a_back[2]-a[2]<-0.005))		  bConverge = false;

			//将本次的结果作为下次的初值:
	    	for(i=0; i<6; i++)         a_back[i] = a[i];

			//如果是当前帧最终结果，则求出和模板的相似性度量结果
			if(bConverge || nCount_2 ==nLoopNumber-1)
			{
				//抠取目标，计算误差：
				if(!GetTargetImageByGivenAffineParameters(m_pBitmapTarget,a))
					return dwSize+1;
				if(TrackType==TT_SVD_AFFINE)
					ReconstructImageUseSVD(m_pBitmapTarget,m_dwTargetHeight*m_dwTargetWidth,m_pEigenSpace,m_nNumberOfSelectedEigenVectors,m_pBitmapTemplate);
				else if(TrackType==TT_KL_AFFINE)
					ReconstructImageUseKL(m_pBitmapTarget,m_dwTargetHeight*m_dwTargetWidth,m_pEigenSpace,m_nNumberOfSelectedEigenVectors,m_pBitmapTemplate);
				dwErrorCounter = CheckResult(m_pBitmapTarget,m_pBitmapTemplate,m_dwTargetHeight*m_dwTargetWidth);

				goto EXIT_CalculateCurrentParameters;
			}

			//计算下一次,累加次数加1
			nCount_2 ++;
		}

		//修改Delt值再作
//		delt_now *= 0.80;  //注意此次修改Delt值累加
		//累计次数
//		nCount++;
	}

	for(i=0;i<6;i++)
		a[i] = a_back[i];

EXIT_CalculateCurrentParameters:

	pTemp = pInputImage[1];
	if(!pTemp) delete []pTemp;
	pInputImage[1] = NULL;
	pTemp = *(pTemplate[1]);
	if(!pTemp) delete []pTemp;
	pTemplate[1] = NULL;

	
	return dwErrorCounter;
}
//////////////////////////////////////////////////////////////////
//
//  抽层：
//////////////////////////////////////////////////////////////////
bool CTrack::PyramidSampleImage(DWORD dwHeight, DWORD dwWidth, double *pImageInput,double *&pImageOutput)
{
	DWORD dwHeight_now = dwHeight/2;
	DWORD dwWidth_now  = dwWidth/2;
	
	if(pImageOutput) delete []pImageOutput;
	pImageOutput = (double*) new double [dwHeight_now*dwWidth_now];
	if(!pImageOutput) 
	{
		AfxMessageBox("PyramidSampleImage:pImage 内存申请错误!");
		return false;
	}

	DWORD dwPosition = 0;
	DWORD dwPosition_l = dwWidth;
	DWORD temp0 = dwWidth%2;

	DWORD i,j;
	for(i=0;i<dwHeight_now;i++)
	for(j=0;j<dwWidth_now;j++)
		pImageOutput[dwPosition++] = pImageInput[(i*dwWidth+j)*2];

	return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////
//  零均值化
//////////////////////////////////////////////////////////////////////////////////////////////////////
bool CTrack::ZeroMean(double *pBitmap, DWORD dwSize,double& dMean)
{
	DWORD i;
	dMean = 0;
	for(i=0;i<dwSize;i++)
		dMean += pBitmap[i];
	
	dMean /= (double) dwSize;
	
	for(i=0;i<dwSize;i++)
		pBitmap[i]-= dMean;

    return true;
}
////////////////////////////////////////////////////////////////////////////////////////
//  检查当前结果 和模板图象的相似性程度
////////////////////////////////////////////////////////////////////////////////////////
DWORD CTrack::CheckResult(double *pTarget, double *pTemplate, DWORD dwSize)
{
	DWORD  dwResult=0;
	double dthreshold,temp;

	dthreshold = m_dVarianceTarget / 4;

	for(DWORD i=0;i<dwSize;i++)
	{
		temp = pTarget[i]-pTemplate[i];
		if(temp<0) temp = 0-temp;
		if(temp>dthreshold) 
			dwResult ++;
	}

	return dwResult;
}

////////////////////////////////////////////////////////////////////////////////
//
//  测试用将某块区域以图像格式写到硬盘里
//
////////////////////////////////////////////////////////////////////////////////
bool CTrack::Test(DWORD dwHeight, DWORD dwWidth, double *pInput)
{
	  static CString str="e:\\test000.bmp";
	  DWORD dwSize = dwHeight*dwWidth;
	  BYTE* pTemp=new BYTE[dwSize];
	  int i,j = dwSize;
	  for(i=0;i<j;i++)
		  pTemp[i] = (BYTE)(pInput[i] + m_dGradationOffset);
	  CZXDib dib;
	  dib.WriteBMPFileFromVector(str,pTemp,dwHeight,dwWidth,8);
	  dib.FindNextFileName(str,1);
	  return true;
}
////////////////////////////////////////////////////////////////////////////////////
//  重构目标图象
////////////////////////////////////////////////////////////////////////////////////
bool CTrack::ReconstructImageUseKL(double *pTarget, DWORD dwSize,double *pEigenSpace,int nNumberOfEigens,double *&pResult)
{
    double *C=NULL;
	if(!(C=(double*)new double [nNumberOfEigens]))
	{
		AfxMessageBox("分配内存失败！C \n PyramidCalculateImitateTargetImage()");
		return false;
	}
    double temp;		
	DWORD position = 0;
	DWORD i,j;
	
	//减去均值：
	if(pResult) delete []pResult;
	pResult =  new double[dwSize];
	for(i=0;i<dwSize;i++)
		pResult[i] = pTarget[i] - pEigenSpace[i];
	
    //计算拟合系数C值
	position = dwSize;
	for( j=0; j<nNumberOfEigens; j++){
		 C[j] = 0;
		 for( i=0; i<dwSize; i++)
			 C[j] += pEigenSpace[i+position] * pResult[i];
		 position += dwSize;
	}

	//计算拟合图像
	for( i=0; i<dwSize; i++)
	{
		position = dwSize;
		temp = 0;
		for(j=0; j<nNumberOfEigens; j++){
			temp += pEigenSpace[i+position] * C[j];
			position += dwSize;
		}
		pResult[i] = temp>255? 255:temp;
	}

	delete []C;
	
	for(i=0;i<dwSize;i++)
		pResult[i] += pEigenSpace[i];

	return true;
}

//////////////////////////////////////////////////////////////////
// 更新模板：
//////////////////////////////////////////////////////////////////
void CTrack::UpdateTemplate()
{
	//更换模板:采用上一贞加权方式
	DWORD dwSize = m_dwTargetHeight*m_dwTargetWidth;
	for(DWORD i=0; i<dwSize; i++)
		m_pBitmapTemplate[i] = m_pBitmapTemplate[i]*(1-m_dTemplateUpdatePower)+m_pBitmapTarget[i]*m_dTemplateUpdatePower;
	PyramidSampleImage(m_dwTargetHeight,m_dwTargetWidth,m_pBitmapTemplate,m_pBitmapTemplate_SecondLevel);

	return ;
}