📄 video.c
字号:
main()
{
void TrackAlgorithm( int ImageNumber,int model_probabilities[],int hh[],int m,int n,int f[],int h[],int r[],int q[],int x[],int p[],int g[],int y[],int x_pre[],int p_pre[] );
int model_probabilities[number_bin];//定义目标模板数组
int hh[1]={0};
//KF初始化－－相邻帧间隔时间短，只有40ms，将目标视为匀速运动
int f[16]={1,0,40,0,0,1,0,40,0,0,1,0,0,0,0,1},h[8]={1,0,0,0,0,1,0,0};//状态转移矩阵、观测矩阵初始化
int r[4]={16384,0,0,16384},q[16]={8192,0,0,0,0,8192,0,0,0,0,8192,0,0,0,0,8192};//噪声协方差初始化r[4]={1,0,0,1},q[16]={0.5,0,0,0,0,0.5,0,0,0,0,0.5,0,0,0,0,0.5}
int x[4]={0,0,0,0},p[16]={131072,0,0,0,0,131072,0,0,0,0,131072,0,0,0,0,131072},g[4]={0,0,0,0};//状态矩阵、误差协方差矩阵及增益矩阵初始化p[16]={8,0,0,0,0,8,0,0,0,0,8,0,0,0,0,8},
int x_pre[4],p_pre[16],y[2];//定义状态暂存数组、误差协方差暂存数组，用来存放一步预测的目标状态及误差协方差；定义观测向量
int m=2,n=4;//定义维数
InitDSPs();
LightenLed( LED_GREEN ); //DelayMs(50);
StartVideoAcquistion();
for(;;)
{
if( ReadyNumber == 0 )
continue;
//DarkenLed( 2 );
PCR1 = PCR1 ^ LED_RED_OFF;
TrackAlgorithm( FieldReady,model_probabilities,hh,m,n,f,h,r,q,x,p,g,y,x_pre,p_pre );
//LightenLed( 2 );
--ReadyNumber;
}
}
//用原投影法确定目标区域，调试时应注意
int DetectObject( int *pX, int *pY , uint *LeftTop, uint *RightBottom) //返回重心坐标
{
int Xw, Yw, Xlt, Ylt, Xrb, Yrb, i, j, *p1, *p2, Tmp1, Tmp2, Tmp3; //Xw,Yw为重心坐标
int th, Sum, Tx, Ty;
Sum = Tmp1 = Tmp2 = Tmp3 = 0;
p1 = pX;
for( i=0; i<360; i++ )
Sum += *p1++;
if( Sum < 60*255 )
return 0xFFFFFFFF;
p1 = pX;
for( i=1; i<200; i++ )
Tmp1 += i*(*p1++);
for( i=200; i<300; i++ )
Tmp2 += i*(*p1++);
for( i=300; i<=360; i++ )
Tmp3 += i*(*p1++);
Tmp1 /= Sum;
Tmp2 /= Sum;
Tmp3 /= Sum;
Xw = Tmp1 + Tmp2 + Tmp3;
if( Xw>358 || Xw<2 )
return 0xffffffff;
if( Sum < 60*255 )
th = Sum*0.9;
else if( Sum < 100*255 )
th = Sum*0.8;
else if( Sum < 200*255 )
th = Sum*0.75;
else if( Sum < 300*255 )
th = Sum*0.72;
else if( Sum < 400*255 )
th = Sum*0.70;
else if( Sum < 500*255 )
th = Sum*0.68;
else if( Sum < 600*255 )
th = Sum*0.66;
else if( Sum < 700*255 )
th = Sum*0.64;
else if( Sum < 800*255 )
th = Sum*0.62;
else if( Sum < 900*255 )
th = Sum*0.60;
else if( Sum < 1000*255 )
th = Sum*0.58;
else
th = Sum*0.56;
Tmp1 = 0;
p1 = pX+Xw-1;
p2 = p1+1;
i=Xw;
j=Xw+1;
for(;;)
{
if(i<4 || j>356 )
break;
Tmp1 += *p1--;
Tmp1 += *p1--;
Tmp1 += *p2++;
Tmp1 += *p2++;
if( Tmp1 >= th )
break;
i -= 2;
j += 2;
}
Xlt = i; //物体横坐标范围
Xrb = j;
Tx = Xrb - Xlt;
if( Tx > 80 || Tx < 4 )
return 0xffffffff;
//确定目标模板的长
model_radiusx=Tx/2;
Xlt=Xw - model_radiusx;
Xrb=Xw + model_radiusx;
//下面代码段把目标框宽度调整到4的整数倍
//Tmp2 = Tmp1 % 4;
//if( Tmp2 == 1 )
//Xlt++;
//else if( Tmp2 == 2 )
//{
//Xlt++;
//Xrb--;
//}
//else if( Tmp2 == 3 )
//{
//if( Xw-Xlt < Xrb-Xw )
//Xlt--;
//else
//Xrb++;
//}
// 计算纵向投影的相关参数
Tmp1 = Tmp2 = Tmp3 = 0;
p1 = pY;
for( i=1; i<150; i++ )
Tmp1 += i*(*p1++);
for( i=150; i<230; i++ )
Tmp2 += i*(*p1++);
for( i=230; i<=288; i++ )
Tmp3 += i*(*p1++);
Tmp1 /= Sum;
Tmp2 /= Sum;
Tmp3 /= Sum;
Yw = Tmp1 + Tmp2 + Tmp3;
if( Yw>286 || Yw<2 )
return 0xffffffff;
Tmp1 = 0;
p1 = pY+Yw-1;
p2 = p1+1;
i=Yw;
j=Yw+1;
for(;;)
{
if( i<2 || j>286 )
break;
Tmp1 += *p1--;
Tmp1 += *p2++;
if( Tmp1 >= th )
break;
i--;
j++;
}
Ylt = i; //物体纵坐标范围
Yrb = j;
Ty = Yrb - Ylt;
if( Ty > 80 || Ty < 2 )
return 0xffffffff;
//确定目标模板的宽
model_radiusy=Ty/2;
Ylt=Yw - model_radiusy;
Yrb=Yw + model_radiusy;
//确定目标模板半径
//if(Tx>=Ty)
//model_radius=Ty/2;
//else
//model_radius=Tx/2;
//确定目标区域，model_radius的值可能要改
//Xlt=Xw - model_radius;
//Xrb=Xw + model_radius;
//Ylt=Yw - model_radius;
//Yrb=Yw + model_radius;
Tmp3 = (Xlt << 16) + (Ylt & 0x01ff);
*LeftTop = Tmp3;
Tmp3 = (Xrb << 16) + (Yrb & 0x01ff);
*RightBottom = Tmp3;
return ((Xw << 16) + Yw);
}
int MarkOutObject( uint LeftUpper, uint RightBottom, uint Center, uint ImgToBeMarked )
{
unsigned int x1, y1, x2, y2;
int x, y, i;
unsigned char *p, *p1;
x1 = LeftUpper >> 16;
y1 = LeftUpper & 0xffff;
x2 = RightBottom >> 16;
y2 = RightBottom & 0xffff;
x = Center >> 16;
y = Center & 0xffff;
if( x<4 || x > 356 || y<4 || y>284 )
return -1;
p = (unsigned char*)( ImgToBeMarked + y*360 + x);
for( i=0; i<8; i++ )
{
if( x-i>0 )
*(p-i) = ~(*(p-i));
if( x+i<359 )
*(p+i) = ~(*(p+i));
if( y-i>0 )
*(p-i*360) = ~(*(p-i*360));
if( y+i<287 )
*(p+i*360) = ~(*(p+i*360));
}
p = (unsigned char*)( ImgToBeMarked + y1*360 + x1);
p1 = (unsigned char*)( ImgToBeMarked + y2*360 + x1);
for( i=x1; i<=x2; i++ )
{
*p = ~(*(p));
*p1 = ~(*(p1));
p++;
p1++;
}
p = (unsigned char*)( ImgToBeMarked + y1*360 + x1);
p1 = (unsigned char*)( ImgToBeMarked + y1*360 + x2);
for( i=y1; i<=y2; i++ )
{
*p = ~(*(p));
*p1 = ~(*(p1));
p += 360;
p1 += 360;
}
return 0;
}
//跟踪程序，每40ms触发一次
void TrackAlgorithm( int ImageNumber,int model_probabilities[],int hh[],int m,int n, int f[],int h[],int r[],int q[],int x[],int p[],int g[],int y[],int x_pre[],int p_pre[] )
{
void Diff( int ImageNumber,int *model_prob,int numb );//函数声明
int MemCopy( int *DstAddr, int *SrcAddr, int Len );
int brinv(int *a,int n);
int Track_target_in_consecutive_frames( int target_centerx,int target_centery,int *mode_prob,int *candidate_prob,int numb,int *candidate_center,int *temp_center,int m,int n,int x_pre[],int y[],int g[],int h[] );
int MarkOutObject( uint LeftUpper, uint RightBottom, uint Center, uint ImgToBeMarked );
int target_center_of_x,target_center_of_y,correct_target_x,correct_target_y,fit_radiusx,fit_radiusy;
int candidate_probabilities[number_bin];
int candidate_center_coordinate[2]={0,0},temp_center_coordinate[2]={0,0};
int img,k,bh=0,max_bh=1638; //0.1的Q14表示为下取整[1638.4]=1638
int h_scale,xlt,ylt,xrb,yrb,xw,yw; //tmp;
int i,j,kk,jj,js;//l,
int *e,*a,*b;
e=(int*)malloc(m*m*sizeof(int));
//l=m;
//if (l<n) l=n;
//a=(double*)malloc(l*l*sizeof(double));
//b=(double*)malloc(l*l*sizeof(double));
a=(int*)malloc(n*n*sizeof(int));
b=(int*)malloc(n*n*sizeof(int));
if( OBJ_POS == 0xffffffff )
{
if( DetectManner == 1 )
{ //刚从相关状态切换过来，应关云台，做一定的初始化工作
DetectManner = 0;
FLAG_CRADLE = 0;
num_filter=0;
Flag_pro_zero=0;
Flag_zero = 0;
ObjPos1 = ObjPos2 = 0xffffffff;
p[0]=p[5]=p[10]=p[15]=131072;//误差协方差初始值重新复赋值
p[1]=p[2]=p[3]=p[4]=p[6]=p[7]=p[8]=p[9]=p[11]=p[12]=p[13]=p[14]=0;//p[16]={8,0,0,0,0,8,0,0,0,0,8,0,0,0,0,8};
}
Diff(ImageNumber,model_probabilities,number_bin);
hh[0]=(int)floor((model_radiusx+model_radiusy)/10); //h-scale根据差分后模板半径选择0.2*[(model_radiusx+model_radiusy)/2]
return;
}
img = (int)(ImageBuf + ImageNumber * ImageLen );
MemCopy( (int*)0x00465400, (int*)img, 103680 );
target_center_of_x = OBJ_POS>>16;
target_center_of_y = OBJ_POS & 0xffff;
/*tmp = FLAG_CRADLE;
if( xw > 180+40 )
tmp |= 0x20; // 云台右转
if( xw < (180-40) )
tmp |= 0x10; // 云台左转
if( (xw>180-5) && (xw<180+5) )
tmp &= 0x03; // 云台水平方向停转
if( yw > 144+30 )
tmp |= 0x02; // 云台下俯
if( yw < 144-30 )
tmp |= 0x01; // 云台上仰
if( (yw>144-5) && (yw<144+5) )
tmp &= 0x30; // 云台垂直方向停转
FLAG_CRADLE = tmp;*/
//if( tmp == 0 )
//{
//if( DetectManner == 1 )
//{ //刚从相关状态切换过来，应关云台，做一定的初始化工作
//DetectManner = 0;
//FLAG_CRADLE = 0;
//num_filter=0;
//Flag_pro_zero=0;
//Flag_zero = 0;
//ObjPos1 = ObjPos2 = 0xffffffff;
//p[16]={8,0,0,0,0,8,0,0,0,0,8,0,0,0,0,8};
//}
//Diff( ImageNumber );
//}
//else
//{
if( DetectManner == 0 )
DetectManner = 1;
//状态初始化，准备KF
x[0]=target_center_of_x;
x[1]=target_center_of_y;
x[2]=x[3]=0;
//**************************************
// kalman滤波迭代
//**************************************
//------计算：p=f*p*f'+q------计算预估计误差协方差矩阵
for (i=0; i<=n-1; i++)//求a[n][n]=p[n][n]*f[n][n]
for (j=0; j<=n-1; j++)
{ jj=i*n+j; a[jj]=0;
for (kk=0; kk<=n-1; kk++)//p为Q14值
a[jj]=a[jj]+p[i*n+kk]*f[j*n+kk];//此a为Q14值
}
for (i=0; i<=n-1; i++)//求p[n][n]=q[n][n]+f[n][n]*a[n][n]
for (j=0; j<=n-1; j++)
{ jj=i*n+j; p_pre[jj]=q[jj]; //q为Q14值
for (kk=0; kk<=n-1; kk++)
p_pre[jj]=p_pre[jj]+f[i*n+kk]*a[kk*n+j];//p_pre为Q14值
}
//------计算x=f*x(-1)------计算预估计状态矩阵
for (i=0; i<=n-1; i++)//求x[n][1]=f[n][n]*x[n][1]
{ jj=i; x_pre[jj]=0;
for (j=0; j<=n-1; j++)
x_pre[jj]=x_pre[jj]+f[i*n+j]*x[j];
}
//------计算e=h*p*h'+r-----为计算增益矩阵中的需求逆的矩阵作准备
for (i=0; i<=n-1; i++)//求a[n][m]=p[n][n]*h[n][m]
for (j=0; j<=m-1; j++)
{ jj=i*m+j; a[jj]=0;
for (kk=0; kk<=n-1; kk++)
a[jj]=a[jj]+p_pre[i*n+kk]*h[j*n+kk];//此a为Q14值
}
for (i=0; i<=m-1; i++)//求e[m][m]=r[m][m]+h[m][n]*a[n][m]
for (j=0; j<=m-1; j++)
{ jj=i*m+j;e[jj]=r[jj];//r为Q14值
for (kk=0; kk<=n-1; kk++)
e[jj]=e[jj]+h[i*n+kk]*a[kk*m+j];//e为Q14值
}
//------求e的逆矩阵------e=e^(-1)
js=brinv(e,m);
if (js==0) //如果e为奇异矩阵，不可逆
{
free(e);
free(a);
free(b);
OBJ_POS=0xffffffff;
}
else
{
//------计算增益矩阵------g=p*h'*e
for (i=0; i<=n-1; i++)//求g[n][m]=a[n][m]*e[m][m]
for (j=0; j<=m-1; j++)
{ jj=i*m+j; g[jj]=0;
for (kk=0; kk<=m-1; kk++)
g[jj]=g[jj]+(a[i*m+kk]*e[j*m+kk]>>14);//g为Q14值
}
//------计算a=I-g*h------为误差协方差矩阵修正(滤波)作准备
for (i=0; i<=n-1; i++)//求a[n][n]=I[n][n]-g[n][m]*h[m][n]
for (j=0; j<=n-1; j++)
{ jj=i*n+j; a[jj]=0;
for (kk=0; kk<=m-1; kk++)
a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j];//此a为Q14值
if (i==j)
a[jj]=16384+a[jj];//a[jj]=1+a[jj]
}
//------误差协方差修正(滤波)------b=a*p
for (i=0; i<=n-1; i++)//求b[n][n]=a[n][n]*p[n][n]
for (j=0; j<=n-1; j++)
{ jj=i*n+j; b[jj]=0;
for (kk=0; kk<=n-1; kk++)
b[jj]=b[jj]+(a[i*n+kk]*p_pre[kk*n+j]>>14);
}
for (i=0; i<=n-1; i++)//误差协方差的更新
for (j=0; j<=n-1; j++)
p[i*n+j]=b[i*n+j]; //p为Q14值
free(e);
free(a);
free(b);
// 由于kalman初始参数的不准确性，对初始化的头几帧图像要进行特殊处理－－
//kalman滤波器的估计位置和上一帧图像中目标位置按一定比例进行加权修正
if (num_filter<5)
{
num_filter=num_filter+1;//统计初始滤波次数
target_center_of_x=(num_filter*x_pre[0]+(5-num_filter)*x[0])/5;
target_center_of_y=(num_filter*x_pre[1]+(5-num_filter)*x[1])/5;
}
else
{
target_center_of_x=x_pre[0];
target_center_of_y=x_pre[1];
}
//////////////////////////////////////
// mean-shift跟踪程序
//////////////////////////////////////
h_scale=hh[0];
for(k=-h_scale;k<=h_scale;k+=h_scale)
{
candidate_radiusx=model_radiusx+k;
candidate_radiusy=model_radiusy+k;
bh=Track_target_in_consecutive_frames(target_center_of_x,target_center_of_y,model_probabilities,candidate_probabilities,number_bin,candidate_center_coordinate,temp_center_coordinate,m,n,x_pre,y,g,h);
if(Flag_zero==0 && Flag_pro_zero==0)
{
if(bh>max_bh)
{
max_bh=bh;
correct_target_x=temp_center_coordinate[0];
correct_target_y=temp_center_coordinate[1];
fit_radiusx=candidate_radiusx;
fit_radiusy=candidate_radiusy;
}
}
else
{
OBJ_POS=0xffffffff;
break;
}
}
if(Flag_zero==0 && Flag_pro_zero==0)
{
xw=correct_target_x;
yw=correct_target_y;
xlt=xw-fit_radiusx;
xrb=xw+fit_radiusx;
ylt=yw-fit_radiusy;
yrb=yw+fit_radiusy;
OBJ_POS=(xw << 16)|(yw & 0xffff);
OBJ_LEFTTOP=(xlt << 16)+(ylt & 0x01ff);
OBJ_RIGHTBOT=(xrb << 16)+(yrb & 0x01ff);
ObjPos1 = ObjPos2;
ObjPos2 = OBJ_POS;
model_radiusx=(int)floor((fit_radiusx+9*model_radiusx)/10);
model_radiusy=(int)floor((fit_radiusx+9*model_radiusy)/10);//自适应更新带宽，系数为0.1
}
}
if( FLAG_MARK == 1 )
MarkOutObject( OBJ_LEFTTOP, OBJ_RIGHTBOT, (uint)OBJ_POS, 0x00465400 );
if( PCI_LOCK == 0 )//下面搬图像到合适的区域供上位机显示，考虑了同步因素
{ //如果PCI正在读取块0， 则填入块1
DSP_LOCK = 1;
MemCopy( (int*)0x004B1300, (int*)0x00465400, 103680 ); //写入块1
}
else
{ //如果PCI正在读取块1，或者PCI空闲，则填入块0
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -