📄 video.c
字号:
{
image_candidate=(int *)(0x00465400+(y+candidate_center_y)*360+(x+candidate_center_x));//候选模板像素在源图中的位置
l_color=(int)floor(*image_candidate/dividor_image);
temp=(int)floor((*(p+l_color)<<14)/(*(q+l_color)+1));//1e-4的Q14值为[1.6384]，下取整为1
flir_weight=(int)floor(sqrt(temp)*128);
}
*(w+(y+candidate_radiusy)*(2*candidate_radiusx+1)+x+candidate_radiusx)=flir_weight;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// calculate the new location of the target (位置用Q10进行计算)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void Determine_new_candidate_location(int candidate_center_x,int candidate_center_y, int *wei,int *candidate_center)
{
int Derivative_of_eponechnikov_profile(int ph);
int x,y,pixelx,pixely,pixel_length2,weighted;
int denominator=0, numarator_x=0, numarator_y=0;
int *p=candidate_center,*w=wei;
for (y=-candidate_radiusy; y<=candidate_radiusy; y++)
for (x=-candidate_radiusx; x<=candidate_radiusx; x++)
{
pixelx=(int)floor((x<<14)/candidate_radiusx);
pixely=(int)floor((y<<14)/candidate_radiusy);
pixel_length2=(pixelx*pixelx>>14)+(pixely*pixely>>14);
weighted=(*(w+(y+candidate_radiusy)*(2*candidate_radiusx+1)+x+candidate_radiusx))*
Derivative_of_eponechnikov_profile(pixel_length2)>>18;
denominator+=weighted;
numarator_x+=weighted*(x+candidate_center_x);
numarator_y+=weighted*(y+candidate_center_y);
}
if(denominator!=0)
{
if (numarator_x!=0 || numarator_y!=0)
{
*p =(int)floor((numarator_x<<10)/denominator);
*(p+1) =(int)floor((numarator_y<<10)/denominator);
}
else
Flag_zero=1;
}
else
Flag_zero=1;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// algorithm for the estimated location of the target in two frames
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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[] )
{
void Candidate_density_function_p(int candidate_center_x,int candidate_center_y,int *candidate_prob,int numb);
int Bhattacharyya_coefficient(int *mode_prob,int *candidate_prob,int numb);
void Calculate_weights(int candidate_center_x,int candidate_center_y,int *mode_prob,int *candidate_prob,int *wei);
void Determine_new_candidate_location(int candidate_center_x,int candidate_center_y,int *wei,int *candidate_center);
int i,j,iter_cnt=0,inner_iter_cnt;
int distance2;
int bhattacharyya_coefficient_1=0,bhattacharyya_coefficient_2=0;
int candidate_centerx,candidate_centery,temp_x,temp_y;
int candidate_center_temp_x,candidate_center_temp_y,target_center_temp_x,target_center_temp_y;
int *FLIR_weights,nrows,ncols; // **FLIR_weights,
int *pp=candidate_center,*qq=temp_center;
int bb[2],xx[4],jj;
//initialize the location of the target in the current frame
candidate_centerx=target_centerx;
candidate_centery=target_centery;
nrows=2*candidate_radiusy+1;
ncols=2*candidate_radiusx+1;
//待调试，看算法执行过程中bhattacharyya_coefficient_1是否需要改变
do
{ //calculate the distrubution for the target at candidate location
Candidate_density_function_p(candidate_centerx,candidate_centery,candidate_prob,numb);
//evaluate the bhattacharyya coefficient
if(Flag_pro_zero==0)
{
bhattacharyya_coefficient_1=Bhattacharyya_coefficient(mode_prob,candidate_prob,numb);
//do
//(
//开辟weight空间
//--------------------------------------------------------------------
//1.二维数组
//FLIR_weights=(int **)malloc(nrows*sizeof(int *)); //开行首指针列表
//for(i=0;i<nrows;i++)
//FLIR_weights[i]=(int *)malloc(ncols*sizeof(int)); //开某一横行的指针
//for(i=0;i<nrows;i++)
//for(j=0;j<ncols;j++)
//{
//FLIR_weights[i][j]=0;//动态数组的成员都可以用正常的数组下标FLIR_weights[i][j]进行初始化
//}
//2.开辟一维数组空间，用来代替二维数组
FLIR_weights=(int*)malloc(nrows*ncols*sizeof(int));
for(i=0;i<nrows;i++)
for(j=0;j<ncols;j++)
{
FLIR_weights[i*ncols+j]=0;
}
//derive the weigths
Calculate_weights(candidate_centerx,candidate_centery,mode_prob,candidate_prob,FLIR_weights);
//Calculate the new location of the target
Determine_new_candidate_location(candidate_centerx,candidate_centery,FLIR_weights,candidate_center);
//释放二维weight空间
//for(i=0;i<nrows;i++)
//{
//free(FLIR_weights[i]); //释放为每一行所分配的空间
//}
//free(FLIR_weights); //释放行指针
//释放一维weight空间
free(FLIR_weights);
//--------------------------------------------------------------------
if(Flag_zero == 0)
{
candidate_centerx = *pp>>10;
candidate_centery = *(pp+1)>>10;
//update the distrubution for the target at the new location
Candidate_density_function_p(candidate_centerx,candidate_centery,candidate_prob,numb);
if(Flag_pro_zero==0)
{
//evaluate the updated bhattacharyya coefficient
bhattacharyya_coefficient_2=Bhattacharyya_coefficient(mode_prob,candidate_prob,numb);
inner_iter_cnt=0;
target_center_temp_x=target_centerx<<14;
target_center_temp_y=target_centery<<14;
candidate_center_temp_x=candidate_centerx<<14;
candidate_center_temp_y=candidate_centery<<14;
while (bhattacharyya_coefficient_2-bhattacharyya_coefficient_1<0 && inner_iter_cnt<30)
{
candidate_center_temp_x=(int)floor(((candidate_center_temp_x+target_center_temp_x)<<14)/32768);//2的Q14值为32768
candidate_center_temp_y=(int)floor(((candidate_center_temp_y+target_center_temp_y)<<14)/32768);
temp_x=candidate_center_temp_x;
temp_y=candidate_center_temp_y;
candidate_centerx=temp_x>>14;
candidate_centery=temp_y>>14;
//update the distrubution for the target at the new candidate location
Candidate_density_function_p(candidate_centerx,candidate_centery,candidate_prob,numb);
//evaluate the updated bhattacharyya coefficient
bhattacharyya_coefficient_2 = Bhattacharyya_coefficient(mode_prob,candidate_prob,numb);
inner_iter_cnt++;
}
//Calculate the distance between two vectors调试时注意需不需要用Q14表示
distance2=(((candidate_center_temp_x-target_center_temp_x)*(candidate_center_temp_x-target_center_temp_x))>>14)
+(((candidate_center_temp_y-target_center_temp_y)*(candidate_center_temp_y-target_center_temp_y))>>14);
//distance=(int)floor((float)sqrt(distance2))<<7;
target_centerx=candidate_centerx;
target_centery=candidate_centery;
//bhattacharyya_coefficient_1=bhattacharyya_coefficient_2; //有关do-while循环的位置
iter_cnt++;
}
else
break;
}
else
break;
}
else
break;
}
while ( distance2>=16384 && iter_cnt<10);//(distance>1)//distance>0.5,则distance2>0.25 --Q14值为4096
y[0]=target_centerx;
y[1]=target_centery;
//------计算b=y-h*x------为状态修正(滤波)作准备
for (i=0; i<=m-1; i++)//求bb[m][1]=y[m][1]-h[m][n]*x[n][1]
{ jj=i; bb[jj]=y[jj];
for (j=0; j<=n-1; j++)
bb[jj]=bb[jj]-h[i*n+j]*x_pre[j];
}
//------状态修正(滤波)------x=x+g*b
for (i=0; i<=n-1; i++)//求xx[n][1]=x[n][1]+g[n][m]*b[m][1]
{ jj=i;xx[jj]=x_pre[jj];
for (j=0; j<=m-1; j++)//g为Q14值
xx[jj]=xx[jj]+(g[i*m+j]*bb[j]>>14);
}
*qq=xx[0];
*(qq+1)=xx[1];
return bhattacharyya_coefficient_2;
}
//全主元高斯-约当消去法求矩阵逆
//若用整型，数据精度不够，因而仍需用Q14来表示
int brinv(int *a,int n)
{
int *is,*js,i,j,k,l,u,v;
int d,p;
is=(int*)malloc(n*n*sizeof(int)); //申请空间
js=(int*)malloc(n*n*sizeof(int));
for(k=0;k<=n-1;k++)
{
d=0;
for(i=k;i<=n-1;i++) //搜索原矩阵右下方n-k+1维子方阵中的主元
for(j=k;j<=n-1;j++)
{
l=i*n+j;
p=abs(a[l]); //绝对值
if(p>d)
{
d=p;
is[k]=i;
js[k]=j;
}
}
if(d+16384==16384) //[if(d+1==1)]主元为零,不可逆,返回零
{
free(is);
free(js);
return(0);
}
if(is[k]!=k) //将主元搬移到对角线上
for(j=0;j<=n-1;j++) //交换两行
{
u=k*n+j;
v=is[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
}
if(js[k]!=k)
for(i=0;i<=n-1;i++) //交换两列
{
u=i*n+k;
v=i*n+js[k];
p=a[u];
a[u]=a[v];
a[v]=p;
}
l=k*n+k; //主元
a[l]=(int)floor((16384<<14)/a[l]); //[a[l]=1/a[l]]对角主元取倒数
for(j=0;j<=n-1;j++)
if(j!=k) //第k行中的非对角元的处理
{
u=k*n+j;
a[u]=a[u]*a[l]>>14; //(a[u]=a[u]*a[l])该行的非对角元除以其对角元素,akj=akj/akk
}
for(i=0;i<=n-1;i++) //目的是将第k列中的非对角元素变为零
if(i!=k)
for(j=0;j<=n-1;j++) //这里j可以从k+1开始
if(j!=k) //除去对角主元所在的行和列
{
u=i*n+j;
a[u]=a[u]-(a[i*n+k]*a[k*n+j]>>14); //aij=aij-aik*akj
}
for(i=0;i<=n-1;i++) //第k列中的非对角元的处理,这就是未进行行列恢复的逆矩阵的元素
if(i!=k)
{
u=i*n+k;
a[u]=-(a[u]*a[l]>>14);//该列的非对角元除以其对角元素,再取负号
}
}
//行列恢复,R*inv(R)*inv(A)*inv(L)*L=R*inv(L*A*R)*L=inv(A)
for(k=n-1;k>=0;k--) //将前面主元交换的位置,放在原位的对称位置上
{
if(js[k]!=k)
for(j=0;j<=n-1;j++) //交换两行,使用前面列交换的标号
{
u=k*n+j;
v=js[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
}
if(is[k]!=k)
for(i=0;i<=n-1;i++) //交换两列,使用前面行交换的标号
{
u=i*n+k;
v=i*n+is[k];
p=a[u];
a[u]=a[v];
a[v]=p;
}
}
free(is);
free(js);
return(1);
}
//////////////////////////////pre//////////////////////////////////////////////
Mutuality( char* Img1, char* Img2, int nX, int Width, int W, int *xy, int *yy )
{
int sumxy, sumyy, i, j, Height, d1, d2;
char *p1, *p2;
Height = nX/Width;
sumxy = sumyy = 0;
for( i=0; i<Height; i++ )
{
p1 = Img1;
p2 = Img2;
for( j=0; j<Width; j++ )
{
d1 = (*p1++) & 0xff;
d2 = (*p2++) & 0xff;
sumxy += d1 * d2;
sumyy += d2 * d2;
}
Img1 += Width;
Img2 += W;
}
*xy = sumxy;
*yy = sumyy;
}
int sqarex( int *pImg, int ImgLen )
{
unsigned char *p = (unsigned char*)pImg;
int i, d, dd=0;
for( i=0; i<ImgLen; i++ )
{
d = *p++;
dd += d*d;
}
return dd;
}
int MatchObject( int ImageNumber )
{
unsigned int Max, xx, yy, xy;
unsigned int Img1, Img2, Img3, i, j;
int xlt, ylt, xrb, yrb, Xlt, Ylt, Xrb, Yrb, xw, yw;
// unsigned int Height, Width, nX, W, H;
TIMER t1;
StartTimer( &t1 );
Img1 = (int)(ImageBuf + ImageNumber * ImageLen );
MemCopy( (int*)0x00465400, (int*)Img1, 103680 );
xw = (OBJ_POS>>16) - (ObjPos1>>16);
yw = (OBJ_POS&0x1ff) - (ObjPos1&0x1ff);
xlt = OBJ_LEFTTOP >> 16;
ylt = OBJ_LEFTTOP & 0xffff;
xrb = OBJ_RIGHTBOT >> 16;
yrb = OBJ_RIGHTBOT & 0xffff;
xlt += xw;
xrb += xw;
ylt += yw;
yrb += yw;
hhh = yrb - ylt;
www = xrb - xlt;
bytesX = hhh * www;
Xlt = ((xlt - 10) < 10)? 10: (xlt - 10);
Ylt = ((ylt - 10) < 2)? 2: (ylt - 10);
Xrb = ((xrb + 10) > 356)? 356: (xrb + 10);
Yrb = ((yrb + 10) > 286)? 286: (yrb + 10);
wideY = Xrb - Xlt;
heightY = Yrb - Ylt; // 搜索区域的宽度、高度 扩大10
if( wideY < www || heightY < hhh )
{
OBJ_POS = 0xffffffff; //目标跑出边界，放弃跟踪
return;
}
i = 4 - wideY % 4;
if( i != 4 )
{
if( Xlt > 8 )
Xlt -= i;
else
Xrb += i;
}
wideY = Xrb - Xlt;
AREA_W_H = (wideY<<16) | heightY;
Xrb = Xrb - www;
Yrb = Yrb - hhh; //Xlt, Ylt, Xrb, Yrb标识了模板左上角滑动的区域
//* 搬移模板、搜索区域的相关数据到内部RAM制定的地址处:
MemCopy( (int*)0x80005000, (int*)0x004CA800, www*hhh ); //拷贝模板
Img1 = 0x00465400 + Ylt*360+Xlt;
Img2 = 0x80007000;
for( i=0; i<heightY; i++ )
{
ByteMemCopy( (int*)Img2, (int*)Img1, wideY );
Img1 += 360;
Img2 += wideY;
}
//* 开始计算各点的相关匹配值
// Ylt = 100;
// Yrb = 140;
// Xlt = 100;
// Xrb = 140;
Max = 0;
Img1 = 0x80005000; // 模板
Img3 = 0x80007000; // 搜索区域
startX = Xlt;
endX = Xrb;
startY = Ylt;
endY = Yrb;
for( i = startY; i<endY; i+=2 )
{
Img2 = Img3;
for( j=startX; j<endX; j+=2)
{
// DelayUs(80);
//;==============================================================
//;子程序名： MutualityH
//;功 能： 两块区域做相关
//;输入参数： 模板首地址啊A4,区域二首地址B4,模板容量A6，模板宽度B6,
//; 区域宽度A8,
//;输出参数： 返回值1--B8, 返回值2--A10
//;==============================================================
MutualityH( (int*)Img1, (int*)Img2, bytesX, www, wideY, &xy, &yy );
xy = (int)( (float)xy/sqrt( yy ) );
if( xy > Max )
{
xxx = j;
yyy = i;
Max = xy;
}
Img2 += 2;
}
Img3 += wideY*2;
}
xx = SquareX( (int*)0x80005000, bytesX );
xx = sqrt( xx )*0.95;
if( Max > xx )
{
xlt = xxx;
ylt = yyy;
xrb = xlt + www;
yrb = ylt + hhh;
xw = xlt + www/2;
yw = ylt + hhh/2;
if(xlt>360 || xrb>360 || xlt>xrb )
TestTime = 10;
if(ylt>288 || yrb>288 || ylt>yrb )
TestTime = 11;
if(xw>360 || yw>288)
TestTime = 12;
OBJ_POS = (xw << 16) | (yw & 0xffff);
OBJ_LEFTTOP = (xlt << 16) | (ylt & 0xffff);
OBJ_RIGHTBOT =(xrb << 16) | (yrb & 0xffff);
ObjPos1 = ObjPos2;
ObjPos2 = OBJ_POS;
}
else
OBJ_POS = 0xffffffff;
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
DSP_LOCK = 0;
MemCopy( (int*)0x00497E00, (int*)0x00465400, 103680 ); //写入块0
}
TestTime = ReadTimer( t1 );
return 0;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -