Multi state particle filter

Hi, I have a particle filter with state vector as(xposition, yposiiton, width). I want to modify the particle filter to a multistate particle filter where the state vector is (xposition, yposiiton, width,pose angle). Here the xposition, yposition and width are continuous variables and pose angle is a discrete variable.

How can I modify the code to make the state vector a 4D and do the tracking?

edit retag close merge delete

2
1

I will try to put in words. 1. A particle filter which is used for object tracking has a state vector of the form (x,y,width,height) where x and y denotes the position of the object and width and height defines the object size 2. Tracking happens by finding the observation model in each frame 3. Now I want to modify the particle filter according to this paper http://www.idiap.ch/~odobez/publicati... so that I can increase the pose recognition accuracy.

According to this paper, the state vector is of the form (x,y,width,height,pose). I wanted to implement this to estimate the pose of a person.

All that I have is a c++ code for object tracking using particle filter. Now I wanted it to modify for this problem.

for (int p = 0; p < partn; p++) {

float u1,u2,u3,u4,u5,u6,n1,n2,n3,n4,probability,scale,rCorr,gCorr,bCorr,likelihood;
u1 = ((float)rand())/RAND_MAX;
u2 = ((float)rand())/RAND_MAX;
u3 = ((float)rand())/RAND_MAX;
u4 = ((float)rand())/RAND_MAX;
u5 = ((float)rand())/RAND_MAX;
u6 = ((float)rand())/RAND_MAX;
//get normally distributed random numbers using box-muller transform (has mean 0 and std 1)
n1 = sqrt(-2*log(u1)) * cos(2*3.14159265359*u2);
n2 = sqrt(-2*log(u1)) * sin(2*3.14159265359*u2);
n3 = sqrt(-2*log(u3)) * sin(2*3.14159265359*u4);

n1=n1*pArr[p].width * 0.15;
n1+=pArr[p].x;

n2=n2*pArr[p].height * 0.12;
n2+=pArr[p].y;

n3=n3*pArr[p].scale*0.04;
n3+=pArr[p].scale;

oArr[p].x = pArr[p].x ; oArr[p].y = pArr[p].y; oArr[p].scale = pArr[p].scale; oArr[p].dx=pArr[p].dx; oArr[p].dy=pArr[p].dy; oArr[p].ds=pArr[p].ds; //assign new position and scale

pArr[p].x  = nx;
pArr[p].y  = ny;
pArr[p].scale = ns;
pArr[p].dx=new_dx;
pArr[p].dy=new_dy;
pArr[p].ds=new_ds;

roi = new Rect(nx - pArr[p].width*ns/2,
ny - pArr[p].height*ns/2,
pArr[p].width*ns,
pArr[p].height*ns);
partHist = getHistogramHSV(hsvIm(*roi));

pArr[p].w = (1-compareHist(partHist,refHist,CV_COMP_BHATTACHARYYA));
totalW   += pArr[p].w;
}
//normalize weights
for(int p=0; p < partn; p++)
{
pArr[p].w = pArr[p].w/totalW;
}

}

//select the particle with greater weight particle getBest(particle* pArr,int partn) { particle part; part = pArr; for(int k=1; k < partn; k++) { if(pArr[k].w > part.w) { part = pArr[k]; } } return part; } particle* resampleParticles(particle* pArr,int partn) { int pNum; int m=0,i;

particle* newArr = (particle*)malloc(sizeof(particle)*partn);
sortParticles(pArr,partn);
int counter=partn/15;
int temp=0;

int temp1=0;
for(int p=0;p<partn;p++)
{
temp1 = counter;
while(counter!=0)
{
newArr[temp] = pArr[p];
temp = temp + 1;
counter = counter - 1;
if(temp == partn)
break;
}
if(temp == partn)
break;
counter = temp1 - 1;
if(counter <= 0)
counter = 1;
}

return newArr}

void sortParticles(particle* pArr, int partn) { particle temp; bool swapped; do { swapped = false; for(int p=1; p < partn; p++) { temp = pArr[p]; if(pArr[p].w > pArr[p-1].w) { pArr[p] = pArr[p-1]; pArr[p-1] = temp; swapped = true; } } } while(swapped); }