Multi state particle filter [closed]

edit

I do not see any code here.

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.

just put it on some pastebin, and let me do it ;)

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[0]; 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); }