📄 video.c
字号:
DSP_LOCK = 0;
MemCopy( (int*)0x00497E00, (int*)0x00465400, 103680 ); //写入块0
}
//}
}
int GraCenter(int *px,int *py,unsigned int width,unsigned int height)
{
int xw, yw, i, *p1, temp,sum;
sum = temp=0;
p1 = px;
for( i=1; i<=width; i++ )
sum += *p1++;
p1 = px;
for( i=1; i<=width; i++ )
temp += i*(*p1++);
xw = temp/sum;
p1=py;
temp=0;
for( i=1; i<=height; i++ )
temp += i*(*p1++);
yw = temp/sum;
return ((xw<<16+yw));
}
int Invar(unsigned int width,unsigned int height,unsigned int *t1,unsigned int *t2,unsigned int *t3,unsigned int *t4,unsigned int *t5,unsigned int *t6,unsigned int *t7)
{
int center,xg,yg,v00,v002,v003,u11,u02,u03,u20,u30,u21,u12,i,j,tempx2,tempy2;
unsigned char *img;
unsigned int temp1,temp2,temp3,temp4,temp5,temp6,temp7;
MemCopy( (int*)0x80005000, (int*)0x004CA800, width*height );
PixStat(0x80005000,0,height,(int*)(HorizontalEnergy), (int*)(VerticalEnergy),(int)(width/4));
center=GraCenter((int*)(HorizontalEnergy), (int*)(VerticalEnergy),width,height);
xg=center>>16;
yg=center&0xffff;
img=(unsigned char *)(0x80005000);
v00=0;
for(i=0;i<width*height;i++)
{
v00+=*img++;
}
v002=v00*v00;
v003=v002*IntSqrt(v00);
img=(unsigned char *)(0x80005000);
u11=u02=u03=u20=u30=u21=u12=0;
for(i=1;i<=height;i++)
{
for(j=1;j<=width;j++)
{
u11+=(i-xg)*(j-yg)*(*img);
tempx2=(i-xg)*(i-xg);
tempy2=(j-yg)*(j-yg);
u02+=tempy2*(*img);
u03+=tempy2*(j-yg)*(*img);
u20+=tempx2*(*img);
u30+=tempx2*(i-xg)*(*img);
u21+=tempx2*(j-yg)*(*img);
u12+=(i-xg)*tempy2*(*img);
img++;
}
}
u11=u11/v002;
u02=u02/v002;
u03=u03/v003;
u20=u20/v002;
u30=u30/v003;
u21=u21/v003;
u12=u12/v003;
temp1=4*u11;
temp2=u30-3*u12;
temp3=3*u21-u03;
temp4=u30+u12;
temp5=temp4*temp4;
temp6=u21+u03;
temp7=temp6*temp6;
*t1=(unsigned int)(abs(u20+u02));
*t2=(unsigned int)(abs((u30-u02)*(u30-u02)+temp1*u11));
*t3=(unsigned int)(abs(temp2*temp2+temp3*temp3));
*t4=(unsigned int)(abs(temp5+temp7));
*t5=(unsigned int)(abs(temp2*temp4*(temp5-3*temp7)+temp3*temp6*(3*temp5-temp7)));
*t6=(unsigned int)(abs((u20-u02)*(temp5-temp7)+temp1*temp4*temp6));
*t7=(unsigned int)(abs(temp3*temp4*(temp5-3*temp7)+temp2*temp6*(3*temp5-temp7)));
if(*t1==0xffffffff||*t2==0xffffffff||*t3==0xffffffff||*t4==0xffffffff||*t5==0xffffffff||*t6==0xffffffff||*t7==0xffffffff)
return 1;
else
return 0;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Eponechnikov profile's implementation for 2D (x>=0)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
int Eponechnikov_profile(int ph) //ph为Q14定点数
{
int volume_of_unit_d_dimensional_sphere=M_PI; //since d=2 //ph1,
//int i,Q=7;
//ph1=(int)floor((float)sqrt(ph))<<7;
if (ph<16384) return (int)floor(((32768*(16384-ph)>>14)<<14)/(volume_of_unit_d_dimensional_sphere));//1<<14=16384,调试时注意优先级
else return 0;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Uniform profile's implementation for 2D (x>=0)
// derivative of eponechnikov profile (don't put minus)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
int Derivative_of_eponechnikov_profile(int ph)
{
int volume_of_unit_d_dimensional_sphere=M_PI; //since d=2 //ph1,
//int i,Q=7;
//float temp;
//temp=(float)sqrt(ph);
//for(i=0;i<Q;i++)
//temp=temp*2;
//ph1=(int)floor(temp);
if (ph<16384) return (int)floor((32768<<14)/(volume_of_unit_d_dimensional_sphere));//2的Q14定点数为32768
else return 0;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate constant c used in model density function
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
int Constant_C()
{
int Eponechnikov_profile(int ph);
int x,y,pixelx,pixely,pixel_length2,sum=0;
for (y=-model_radiusy;y<=model_radiusy;y++)
for (x=-model_radiusx;x<=model_radiusx;x++)
{
pixelx=(int)floor((x<<14)/model_radiusx);
pixely=(int)floor((y<<14)/model_radiusy);
pixel_length2=(pixelx*pixelx>>14)+(pixely*pixely>>14);
sum+=Eponechnikov_profile(pixel_length2);
}
return (int)floor((16384<<14)/sum);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate constant ch used in candidate density function
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
int Constant_Ch()
{
int Eponechnikov_profile(int ph);
int x,y,pixelx,pixely,pixel_length2,sum=0;
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);
sum+=Eponechnikov_profile(pixel_length2);
}
return (int)floor((16384<<14)/sum);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate model's density estimation q
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void Model_density_function_q(int target_center_x,int target_center_y,int *mode_prob,int numb)
{
int Eponechnikov_profile(int ph);
int Constant_C();
int x,y,pixelx,pixely,epanechnikov_number,pixellength2,l_color;
int *p,*mode_prob_end,*image_model;
int constantC=Constant_C(); //normalization constant
mode_prob_end=mode_prob+numb;
for(p=mode_prob;p<mode_prob_end;p++)
*p=0;//模板的概率密度初值
p=mode_prob;
for (y=-model_radiusy; y<=model_radiusy; y++)
for (x=-model_radiusx; x<=model_radiusx; x++)
{
if (y+target_center_y<=360 && y+target_center_y>=1 &&
x+target_center_x<=288 && x+target_center_x>=1)
{
pixelx=(int)floor((x<<14)/model_radiusx);
pixely=(int)floor((y<<14)/model_radiusy);
pixellength2=(pixelx*pixelx>>14)+(pixely*pixely>>14);
epanechnikov_number=Eponechnikov_profile(pixellength2);
image_model=(int*)(0x00465400+(y+target_center_y)*360+(x+target_center_x));//模板像素在源图中的位置
l_color=(int)floor(*image_model/dividor_image);
*(p+l_color)+= epanechnikov_number*constantC>>14;
}
else
OBJ_POS = 0xffffffff;
}
}
//图像差分，提取目标
void Diff( int ImageNumber,int *model_prob,int numb )
{
void Model_density_function_q(int target_center_x,int target_center_y,int *mode_prob,int numb);
int DetectObject( int *pX, int *pY , uint *LeftTop, uint *RightBottom);
int MarkOutObject( uint LeftUpper, uint RightBottom, uint Center, uint ImgToBeMarked );
int Img1, Img2, Img3, SubSrc, SubDst, i, j, Res;//time,
unsigned int Center, LeftTop, RightBottom, Width, Height,tt1,t2,t3,t4,t5,t6,t7;
int target_center_x,target_center_y;
//TIMER t1;
Img1 = (int)(ImageBuf + ImageNumber * ImageLen);
Img2 = (int)(ImageBuf + ((ImageNumber-1)&0x03) * ImageLen);
Img3 = (int)(ImageBuf + ImageLen*5);
//StartTimer( &t1 );
MemSet( (int*)HorizontalEnergy, 0, 360*4 );
MemSet( (int*)VerticalEnergy, 0, 288*4 );
MemSet( (int*)0x8000b540, 0, 720 ); //差分图之上的两行清除，为滤波做准备
for( i=0; i<8; i++ )
{
MemCopy( (int*)0x80005000, (int*)Img1, 0x32A0 );
MemCopy( (int*)0x800082A0, (int*)Img2, 0x32A0 );
ImageSub( (int*)0x80005000, (int*)0x800082A0, (int*)0x8000B810, 0x32A0, BIN_THRESHOLD );
SubSrc = 0x8000b6a8; //差分图之上的一行
SubDst = 0x800082a0;
for(j=0; j<36; j++)
{ //对差分图进行滤波，去除噪声点，同时对运动目标进行插补增强
if( FLAG_FILTER != 0 )
PixLineFilter( (int*)SubSrc, (int*)SubDst );
else
MemCopy( (int*)SubDst, (int*)SubSrc, 360 );
SubSrc += 360;
SubDst += 360;
}
MemCopy( (int*)0x8000b540, (int*)0x8000e7e0, 720 ); //拷贝差分图的后两行到差分头
MemCopy( (int*)Img3, (int*)0x800082a0, 0x32A0 );
PixStat( (int*)0x800082a0, i*36, 36, (int*)(HorizontalEnergy), (int*)(VerticalEnergy),90 );
MemCopy( (int*)( 0x00465400 + 0x32A0*i), (int*)0x80005000, 0x32A0 );
Img1 += 0x32A0;
Img2 += 0x32A0;
Img3 += 0x32A0;
}
Center = DetectObject( (int*)(HorizontalEnergy), (int*)(VerticalEnergy), &LeftTop, &RightBottom );
if( Center == 0xffffffff)
{
ObjPos1 = ObjPos2 = OBJ_POS = 0xffffffff;
}
else
{ //保存目标模板，备随后相关算法用
if( ObjPos1 == 0xffffffff)
{//位置队列尚未填满
if( ObjPos2 == 0xffffffff)
{
ObjPos2 = Center;//第一次，无法判断当前位置的合理性
}
else if( abs( (Center>>16) - (ObjPos2>>16) ) < 20 &&
abs( (Center&0x1ff) - (ObjPos2&0x1ff) ) < 20)
{// 第二次，并且当前位置是合理的
ObjPos1 = ObjPos2;
ObjPos2 = Center;
}
else // 第二次，并且当前位置是不合理的
ObjPos1 = ObjPos2 = OBJ_POS = 0xffffffff;
}
else
{//位置队列已经填满
if( abs( (Center>>16) - (ObjPos2>>16) ) < 20 &&
abs( (Center&0x1ff) - (ObjPos2&0x1ff) ) < 20)
{// 当前位置合理，保存模板
ObjPos1 = ObjPos2;
ObjPos2 = Center;
OBJ_POS = Center;
OBJ_RECT = LeftTop;
OBJ_RIGHTBOT = RightBottom;
Width = (RightBottom >> 16) - (LeftTop >> 16)+1;
Height = (RightBottom & 0xffff) - (LeftTop & 0xffff)+1;
//Img1 = 0x00465400 + (LeftTop & 0xffff)*360 + (LeftTop >> 16);//模板在源图的位置
//Img2 = 0x004CA800; //模板数据存放的目的地址
//for( i=0; i<Height; i++)
//{
//ByteMemCopy( (int*)Img2, (int*)Img1, Width ); //拷贝模板第i行
//Img1 += 360;
//Img2 += Width;
//}
//计算目标模板概率密度
target_center_x=OBJ_POS>>16;
target_center_y=OBJ_POS&0xffff;
Model_density_function_q(target_center_x,target_center_y,model_prob,numb);
//计算7个矩特征
Res=Invar(Width,Height,&tt1,&t2,&t3,&t4,&t5,&t6,&t7);
if(Res)
{
ObjPos1 = ObjPos2 = OBJ_POS = 0xffffffff;
I11=I21=I31=I41=I51=I61=I71=I1=I2=I3=I4=I5=I6=I7=0xffffffff;
}
else
{
if(I11==0xffffffff||I21==0xffffffff||I31==0xffffffff||I41==0xffffffff||I51==0xffffffff||I61==0xffffffff||I71==0xffffffff)
{
I11=tt1;
I21=t2;
I31=t3;
I41=t4;
I51=t5;
I61=t6;
I71=t7;
OBJ_POS = 0xffffffff;
}
else if((abs(tt1-I11)+abs(t2-I21)+abs(t3-I31)+abs(t4-I41)+abs(t5-I51)+abs(t6-I61)+abs(t7-I71))<800)
{
I11=tt1;
I21=t2;
I31=t3;
I41=t4;
I51=t5;
I61=t6;
I71=t7;
I1=tt1;
I2=t2;
I3=t3;
I4=t4;
I5=t5;
I6=t6;
I7=t7;
}
else
{
ObjPos1 = ObjPos2 = OBJ_POS = 0xffffffff;
I11=I21=I31=I41=I51=I61=I71=I1=I2=I3=I4=I5=I6=I7=0xffffffff;
}
}
}
else
{ //当前位置不合理，清空位置队列，重新再来
ObjPos1 = ObjPos2 = OBJ_POS = 0xffffffff;
I11=I21=I31=I41=I51=I61=I71=I1=I2=I3=I4=I5=I6=I7=0xffffffff;
}
}
}
if( FLAG_MARK == 1 )
MarkOutObject( LeftTop, RightBottom, (uint)Center, 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
}
//time = ReadTimer( t1 );
}
//原来的形式(调试时也可以先转换为正常计算，算完之后在转换)
//还要考虑可否直接用sqrt()函数，注意类型转换int形式的
int IntSqrt( int A )
{
int b, b1, i;
if( A < 3 )
return 1;
b = GetInitSqrt(A);
for( i=0; i<200; i++ )
{
b1 = (b + A/b )/2;
if( abs(b - b1) < 2 )
break;
b = b1;
}
return b1;
}
// 调试的时候要注意此子函数用的对不对？？？
//int IntSqrt( int A ) //求方根Q14表示法
//{
//int b, b1,b2,b3,i;
//if(A<49152)
//return 16384;
//b2 = GetInitSqrt(A);
//b=b2<<14;
//for(i=0;i<200;i++)
//{
//b3 =(int)floor((A<<14)/b);
//b1 =(int)floor((b +b3)/2);
//if(abs(b-b1)<32768)
//break;
//b=b1;
//}
//return b1;
//}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate candidate's density estimation p
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void Candidate_density_function_p(int candidate_center_x,int candidate_center_y,int *candidate_prob,int numb)
{
int Eponechnikov_profile(int ph);
int Constant_Ch();
int x,y,pixelx,pixely,epanechnikov_number,pixellength2,l_color;
int *p,*candidate_prob_end,*image_candidate;
int constantCh=Constant_Ch(); //normalization constant
candidate_prob_end=candidate_prob+numb;
for(p=candidate_prob;p<candidate_prob_end;p++)
*p=0; //候选模板的概率密度初值
p=candidate_prob;
for (y=-candidate_radiusy;y<=candidate_radiusy;y++)
for (x=-candidate_radiusx;x<=candidate_radiusx;x++)
{
if (y+candidate_center_y<=360 && y+candidate_center_y>=1 &&
x+candidate_center_x<=288 && x+candidate_center_x>=1)
{
pixelx=(int)floor((x<<14)/candidate_radiusx);
pixely=(int)floor((y<<14)/candidate_radiusy);
pixellength2=(pixelx*pixelx>>14)+(pixely*pixely>>14);
epanechnikov_number=Eponechnikov_profile(pixellength2);
image_candidate=(int *)(0x00465400+(y+candidate_center_y)*360+(x+candidate_center_x));//候选模板像素在源图中的位置
l_color=(int)floor(*image_candidate/dividor_image);
*(p+l_color)+= epanechnikov_number*constantCh>>14;
}
else
Flag_pro_zero=1;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// calculate the bhattacharyya coefficient
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
int Bhattacharyya_coefficient(int *mode_prob,int *candidate_prob,int numb)
{
int u,flir_sum=0,temp;
int *p=mode_prob,*q=candidate_prob;
for (u=0;u<numb;u++)
{
temp=(*(p+u))*(*(q+u))>>14;
flir_sum+=(int)floor(sqrt(temp)*128); //左移7位，即乘以128
}
return (flir_sum);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// determine the weights for calculating the mean shift
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void Calculate_weights(int candidate_center_x,int candidate_center_y,int *mode_prob,int *candidate_prob,int *wei)
{
int x,y,pixelx,pixely,flir_weight,l_color,pixel_length2,temp; //pixel_length,
int *p=mode_prob,*q=candidate_prob,*w=wei,*image_candidate;
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);
flir_weight=0;
pixel_length2=(pixelx*pixelx>>14)+(pixely*pixely>>14);
//temp=(float)sqrt(pixel_length2);
//for(j=0;j<Q1;j++)
//temp=temp*2;
//pixel_length=(int)floor(temp);
if ( pixel_length2<16384 &&
y+candidate_center_y<=360 && y+candidate_center_y>=1 &&
x+candidate_center_x<=288 && x+candidate_center_x>=1)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -