business

#include <stdio.h>
#include <math.h>

#define min(a,b) (((a)<(b))?(a):(b))
#define Diff_ImgH 480
#define Diff_ImgW 720
#define Table_Col Diff_ImgW
#define Table_Row Diff_ImgH
#define Max_DetectedP_Number 100
#define Max_Particle_Number 100
#define Max_KdNodes_Number 100

 

int AddParticles_index = 0;
int Used_ObjectParticle_Number = 0;
int Used_PropaParticle_Number = 0;

int Used_NextObject_Number = 0;
int Used_Objects_Number = 0;
int Used_PropaObjects_Number = 0;
int Used_NewObjects_Number = 0;
int ParticleSet_index =0;
typedef struct
{
 int index[100];
 int count;
}myKd;

typedef struct
{
 int ParticleNum;
 int hang;
 int lie;
 double wk;
 double uk1, uk2;
 double vk1, vk2;
 int cluster;
 int count;
 int IndexInS[100];
}myKdNodes;
int Used_KdNode_Number = 0;

 

typedef struct
{
 int ID, count;
 double vi1, vi2;
 double ui1, ui2;
 double LeftB, RightB, UpB, DownB;
 double Particles[Max_Particle_Number][5];
}myObjects;

 

int round(double x)
{
 return (int)(x+0.5);
}

void getDetectedP(double *DetectedP,int Number)
{
 
 return;
}

void getDiff_Img(double *Diffimg,int Number)
{
 
 return;
}

double normrnd(double X, double Y)
{
 return 0;
}

void getDetectTable( double *DetectedP, int ImgH, int ImgW, double *DetectTable)
{
 int HalfDetectedPW = 6;
 int i, j, k;
 int rowStart, rowEnd, colStart, colEnd;
 for (i = 0; i < Max_DetectedP_Number; i++)
 {
  rowStart = round(DetectedP[i*2+1]) - HalfDetectedPW;
  if (rowStart < 1)
   rowStart = 1;
  rowEnd = round(DetectedP[i*2+1]) + HalfDetectedPW;
  if (rowEnd > ImgH)
   rowEnd = ImgH;
  colStart = round(DetectedP[i*2+0]) - HalfDetectedPW;
  if (colStart < 1)
   colStart = 1;
  colEnd =  round(DetectedP[i*2+0]) + HalfDetectedPW;
  if (colEnd > ImgW)
   colEnd = ImgW;

  for(j = rowStart; j < rowEnd +1; j++)
   for(k = colStart; k < colEnd + 1; k++)
    DetectTable[j*(Diff_ImgW+1)+k] = 1;
 }

}

void AddObjectParticles( int ObjectIndex, double *DiffImg, double *DetectTable, double *ObjectTable, double *ParticleSet)
{
 double WeightT1 = 0.1;
 double WeightT2 = 0.08;
 int i, j, m, n;
 int WeightW = 2;
 double Temp_DiffImg[Diff_ImgH + 1][Diff_ImgW + 1] = {0};
 double Weight, sum = 0;
 AddParticles_index = 0;

 for(i = 1; i < Diff_ImgH + 1; i++)
  for(j = 1; j < Diff_ImgW + 1; j++)
   Temp_DiffImg[i][j] = DiffImg[i * (Diff_ImgW + 1) + j] / 255;

 for( i = WeightW + 1; i < Diff_ImgH - WeightW + 1; i++)
 {
  for( j = WeightW + 1; j < Diff_ImgW - WeightW + 1; j++)
  {
   if (ObjectTable[i*(Diff_ImgW+1)+j] == ObjectIndex)
   {
    for (m = i - WeightW; m < i + WeightW +1; m++)
     for (n = j - WeightW; m < j + WeightW +1; n++)
      sum += Temp_DiffImg[m][n];
    Weight = sum / pow(WeightW * 2 + 1, 2);
           
    if ( (DetectTable[i*(Diff_ImgW+1)+j]==1&&Weight > WeightT2) || (DetectTable[i*(Diff_ImgW+1)+j]==0&&Weight > WeightT1) )
    {
     ParticleSet_index++;
     ParticleSet[ParticleSet_index*5] = j;
     ParticleSet[ParticleSet_index*5+1] = i;
     ParticleSet[ParticleSet_index*5+2] = 0;
     ParticleSet[ParticleSet_index*5+3] = 0;
     ParticleSet[ParticleSet_index*5+4] = Weight;
    }
   }
  }
 }
}

void ParticleFilter( int *DiffImg, double *ObjectParticle, double *PropaParticle)
{
 int i, j, m, n;
 int ParticleNum = Used_ObjectParticle_Number;
 double Temp_DiffImg[Diff_ImgH + 1][Diff_ImgW + 1] = {0};
  int WeightW = 2;
  double rp = 0.08;
  double rv = 0.08;
 int row, col, NewRow, NewCol;
  double WPre, sum;
 
 for(i = 1; i < Diff_ImgH + 1; i++)
  for(j = 1; j< Diff_ImgW + 1; j++)
   Temp_DiffImg[i][j] = (double)DiffImg[i * (Diff_ImgW+1) + j] / 255;

 for (i = 1; i < ParticleNum +1; i++)
 {
  row = round(ObjectParticle[i*5+1]);
  col = round(ObjectParticle[i*5]);
  for (m = row - WeightW; m < row + WeightW +1; m++)
   for (n = col - WeightW; m < col + WeightW +1; n++)
    sum += Temp_DiffImg[m][n];
  WPre = sum / pow(WeightW * 2 + 1,2);
  NewRow = round(ObjectParticle[i*5+1]+ObjectParticle[i*5+3]+normrnd(0,rp/WPre));
  NewCol = round(ObjectParticle[i*5]+ObjectParticle[i*5+2]+normrnd(0,rp/WPre));
  if (NewRow - WeightW > 0 && NewRow + WeightW <= Diff_ImgH && NewCol - WeightW > 0 && NewCol + WeightW <= Diff_ImgW)
  {
   PropaParticle[i*5] = NewCol;
   PropaParticle[i*5+1] = NewRow;
   PropaParticle[i*5+2] = PropaParticle[i*5] - ObjectParticle[i*5] + normrnd(0, rv / WPre);
   PropaParticle[i*5+3] = PropaParticle[i*5+1] - ObjectParticle[i*5+1] + normrnd(0, rv / WPre);
   for (m = NewRow - WeightW; m < NewRow + WeightW +1; m++)
    for (n = NewCol - WeightW; m < NewCol + WeightW +1; n++)
     sum += Temp_DiffImg[m][n];
   PropaParticle[i*5+4] = sum / pow(WeightW * 2 + 1,2);
  }
 }
 Used_PropaParticle_Number = ParticleNum;
}

void BildeKdTree(myKdNodes KdNodes[], double *ParticleSet, int Used_Number)
{
 int i, j;
 int Num = Used_Number;//这个值的问题
 int ImageH = Diff_ImgH;
 int ImageW = Diff_ImgW;
 int Value = 3;
 int NodeSize = 10;
 int ParticleNumT = 2;
 double DensityT = 0.05;
 int count = 1;
 int Kd_index;
 int ParticleNum;
 double wk;
 double Density;
 double sum;
 int hang = (int)ceil(ImageH / NodeSize);
 int lie = (int)ceil(ImageW / NodeSize);
 Used_KdNode_Number = 0;
 myKd KdProperty[100]={0};
 
 
 if (Used_Number == 0)
 {
  return;
 }
 
 
 for (i = 1; i < Num + 1; i++)
 {
  if (ParticleSet[i*5+1] > 0 && ParticleSet[i*5+1]<=ImageH && ParticleSet[i*5]>0 && ParticleSet[i*5]<=ImageW)
  {
   Kd_index = ((int)ceil(ParticleSet[i*5+1]/NodeSize)-1)*lie+(int)ceil(ParticleSet[i*5] / NodeSize);
   KdProperty[Kd_index].index[KdProperty[Kd_index].count++] = i;
  }
 }

 for (i = 1; i < hang*lie + 1; i++)
 {
  if (KdProperty[i].count == 0)
   continue;
  for(j = 0; j < KdProperty[i].count; j++)
   wk += ParticleSet[KdProperty[i].index[j]*5+4];
  ParticleNum = KdProperty[i].count;
  Density = wk/ParticleNum;
  if (Density > DensityT && ParticleNum > ParticleNumT)
  {
   for(j = 0; j < KdProperty[i].count; j++)
    KdNodes[count].IndexInS[j] = KdProperty[i].index[j];
   KdNodes[count].count = KdProperty[i].count;
   KdNodes[count].ParticleNum = ParticleNum;
   KdNodes[count].hang = i / lie;
   KdNodes[count].lie = i % lie;
   KdNodes[count].wk = wk;

   sum = 0;
   for(j = 0; j < KdProperty[i].count; j++)
    sum += ParticleSet[KdProperty[i].index[j]*5+4] * ParticleSet[KdProperty[i].index[j]*5];
   KdNodes[count].uk1 = sum / wk;

   sum = 0;
   for(j = 0; j < KdProperty[i].count; j++)
    sum += ParticleSet[KdProperty[i].index[j]*5+4] * ParticleSet[KdProperty[i].index[j]*5+1];
   KdNodes[count].uk2 = sum / wk;
  
   sum = 0;
   for(j = 0; j < KdProperty[i].count; j++)
    sum += ParticleSet[KdProperty[i].index[j]*5+4] * pow((ParticleSet[KdProperty[i].index[j]*5] - KdNodes[count].uk1), 2);
   KdNodes[count].vk1 = sum / wk;

   sum = 0;
   for(j = 0; j < KdProperty[i].count; j++)
    sum += ParticleSet[KdProperty[i].index[j]*5+4] * pow((ParticleSet[KdProperty[i].index[j]*5+1] - KdNodes[count].uk1), 2);
   KdNodes[count].vk2 = sum / wk;
   KdNodes[count].cluster = 0;
   count = count + 1;
  }
 }
 if (count > Used_KdNode_Number)
 {
  Used_KdNode_Number = count;
 }
}

int ParticleCluster(myKdNodes KdNodes[], int *IDTable, double *PropaParticle, myObjects PropaObject[])
{

 int TimeofV = 3;
 int NodesNum = Used_KdNode_Number;
 int SeedPointer = 0;
 int ClassLable = 0;
 int DisT = 30;
 int id;
 double wi, ui1, ui2;
 int StackPointer;
 int i, j, k;
 int Seed;
 int Stack[100] = {0};

 if (Used_KdNode_Number == 0)
  return;
 while (SeedPointer < NodesNum)
 {
  SeedPointer = SeedPointer+1;
  if (KdNodes[SeedPointer].cluster!= 0)
   continue;
  ClassLable = ClassLable + 1;
  for (id = 1; id < MostOBNum + 1; id++)
  {
   if (IDTable[id] == 0)
    break;
  }
  IDTable[id] = 1;
  PropaObject[ClassLable].ID = id;
  PropaObject[ClassLable].ui1 = KdNodes[SeedPointer].uk1;
  PropaObject[ClassLable].ui2 = KdNodes[SeedPointer].uk2;
  PropaObject[ClassLable].vi1 = KdNodes[SeedPointer].vk1;
  PropaObject[ClassLable].vi2 = KdNodes[SeedPointer].vk2;
  for( i = 1; i < KdNodes[SeedPointer].count + 1; i++)
  {
   for(j = 0; j < 5; j++)
   {
    PropaObject[ClassLable].count++;
    PropaObject[ClassLable].Particles[Objects[ClassLable].count][j] = PropaParticle[KdNodes[SeedPointer].IndexInS[i]][j];
   }
  }
  wi = KdNodes[SeedPointer].wk;
  ui1 = PropaObject[ClassLable].ui1;
  ui2 = PropaObject[ClassLable].ui2;
  KdNodes[SeedPointer].cluster = ClassLable;
  StackPointer = 1;
  Stack[StackPointer] = SeedPointer;
  while ( StackPointer != 0)
  {
   Seed = Stack[StackPointer];
   StackPointer = StackPointer - 1;
   for (i = SeedPointer+1; i < NodesNum; i++)
   {
    if (KdNodes[i].cluster == 0 && sqrt(pow(KdNodes[Seed].uk1 - KdNodes[i].uk1, 2) + pow(KdNodes[Seed].uk2 - KdNodes[i].uk2, 2)) < DisT)
    {
     StackPointer = StackPointer + 1;
     Stack[StackPointer] = i;
     KdNodes[i].cluster = ClassLable;
    
     for (j = 1; j < KdNodes[i].count + 1; j++)
     {
      for(k = 0; k < 5; k++)
      {
       PropaObject[ClassLable].count++;
       PropaObject[ClassLable].Particles[Objects[ClassLable].count][j] = PropaParticle[KdNodes[i].IndexInS[j]][k];
      }
     }
     PropaObject[ClassLable].ui1 = (wi*PropaObject[ClassLable].ui1 + KdNodes[i].wk*KdNodes[i].uk1)/(wi+KdNodes[i].wk);
     PropaObject[ClassLable].ui2 = (wi*PropaObject[ClassLable].ui2 + KdNodes[i].wk*KdNodes[i].uk2)/(wi+KdNodes[i].wk);
     PropaObject[ClassLable].vi1 = (wi*(PropaObject[ClassLable].vi1+pow(Objects[ClassLable].ui1 - ui1, 2)) +
      KdNodes[i].wk*(KdNodes[i].vk1+pow((PropaObject[ClassLable].ui1 - KdNodes[i].uk1), 2)))/(wi+KdNodes[i].wk);
     PropaObject[ClassLable].vi2 = (wi*(PropaObject[ClassLable].vi2+pow(Objects[ClassLable].ui2 - ui2, 2)) +
      KdNodes[i].wk*(KdNodes[i].vk2+pow((PropaObject[ClassLable].ui2 - KdNodes[i].uk2), 2)))/(wi+KdNodes[i].wk);
     wi = wi + KdNodes[i].wk;
     ui1 = PropaObject[ClassLable].ui1;
     ui2 = PropaObject[ClassLable].ui2;
    }
   }
  }
  PropaObject[ClassLable].LeftB = PropaObject[ClassLable].ui1 - TimeofV * sqrt(PropaObject[ClassLable].vi1);
  PropaObject[ClassLable].RightB = PropaObject[ClassLable].ui1 + TimeofV * sqrt(PropaObject[ClassLable].vi1);
  PropaObject[ClassLable].UpB = PropaObject[ClassLable].ui2 - TimeofV * sqrt(PropaObject[ClassLable].vi2);
  PropaObject[ClassLable].DownB = PropaObject[ClassLable].ui2 + TimeofV * sqrt(PropaObject[ClassLable].vi2);
 }
  return ClassLable;
}
 
  void ModifyID( myObjects PropaObject[], double *IDTable, int ID )
{

 int OBNum = Used_PropaObjects_Number;
 int MaxAreaIn = 0;
 double MaxArea = -1;
 double OBArea = 0;
 int i;
 for (i = 1; i < OBNum +1; i++)
 {
  OBArea = (PropaObject[i].RightB - PropaObject[i].LeftB) * (PropaObject[i].DownB-PropaObject[i].UpB);
  if (OBArea > MaxArea)
  {
   MaxArea = OBArea;
   MaxAreaIn = i;
  }
 }
 IDTable[PropaObject[MaxAreaIn].ID] = 0;
 PropaObject[MaxAreaIn].ID = ID;
}

void GetObjectTable( myObjects NextObject[],int ImgH, int ImgW, double *ObjectTable)
{
 int ObjectNum = Used_NextObject_Number;
 int TimeofV = 3;
 int i;
 double Dis1, Dis2;
 int startRow, endRow, startCol, endCol;
 int j, k;

 for (i = 1; i < ObjectNum + 1; i++)
 {
  Dis1 = TimeofV * sqrt(NextObject[i].vi1);
  Dis2 = TimeofV * sqrt(NextObject[i].vi2);
  startRow = round(NextObject[i].ui2 -  Dis2);
  if (startRow < 1)
   startRow = 1;
  endRow = round(NextObject[i].ui2 +  Dis2);
  if (endRow > ImgH)
   endRow = ImgH;
  startCol = round(NextObject[i].ui1 - Dis1);
  if (startCol < 1)
        startCol = 1;
  endCol = round(NextObject[i].ui1 + Dis1);
  if (endCol > ImgW)
   endCol = ImgW;
  for(j = startRow; j < endRow +1; j++)
   for(k = startCol; k < endCol + 1; k++)
    ObjectTable[j*(Diff_ImgW + 1)+k] = i; 
 }
}

void NewObjectSample(int *DiffImg, double *DetectTable, double *ObjectTable, double *ParticleSet)
{
 double  WeightT = 0.08;
 int WeightW = 2;
 int i, j, m, n;
 double Temp_DiffImg[Diff_ImgH + 1][Diff_ImgW + 1] = {0};
 double Weight, sum = 0;
 ParticleSet_index = 0;
 

 for(i = 1; i < Diff_ImgH + 1; i++)
  for(j = 1; j< Diff_ImgW + 1; j++)
   Temp_DiffImg[i][j] = (double)DiffImg[i * (Diff_ImgW + 1) + j] / 255;

 for( i = WeightW + 1; i < Diff_ImgH - WeightW + 1; i++)
 {
  for( j = WeightW + 1; j < Diff_ImgW - WeightW + 1; j++)
  {
   if (ObjectTable[i*(Diff_ImgW+1)+j] == 0&&DetectTable[i*(Diff_ImgW+1)+j] == 1)
   {
    for (m = i - WeightW; m < i + WeightW +1; m++)
     for (n = j - WeightW; m < j + WeightW +1; n++)
      sum += Temp_DiffImg[m][n];
    Weight = sum / pow(WeightW * 2 + 1, 2);
           
    if ( Weight > WeightT)
    {
     ParticleSet_index++;
     ParticleSet[ParticleSet_index*5] = j;
     ParticleSet[ParticleSet_index*5+1] = i;
     ParticleSet[ParticleSet_index*5+2] = 0;
     ParticleSet[ParticleSet_index*5+3] = 0;
     ParticleSet[ParticleSet_index*5+4] = Weight;
    
    }
   }
  }
 }
}

void sort(double *x,double *y)
{
 int i, j;
 double temp;
 for (i = 0; i < 4; i++)
 {
  *(x+i) = *(y+i);
 }
 for (j = 1; j < 4; j++)
 {
  for (i = 0; i < 3; i++)
  {
   if(*(x+i) > *(x+i+1))
   {
    temp = *(x+i);
    *(x+i)= *(x+i+1);
    *(x+i+1) = temp;
   }
  }
 }
}

void ObAssociate( myObjects Object[], int *IDTable)
{
 double Particles[Max_Particle_Number][5];
 int Used_Particle_Number = 0;
 myKdNodes KdNodes[Max_KdNodes_Number]={0};
 int Used_KdNode_Number = 0;

 int OBNum = Used_Objects_Number;
 int i, j, m, n;
 double TimeAreaT = (double)1/3;
 char Disappear[100] = {0};
 int Used_Disappear_Number = 0;
 double OBiArea, OBjArea, InterArea;
 double XLable[4]={0}, YLable[4]={0}, XSort[4]={0}, YSort[4]={0};
 int temp[4]={0};

 if(OBNum == 0)
  return;
 for (i = 1; i < OBNum+1; i++)
 {
  OBiArea = (Objects[i].RightB-Objects[i].LeftB)*(Objects[i].DownB-Objects[i].UpB);
  for (j = i+1; j < OBNum +1; j++)
  {
   if (Objects[i].LeftB < Objects[j].LeftB)
   {
    XLable[0] = Objects[i].LeftB;
    XLable[1] = Objects[i].RightB;
    XLable[2] = Objects[j].LeftB;
    XLable[3] = Objects[j].RightB;
   }
   else
   {
    XLable[0] = Objects[j].LeftB;
    XLable[1] = Objects[j].RightB;
    XLable[2] = Objects[i].LeftB;
    XLable[3] = Objects[i].RightB;
   }
   temp[0] = Objects[j].LeftB;
   temp[1] = Objects[j].RightB;
   temp[2] = Objects[i].LeftB;
   temp[3] = Objects[i].RightB;
            sort(XSort, temp);
       
   if (Objects[i].UpB < Objects[j].UpB)
   {
    YLable[0] = Objects[i].UpB;
    YLable[1] = Objects[i].DownB;
    YLable[2] = Objects[j].UpB;
    YLable[3] = Objects[j].DownB;
   }
   else
   {
    YLable[0] = Objects[j].UpB;
    YLable[1] = Objects[j].DownB;
    YLable[2] = Objects[i].UpB;
    YLable[3] = Objects[i].DownB;
   }
   temp[0] = Objects[j].UpB;
   temp[1] = Objects[j].DownB;
   temp[2] = Objects[i].UpB;
   temp[3] = Objects[i].DownB;
   sort(YSort, temp);
   if ((XLable[0]==XSort[0]&&XLable[1]==XSort[1]&&XLable[2]==XSort[2]&&XLable[3]==XSort[3])
    ||(YLable[0]==YSort[0]&&YLable[1]==YSort[1]&&YLable[2]==YSort[2]&&YLable[3]==YSort[3]))
            continue;
   OBjArea = (Objects[j].RightB-Objects[j].LeftB)*(Objects[j].DownB-Objects[j].UpB);
   InterArea = (XSort[3]-XSort[2]) * (YSort[3]-YSort[2]);
   if (InterArea / min(OBiArea,OBjArea) > TimeAreaT)
   {
   
    for(m = 1; m < Objects[i].count + 1; m++)
    {
     for (n = 0; n < 5; n++)
     {
      Used_Particle_Number++;
      Particles[Used_Particle_Number][n] = Objects[i].Particles[m][n];
     }
    }
    for(m = 1; m < Objects[j].count + 1; m++)
    {
     for (n = 0; n < 5; n++)
     {
      Used_Particle_Number++;
      Particles[Used_Particle_Number][n] = Objects[j].Particles[m][n];
     }
    }
    BildeKdTree(KdNodes, Particles);
    if (Used_KdNode_Number != 0)
    {
     //[ NewObject ] = GenerateObject( KdNodes, Particles, Objects[j].ID );
     //Objects[j] = NewObject;
     Disappear[++Used_Disappear_Number] = i;
     IDTable[Objects[i].ID] = 0;
     break;
    }
   }
  }
 }
 //UnionObject = Objects(setdiff(1:OBNum,Disappear));   
}

void DrawResult( myObjects Objects[])
{
 int i, j;
 int ObjectNum = Used_Objects_Number;
 int TimeofV = 3;
 int Dis1, Dis2;
 int startRow, endRow, startCol, endCol;
 int row, col;

 for (i = 1; i < ObjectNum +1; i++)
 {
 
  Dis1=TimeofV*sqrt(Objects[i].vi1);
  Dis2=TimeofV*sqrt(Objects[i].vi2);
  startRow = round(Objects[i].ui2 -  Dis2);
  if(startRow < 1)
   startRow = 1;
  endRow = round(Objects[i].ui2 +  Dis2);
  if (endRow > Diff_ImgH)
   endRow = Diff_ImgH;
  startCol = round(Objects[i].ui1 - Dis1);
  if (startCol < 1)
   startCol = 1;
  endCol = round(Objects[i].ui1 + Dis1);
  if (endCol > Diff_ImgW)
   endCol = Diff_ImgW;

  for(j = 1; j < Objects[i].count; j++)
  {
        row = round(Objects[i].Particles[j][2]);
        col = round(Objects[i].Particles[j][1]);
        printf("Objects %d Particle %d row %d col %d", i, j, row, col);
      
  }
 }

}

int main()
{
 int MostOBNum = 30;
 int IDTable[MostOBNum + 1] = {0};
 int StartFrame = 1;
 int FrameNum = 400;
 
 double DetectedP[Max_DetectedP_Number + 1][2] = {0};
 double DiffImg[IDTable_Row+1][IDTable_Col+1] = {0};
 double DetectTable[Diff_ImgH+1][Diff_ImgW+1] = {0};
 double ObjectTable[Diff_ImgH+1][Diff_ImgW+1] = {0};
 double AddParticles[Max_Particle_Number][5] = {0};
 double ObjectParticle[Max_Particle_Number][5] = {0};
 double PropaParticle[Max_Particle_Number][5] = {0};
 double ParticleSet[Max_Particle_Number][5] = {0};
 
 myKdNodes KdNodes[Max_KdNodes_Number]={0};
 
 myObjects Objects[100] = {0};
 myObjects PropaObject[100] = {0};
 myObjects NextObject[100]={0};
 myObjects NewObjects[100]={0};
 
 
 
 int i, j, k, m, n;

 //ShowArr2(DetectedP, 100, 2);

 for(m = StartFrame; m < FrameNum + 1; m++)
 {
  printf("Frame %d /n", m);
  
  getDiff_Img(DiffImg, m);
  getDetectedP(DetectedP, m);
  GetDetectTable(DetectedP, Diff_ImgH, Diff_ImgH, DetectTable);
  ObjectsNum = Used_Objects_Number;
  Used_NextObject_Number = 0;
  
  for( k = 1; k < ObjectsNum + 1; k++)
  {
   AddObjectParticles(k, DiffImg, DetectTable, ObjectTable, AddParticles);
   Used_ObjectParticle_Number = 0;
    for( j = 1; j < Objects[k].count +1; j++)
    {
     Used_ObjectParticle_Number++;
      ObjectParticle[Used_ObjectParticle_Number][0] = Objects[k].Particles[j][0];
      ObjectParticle[Used_ObjectParticle_Number][1] = Objects[k].Particles[j][1];
      ObjectParticle[Used_ObjectParticle_Number][2] = Objects[k].Particles[j][2];
      ObjectParticle[Used_ObjectParticle_Number][3] = Objects[k].Particles[j][3];
      ObjectParticle[Used_ObjectParticle_Number][4] = Objects[k].Particles[j][4];
    }
    for (j = 1; j < ParticleSet_index + 1; j++)
    {
     Used_ObjectParticle_Number++;
      ObjectParticle[Used_ObjectParticle_Number][0] = AddParticles[j][0];
      ObjectParticle[Used_ObjectParticle_Number][1] = AddParticles[j][1];
      ObjectParticle[Used_ObjectParticle_Number][2] = AddParticles[j][2];
      ObjectParticle[Used_ObjectParticle_Number][3] = AddParticles[j][3];
      ObjectParticle[Used_ObjectParticle_Number][4] = AddParticles[j][4];
    }
   
    ParticleFilter(Diffimg, ObjectParticle, PropaParticles);
    BildeKdTree(KdNodes, PropaParticle, Used_PropaParticle_Number);
   if (Used_KdNode_Number != 0)
   {
    Used_PropaObjects_Number = ParticleCluster(KdNodes, IDTable, PropaParticle, PropaObject);
    void ModifyID(PropaObject, IDTable, Objects[k].ID)
    for (i = 1; i < Used_PropaObjects_Number + 1; i++)
    {
     Used_NextObject_Number++;
     NextObject[Used_NextObject_Number].ID = PropaObject[i].ID;
     NextObject[Used_NextObject_Number].DownB = PropaObject[i].DownB;
     NextObject[Used_NextObject_Number].LeftB = PropaObject[i].LeftB;
     NextObject[Used_NextObject_Number].RightB = PropaObject[i].RightB;
     NextObject[Used_NextObject_Number].ui1 = PropaObject[i].ui1;
     NextObject[Used_NextObject_Number].ui2 = PropaObject[i].ui2;
     NextObject[Used_NextObject_Number].vi1 = PropaObject[i].vi1;
     NextObject[Used_NextObject_Number].vi2 = PropaObject[i].vi2;
     NextObject[Used_NextObject_Number].count = PropaObject[i].count;
     for (j = 1; j < PropaObject[i].count + 1; j++)
     {
      NextObject[Used_NextObject_Number].Particles[j][0] = PropaObject[i].Particles[j][0];
      NextObject[Used_NextObject_Number].Particles[j][1] = PropaObject[i].Particles[j][1];
      NextObject[Used_NextObject_Number].Particles[j][2] = PropaObject[i].Particles[j][2];
      NextObject[Used_NextObject_Number].Particles[j][3] = PropaObject[i].Particles[j][3];
      NextObject[Used_NextObject_Number].Particles[j][4] = PropaObject[i].Particles[j][4];
     }
    }
   }
   else
    IDTable[Objects[k].ID] = 0;
  }
 
 
   GetObjectTable(NextObjects, Diff_ImgH, Diff_ImgW, ObjectTable);
   NewObjectSample(DiffImg, DetectTable, ObjectTable, ParticleSet);
   BildeKdTree(KdNodes, ParticleSet, ParticleSet_index);
   Used_NewObjects_Number = ParticleCluster(KdNodes, IDTable, ParticleSet, NewObjects);
   
   Used_Objects_Number = 0;
  for (i = 1; i < Used_NextObject_Number + 1; i++)
    {
     Used_Objects_Number++;
     Objects[Used_Objects_Number].ID = NextObject[i].ID;
     Objects[Used_Objects_Number].DownB = NextObject[i].DownB;
     Objects[Used_Objects_Number].LeftB = NextObject[i].LeftB;
     Objects[Used_Objects_Number].RightB = NextObject[i].RightB;
     Objects[Used_Objects_Number].ui1 = NextObject[i].ui1;
     Objects[Used_Objects_Number].ui2 = NextObject[i].ui2;
     Objects[Used_Objects_Number].vi1 = NextObject[i].vi1;
     Objects[Used_Objects_Number].vi2 = NextObject[i].vi2;
     Objects[Used_Objects_Number].count = NextObject[i].count;
     for (j = 1; j < NextObject[i].count + 1; j++)
     {
      Objects[Used_Objects_Number].Particles[j][0] = NextObject[i].Particles[j][0];
      Objects[Used_Objects_Number].Particles[j][1] = NextObject[i].Particles[j][1];
      Objects[Used_Objects_Number].Particles[j][2] = NextObject[i].Particles[j][2];
      Objects[Used_Objects_Number].Particles[j][3] = NextObject[i].Particles[j][3];
      Objects[Used_Objects_Number].Particles[j][4] = NextObject[i].Particles[j][4];
     }
    }
  for (i = 1; i < Used_NewObjects_Number + 1; i++)
    {
     Used_Objects_Number++;
     Objects[Used_Objects_Number].ID = NewObjects[i].ID;
     Objects[Used_Objects_Number].DownB = NewObjects[i].DownB;
     Objects[Used_Objects_Number].LeftB = NewObjects[i].LeftB;
     Objects[Used_Objects_Number].RightB = NewObjects[i].RightB;
     Objects[Used_Objects_Number].ui1 = NewObjects[i].ui1;
     Objects[Used_Objects_Number].ui2 = NewObjects[i].ui2;
     Objects[Used_Objects_Number].vi1 = NewObjects[i].vi1;
     Objects[Used_Objects_Number].vi2 = NewObjects[i].vi2;
     Objects[Used_Objects_Number].count = NewObjects[i].count;
     for (j = 1; j < NewObjects[i].count + 1; j++)
     {
      Objects[Used_Objects_Number].Particles[j][0] = NewObjects[i].Particles[j][0];
      Objects[Used_Objects_Number].Particles[j][1] = NewObjects[i].Particles[j][1];
      Objects[Used_Objects_Number].Particles[j][2] = NewObjects[i].Particles[j][2];
      Objects[Used_Objects_Number].Particles[j][3] = NewObjects[i].Particles[j][3];
      Objects[Used_Objects_Number].Particles[j][4] = NewObjects[i].Particles[j][4];
     }
    }
   
    ObAssociate( Object, IDTable);
  DrawResult( Objects);
}

}

//测试函数中传入结构体数组的问题。