돛단배

// 3-D Particle filter algorithm + Computer Vision exercise
// : object tracking - contour tracking
// lym, VIP Lab, Sogang Univ.
// 2009-11-23
// ref. Probabilistic Robotics: 98p

#include <OpenCV/OpenCV.h> // matrix operations & Canny edge detection
#include <iostream>
#include <cstdlib> // RAND_MAX
#include <ctime> // time as a random seed
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.14159265
#define N 100 //number of particles
#define D 3 // dimension of the state

// uniform random number generator
double uniform_random(void) {
   
    return (double) rand() / (double) RAND_MAX;
   
}

// Gaussian random number generator
double gaussian_random(void) {
   
    static int next_gaussian = 0;
    static double saved_gaussian_value;
   
    double fac, rsq, v1, v2;
   
    if(next_gaussian == 0) {
       
        do {
            v1 = 2.0 * uniform_random() - 1.0;
            v2 = 2.0 * uniform_random() - 1.0;
            rsq = v1 * v1 + v2 * v2;
        }
        while(rsq >= 1.0 || rsq == 0.0);
        fac = sqrt(-2.0 * log(rsq) / rsq);
        saved_gaussian_value = v1 * fac;
        next_gaussian = 1;
        return v2 * fac;
    }
    else {
        next_gaussian = 0;
        return saved_gaussian_value;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}
////////////////////////////////////////////////////////////////////////////

// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
    double distance2 = 0;
    double differance = 0;
    for (int u = 0; u < 3; u++)
    {
        differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
        distance2 += differance * differance;
    }
    return sqrt(distance2);
}

double distanceEuclidean(CvPoint2D64f a, CvPoint2D64f b)
{
    double d2 = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
    return sqrt(d2);
}

// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
   
    CvMat* diff = cvCreateMat(3, 1, CV_64FC1);
    cvSub(sample, mean, diff); // sample - mean -> diff
    CvMat* diff_t = cvCreateMat(1, 3, CV_64FC1);
    cvTranspose(diff, diff_t); // transpose(diff) -> diff_t
    CvMat* cov_inv = cvCreateMat(3, 3, CV_64FC1);
    cvInvert(covariance, cov_inv); // transpose(covariance) -> cov_inv
    CvMat* tmp = cvCreateMat(3, 1, CV_64FC1);
    CvMat* dist = cvCreateMat(1, 1, CV_64FC1);
    cvMatMul(cov_inv, diff, tmp); // cov_inv * diff -> tmp   
    cvMatMul(diff_t, tmp, dist); // diff_t * tmp -> dist
   
    double likeliness = exp( -0.5 * cvmGet(dist, 0, 0) );
    double bound = 0.0000001;
    if ( likeliness < bound )
    {
        likeliness = bound;
    }
    return likeliness;
//    return exp( -0.5 * cvmGet(dist, 0, 0) );
//    return max(0.0000001, exp(-0.5 * cvmGet(dist, 0, 0)));   
}

// likelihood based on normal distribution (ref. 14p eqn(2.3))
double likelihood(double distance, double standardDeviation) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * distance*distance / variance ) / sqrt(2 * PI * variance);
}

int main (int argc, char * const argv[]) {
   
    srand(time(NULL));
   
    IplImage *iplOriginalColor; // image to be captured
    IplImage *iplOriginalGrey; // grey-scale image of "iplOriginalColor"
    IplImage *iplEdge; // image detected by Canny edge algorithm
    IplImage *iplImg; // resulting image to show tracking process   
    IplImage *iplEdgeClone;

    int hours, minutes, seconds;
    double frame_rate, Codec, frame_count, duration;
    char fnVideo[200], titleOriginal[200], titleEdge[200], titleResult[200];
   
    sprintf(titleOriginal, "original");
    sprintf(titleEdge, "Edges by Canny detector");
//    sprintf(fnVideo, "E:/AVCHD/BDMV/STREAM/00092.avi");   
    sprintf(fnVideo, "/Users/lym/Documents/VIP/2009/Nov/volleyBall.mov");
    sprintf(titleResult, "3D Particle filter for contour tracking");
   
    CvCapture *capture = cvCaptureFromAVI(fnVideo);
   
    // stop the process if capture is failed
    if(!capture) { printf("Can NOT read the movie file\n"); return -1; }
   
    frame_rate = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
//    Codec = cvGetCaptureProperty( capture, CV_CAP_PROP_FOURCC );
    frame_count = cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_COUNT);
   
    duration = frame_count/frame_rate;
    hours = duration/3600;
    minutes = (duration-hours*3600)/60;
    seconds = duration-hours*3600-minutes*60;
   
    //  stop the process if grabbing is failed
    //    if(cvGrabFrame(capture) == 0) { printf("Can NOT grab a frame\n"); return -1; }
   
    cvSetCaptureProperty(capture, CV_CAP_PROP_POS_FRAMES, 0); // go to frame #0
    iplOriginalColor = cvRetrieveFrame(capture);
    iplOriginalGrey = cvCreateImage(cvGetSize(iplOriginalColor), 8, 1);
    iplEdge = cvCloneImage(iplOriginalGrey);
    iplEdgeClone = cvCreateImage(cvSize(iplOriginalColor->width, iplOriginalColor->height), 8, 3);
    iplImg = cvCreateImage(cvSize(iplOriginalColor->width, iplOriginalColor->height), 8, 3);   
   
    int width = iplOriginalColor->width;
    int height = iplOriginalColor->height;
   
    cvNamedWindow(titleOriginal);
    cvNamedWindow(titleEdge);
   
    cout << "image width : height = " << width << "  " << height << endl;
    cout << "# of frames = " << frame_count << endl;   
    cout << "capture finished" << endl;   
   
   
    // set the system   
   
    // set the process noise
    // covariance of Gaussian noise to control
    CvMat* transition_noise = cvCreateMat(D, D, CV_64FC1);
    // assume the transition noise
    for (int row = 0; row < D; row++)
    {   
        for (int col = 0; col < D; col++)
        {
            cvmSet(transition_noise, row, col, 0.0);
        }
    }
    cvmSet(transition_noise, 0, 0, 3.0);
    cvmSet(transition_noise, 1, 1, 3.0);
    cvmSet(transition_noise, 2, 2, 0.3);
   
    // set the measurement noise
/*
    // covariance of Gaussian noise to measurement
     CvMat* measurement_noise = cvCreateMat(D, D, CV_64FC1);
     // initialize the measurement noise
     for (int row = 0; row < D; row++)
     {   
        for (int col = 0; col < D; col++)
        {
            cvmSet(measurement_noise, row, col, 0.0);
        }
     }
     cvmSet(measurement_noise, 0, 0, 5.0);
     cvmSet(measurement_noise, 1, 1, 5.0);
     cvmSet(measurement_noise, 2, 2, 5.0); 
 */
    double measurement_noise = 2.0; // standrad deviation of Gaussian noise to measurement
   
    CvMat* state = cvCreateMat(D, 1, CV_64FC1);    // state of the system to be estimated   
//    CvMat* measurement = cvCreateMat(2, 1, CV_64FC1); // measurement of states
   
    // declare particles
    CvMat* pb [N]; // estimated particles
    CvMat* pp [N]; // predicted particles
    CvMat* pu [N]; // temporary variables to update a particle
    CvMat* v[N]; // estimated velocity of each particle
    CvMat* vu[N]; // temporary varialbe to update the velocity   
    double w[N]; // weight of each particle
    for (int n = 0; n < N; n++)
    {
        pb[n] = cvCreateMat(D, 1, CV_64FC1);
        pp[n] = cvCreateMat(D, 1, CV_64FC1);
        pu[n] = cvCreateMat(D, 1, CV_64FC1);   
        v[n] = cvCreateMat(D, 1, CV_64FC1);   
        vu[n] = cvCreateMat(D, 1, CV_64FC1);           
    }   
   
    // initialize the state and particles    
    for (int n = 0; n < N; n++)
    {
        cvmSet(state, 0, 0, 258.0); // center-x
        cvmSet(state, 1, 0, 406.0); // center-y       
        cvmSet(state, 2, 0, 38.0); // radius   
       
//        cvmSet(state, 0, 0, 300.0); // center-x
//        cvmSet(state, 1, 0, 300.0); // center-y       
//        cvmSet(state, 2, 0, 38.0); // radius       
       
        cvmSet(pb[n], 0, 0, cvmGet(state,0,0)); // center-x
        cvmSet(pb[n], 1, 0, cvmGet(state,1,0)); // center-y
        cvmSet(pb[n], 2, 0, cvmGet(state,2,0)); // radius
       
        cvmSet(v[n], 0, 0, 2 * uniform_random()); // center-x
        cvmSet(v[n], 1, 0, 2 * uniform_random()); // center-y
        cvmSet(v[n], 2, 0, 0.1 * uniform_random()); // radius       
       
        w[n] = (double) 1 / (double) N; // equally weighted particle
    }
   
    // initialize the image window
    cvZero(iplImg);   
    cvNamedWindow(titleResult);
   
    cout << "start filtering... " << endl << endl;
   
    float aperture = 3,     thresLow = 50,     thresHigh = 110;   
//    float aperture = 3,     thresLow = 80,     thresHigh = 110;   
    // for each frame
    int frameNo = 0;   
    while(frameNo < frame_count && cvGrabFrame(capture)) {
        // retrieve color frame from the movie "capture"
        iplOriginalColor = cvRetrieveFrame(capture);        
        // convert color pixel values of "iplOriginalColor" to grey scales of "iplOriginalGrey"
        cvCvtColor(iplOriginalColor, iplOriginalGrey, CV_RGB2GRAY);               
        // extract edges with Canny detector from "iplOriginalGrey" to save the results in the image "iplEdge" 
        cvCanny(iplOriginalGrey, iplEdge, thresLow, thresHigh, aperture);

        cvCvtColor(iplEdge, iplEdgeClone, CV_GRAY2BGR);
       
        cvShowImage(titleOriginal, iplOriginalColor);
        cvShowImage(titleEdge, iplEdge);

//        cvZero(iplImg);
       
        cout << "frame # " << frameNo << endl;
       
        double like[N]; // likelihood between measurement and prediction
        double like_sum = 0; // sum of likelihoods
       
        for (int n = 0; n < N; n++) // for "N" particles
        {
            // predict
            double prediction;
            for (int row = 0; row < D; row++)
            {
                prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0)
                            + cvmGet(transition_noise,row,row) * gaussian_random();
                cvmSet(pp[n], row, 0, prediction);
            }
            if ( cvmGet(pp[n],2,0) < 2) { cvmSet(pp[n],2,0,0.0); }
//            cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))),
//             cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);           
            cvCircle(iplEdgeClone, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvRound(cvmGet(pp[n],2,0)), CV_RGB(255, 255, 0));
//            cvCircle(iplImg, cvPoint(iplImg->width *0.5, iplImg->height * 0.5), 100, CV_RGB(255, 255, 0), -1);
//            cvSaveImage("a.bmp", iplImg);

            double cX = cvmGet(pp[n], 0, 0); // predicted center-y of the object
            double cY = cvmGet(pp[n], 1, 0); // predicted center-x of the object
            double cR = cvmGet(pp[n], 2, 0); // predicted radius of the object           

            if ( cR < 0 ) { cR = 0; }
           
            // measure
            // search measurements
            CvPoint2D64f direction [8]; // 8 searching directions
            // define 8 starting points in each direction
            direction[0].x = cX + cR;    direction[0].y = cY;      // East
            direction[2].x = cX;        direction[2].y = cY - cR; // North
            direction[4].x = cX - cR;    direction[4].y = cY;      // West
            direction[6].x = cX;        direction[6].y = cY + cR; // South
            int cD = cvRound( cR/sqrt(2.0) );
            direction[1].x = cX + cD;    direction[1].y = cY - cD; // NE
            direction[3].x = cX - cD;    direction[3].y = cY - cD; // NW
            direction[5].x = cX - cD;    direction[5].y = cY + cD; // SW
            direction[7].x = cX + cD;    direction[7].y = cY + cD; // SE       
           
            CvPoint2D64f search [8];    // searched point in each direction         
            double scale = 0.4;
            double scope [8]; // scope of searching
   
            for ( int i = 0; i < 8; i++ )
            {
//                scope[2*i] = cR * scale;
//                scope[2*i+1] = cD * scale;
                scope[i] = 6.0;
            }
           
            CvPoint d[8];
            d[0].x = 1;        d[0].y = 0; // E
            d[1].x = 1;        d[1].y = -1; // NE
            d[2].x = 0;        d[2].y = 1; // N
            d[3].x = 1;        d[3].y = 1; // NW
            d[4].x = 1;        d[4].y = 0; // W
            d[5].x = 1;        d[5].y = -1; // SW
            d[6].x = 0;        d[6].y = 1; // S
            d[7].x = 1;        d[7].y = 1; // SE           
           
            int count = 0; // number of measurements
            double dist_sum = 0;
           
            for (int i = 0; i < 8; i++) // for 8 directions
            {
                double dist = scope[i] * 1.5;
                for ( int range = 0; range < scope[i]; range++ )
                {
                    int flag = 0;
                    for (int turn = -1; turn <= 1; turn += 2) // reverse the searching direction
                    {
                        search[i].x = direction[i].x + turn * range * d[i].x;
                        search[i].y = direction[i].y + turn * range * d[i].y;
                       
//                        cvCircle(iplImg, cvPoint(cvRound(search[i].x), cvRound(search[i].y)), 2, CV_RGB(0, 255, 0), -1);
//                        cvShowImage(titleResult, iplImg);
//                        cvWaitKey(100);

                        // detect measurements   
//                        CvScalar s = cvGet2D(iplEdge, cvRound(search[i].y), cvRound(search[i].x));
                        unsigned char s = CV_IMAGE_ELEM(iplEdge, unsigned char, cvRound(search[i].y), cvRound(search[i].x));
//                        if ( s.val[0] > 200 && s.val[1] > 200 && s.val[2] > 200 ) // bgr color               
                        if (s > 250) // bgr color                           
                        { // when the pixel value is white, that means a measurement is detected
                            flag = 1;
                            count++;
//                            cvCircle(iplEdgeClone, cvPoint(cvRound(search[i].x), cvRound(search[i].y)), 3, CV_RGB(200, 0, 255));
//                            cvShowImage("3D Particle filter", iplEdgeClone);
//                            cvWaitKey(1);
/*                            // get measurement
                            cvmSet(measurement, 0, 0, search[i].x);
                            cvmSet(measurement, 1, 0, search[i].y);   
                            double dist = distance(measurement, pp[n]);
*/                            // evaluate the difference between predictions of the particle and measurements
                            dist = distanceEuclidean(search[i], direction[i]);
                            break; // break for "turn"
                        } // end if
                    } // for turn
                    if ( flag == 1 )
                    { break; } // break for "range"
                } // for range
               
                dist_sum += dist; // for all searching directions of one particle 

            } // for i direction
           
            double dist_avg; // average distance of measurements from the one particle "n"
//            cout << "count = " << count << endl;
            dist_avg = dist_sum / 8;
//            cout << "dist_avg = " << dist_avg << endl;
           
//            estimate likelihood with "dist_avg"
            like[n] = likelihood(dist_avg, measurement_noise);
//            cout << "likelihood of particle-#" << n << " = " << like[n] << endl;
            like_sum += like[n];   
        } // for n particle
//        cout << "sum of likelihoods of N particles = " << like_sum << endl;
       
        // estimate states       
        double state_x = 0.0;
        double state_y = 0.0;
        double state_r = 0.0;
        // estimate the state with predicted particles
        for (int n = 0; n < N; n++) // for "N" particles
        {
            w[n] = like[n] / like_sum; // update normalized weights of particles           
//            cout << "w" << n << "= " << w[n] << "  ";               
            state_x += w[n] * cvmGet(pp[n], 0, 0); // center-x of the object
            state_y += w[n] * cvmGet(pp[n], 1, 0); // center-y of the object
            state_r += w[n] * cvmGet(pp[n], 2, 0); // radius of the object           
        }
        if (state_r < 0) { state_r = 0; }
        cvmSet(state, 0, 0, state_x);
        cvmSet(state, 1, 0, state_y);       
        cvmSet(state, 2, 0, state_r);
       
        cout << endl << "* * * * * *" << endl;       
        cout << "estimation: (x,y,r) = " << cvmGet(state,0,0) << ",  " << cvmGet(state,1,0)
        << ",  " << cvmGet(state,2,0) << endl;
        cvCircle(iplEdgeClone, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0)) ),
                 cvRound(cvmGet(state,2,0)), CV_RGB(255, 0, 0), 1);

        cvShowImage(titleResult, iplEdgeClone);
        cvWaitKey(1);

   
        // update particles       
        cout << endl << "updating particles" << endl;
        double a[N]; // portion between particles
       
        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
//        cout << "a" << N << "= " << a[N] << "  " << endl;           
       
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            int pselected;
            for (int k = 0; k < N; k++)
            {
                if ( uniform_random() < a[k] )               
                {
                    pselected = k;
                    break;
                }
            }
//            cout << "p " << n << " => " << pselected << "  ";       
           
            // retain the selection 
            for (int row = 0; row < D; row++)
            {
                cvmSet(pu[n], row, 0, cvmGet(pp[pselected],row,0));
                cvSub(pp[pselected], pb[pselected], vu[n]); // pp - pb -> vu
            }
        }
       
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            for (int row = 0; row < D; row++)
            {
                cvmSet(pb[n], row, 0, cvmGet(pu[n],row,0));
                cvmSet(v[n], row , 0, cvmGet(vu[n],row,0));
            }
        }
        cout << endl << endl ;
       
//      cvShowImage(titleResult, iplImg);  
//        cvWaitKey(1000);       
        cvWaitKey(1);       
        frameNo++;
    }
   
    cvWaitKey();   
   
    return 0;
}

// 1-D Particle filter algorithm exercise
// 2009-11-06
// ref. Probabilistic Robotics: 98p

#include <iostream>
#include <cstdlib> //defining RAND_MAX
#include <ctime> //time as a random seed
#include <cmath>
using namespace std;

#define PI 3.14159265
#define N 10 //number of particles

////////////////////////////////////////////////////////////////////////////
// random number generators written by kyu
double uniform_random(void) {
   
    return (double) rand() / (double) RAND_MAX;
   
}

double gaussian_random(void) {
   
    static int next_gaussian = 0;
    static double saved_gaussian_value;
   
    double fac, rsq, v1, v2;
   
    if(next_gaussian == 0) {
       
        do {
            v1 = 2.0 * uniform_random() - 1.0;
            v2 = 2.0 * uniform_random() - 1.0;
            rsq = v1 * v1 + v2 * v2;
        }
        while(rsq >= 1.0 || rsq == 0.0);
        fac = sqrt(-2.0 * log(rsq) / rsq);
        saved_gaussian_value = v1 * fac;
        next_gaussian = 1;
        return v2 * fac;
    }
    else {
        next_gaussian = 0;
        return saved_gaussian_value;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}

////////////////////////////////////////////////////////////////////////////


int main (int argc, char * const argv[]) {
   
    double groundtruth[] = {0.5, 2.0, 3.5, 5.0, 7.0, 8.0, 10.0};
    double measurement[] = {0.4, 2.1, 3.2, 5.3, 7.4, 8.1, 9.6};
    double transition_noise = 0.3; // covariance of Gaussian noise to control
    double measurement_noise = 0.3; // covariance of Gaussian noise to measurement
   
    double x[N]; // N particles
    double x_p[N]; // predicted particles
    double state; // estimated state with particles
    double x_u[N]; // temporary variables for updating particles
    double v[N]; // velocity
    double v_u[N]; // temporary variables for updating velocity   
    double m[N]; // measurement
    double l[N]; // likelihood
    double lsum; // sum of likelihoods
    double w[N]; // weight of each particle
    double a[N]; // portion between particles
   
    double grn[N]; // Gaussian random number
    double urn[N]; // uniform random number
   
    srand(time(NULL));        
   
    // initialize particles
    for (int n = 0; n < N; n++)
    {
        x[n] = 0.0;
        v[n] = 0.0;
        w[n] = (double)1/(double)N;           
    }
   
    int step = 7;
    for (int t = 0; t < step; t++)
    {
        cout << "step " << t << endl;       
        // measure
        m[t] = measurement[t];
       
        cout << "groundtruth = " << groundtruth[t] << endl;
        cout << "measurement = " << measurement[t] << endl;       
       
        lsum = 0;
        for (int n = 0; n < N; n++)
        {
            // predict
            grn[n] = gaussian_random();
            x_p[n] = x[n] + v[n] + transition_noise * grn[n];
//            cout << grn[n] << endl;
           
            // estimate likelihood between measurement and each prediction
            l[n] = normal_distribution(m[t], measurement_noise, x_p[n]); // ref. 14p eqn(2.3)
            lsum += l[n];
        }
//            cout << "lsum = " << lsum << endl;       
       
        // estimate states       
        state = 0;
        for (int n = 0; n < N; n++)
        {
            w[n] = l[n] / lsum; // update normalized weights of particles           
//            cout << "w" << n << "= " << w[n] << "  ";               
            state += w[n] * x_p[n]; // estimate the state with particles
        }   
        cout << "estimation = " << state << endl;
       
        // update       
        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            urn[n] = uniform_random();
            int select;
            for (int k = 0; k < N; k++)
            {
                if (urn[n] < a[k] )
                {
                    select = k;
                    break;
                }
            }
            cout << "select" << n << "= " << select << "  ";       
            // retain the selection 
            x_u[n] = x_p[select];
            v_u[n] = x_p[select] - x[select];
        }
        cout << endl << endl;
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            x[n] = x_u[n];
            v[n] = v_u[n];
//            cout << "v" << n << "= " << v[n] << "  ";   
        }
    }
   
    return 0;
}

// 2-D Particle filter algorithm exercise
// 2009-11-10
// ref. Probabilistic Robotics: 98p

#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <cstdlib> // RAND_MAX
#include <ctime> // time as a random seed
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.14159265
#define N 100 //number of particles

// uniform random number generator
double uniform_random(void) {
   
    return (double) rand() / (double) RAND_MAX;
   
}

// Gaussian random number generator
double gaussian_random(void) {
   
    static int next_gaussian = 0;
    static double saved_gaussian_value;
   
    double fac, rsq, v1, v2;
   
    if(next_gaussian == 0) {
       
        do {
            v1 = 2.0 * uniform_random() - 1.0;
            v2 = 2.0 * uniform_random() - 1.0;
            rsq = v1 * v1 + v2 * v2;
        }
        while(rsq >= 1.0 || rsq == 0.0);
        fac = sqrt(-2.0 * log(rsq) / rsq);
        saved_gaussian_value = v1 * fac;
        next_gaussian = 1;
        return v2 * fac;
    }
    else {
        next_gaussian = 0;
        return saved_gaussian_value;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}
////////////////////////////////////////////////////////////////////////////

// set groundtruth
void set_groundtruth (CvMat* groundtruth, double x, double y)
{
    cvmSet(groundtruth, 0, 0, x); // x-value
    cvmSet(groundtruth, 1, 0, y); // y-value
   
    cout << "groundtruth = " << cvmGet(groundtruth,0,0) << "  " << cvmGet(groundtruth,1,0) << endl;
}


// count the number of detections in measurement process
int count_detections (void)
{
    // set cases of measurement results
    double mtype[4];
    mtype [0] = 0.0;
    mtype [1] = 0.5;
    mtype [2] = mtype[1] + 0.3;
    mtype [3] = mtype[2] + 0.2;
//    cout << "check measurement type3 = " << mtype[3] << endl; // just to check

    // select a measurement case
    double mrn = uniform_random();       
    int type = 1;
    for ( int k = 0; k < 3; k++ )
    {   
        if ( mrn < mtype[k] )
        {
            type = k;
            break;
        }
    }
    return type;
}

// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
    double distance2 = 0;
    double differance = 0;
    for (int u = 0; u < 2; u++)
    {
        differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
        distance2 += differance * differance;
    }
    return sqrt(distance2);
}


// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
   
    CvMat* diff = cvCreateMat(2, 1, CV_64FC1);
    cvSub(sample, mean, diff); // sample - mean -> diff
    CvMat* diff_t = cvCreateMat(1, 2, CV_64FC1);
    cvTranspose(diff, diff_t); // transpose(diff) -> diff_t
    CvMat* cov_inv = cvCreateMat(2, 2, CV_64FC1);
    cvInvert(covariance, cov_inv); // transpose(covariance) -> cov_inv
    CvMat* tmp = cvCreateMat(2, 1, CV_64FC1);
    CvMat* dist = cvCreateMat(1, 1, CV_64FC1);
    cvMatMul(cov_inv, diff, tmp); // cov_inv * diff -> tmp   
    cvMatMul(diff_t, tmp, dist); // diff_t * tmp -> dist
   
    double likeliness = exp( -0.5 * cvmGet(dist, 0, 0) );
    double bound = 0.0000001;
    if ( likeliness < bound )
    {
        likeliness = bound;
    }
    return likeliness;
//    return exp( -0.5 * cvmGet(dist, 0, 0) );
//    return max(0.0000001, exp(-0.5 * cvmGet(dist, 0, 0)));   
   
}



/*
struct particle
{
    double weight; // weight of a particle
    CvMat* loc_p = cvCreateMat(2, 1, CV_64FC1); // previously estimated position of a particle
    CvMat* loc = cvCreateMat(2, 1, CV_64FC1); // currently estimated position of a particle
    CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); // estimated velocity of a particle
    cvSub(loc, loc_p, velocity); // loc - loc_p -> velocity
};
*/


int main (int argc, char * const argv[]) {
   
    srand(time(NULL));

    int width = 400; // width of image window
    int height = 400; // height of image window   
   
    IplImage *iplImg = cvCreateImage(cvSize(width, height), 8, 3);
    cvZero(iplImg);
   
    cvNamedWindow("ParticleFilter-2d", 0);
   
   
     // covariance of Gaussian noise to control
    CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(transition_noise, 0, 0, 3); //set transition_noise(0,0) to 0.1
    cvmSet(transition_noise, 0, 1, 0.0);
    cvmSet(transition_noise, 1, 0, 0.0);
    cvmSet(transition_noise, 1, 1, 6);      
   
    // covariance of Gaussian noise to measurement
    CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(measurement_noise, 0, 0, 2); //set measurement_noise(0,0) to 0.3
    cvmSet(measurement_noise, 0, 1, 0.0);
    cvmSet(measurement_noise, 1, 0, 0.0);
    cvmSet(measurement_noise, 1, 1, 2);  
   
    CvMat* state = cvCreateMat(2, 1, CV_64FC1);
    // declare particles
/*    particle pb[N]; // N estimated particles
    particle pp[N]; // N predicted particles       
    particle pu[N]; // temporary variables for updating particles       
*/
    CvMat* pb [N]; // estimated particles
    CvMat* pp [N]; // predicted particles
    CvMat* pu [N]; // temporary variables to update a particle
    CvMat* v[N]; // estimated velocity of each particle
    CvMat* vu[N]; // temporary varialbe to update the velocity   
    double w[N]; // weight of each particle
    for (int n = 0; n < N; n++)
    {
        pb[n] = cvCreateMat(2, 1, CV_64FC1);
        pp[n] = cvCreateMat(2, 1, CV_64FC1);
        pu[n] = cvCreateMat(2, 1, CV_64FC1);   
        v[n] = cvCreateMat(2, 1, CV_64FC1);   
        vu[n] = cvCreateMat(2, 1, CV_64FC1);           
    }   
   
    // initialize particles and the state
    for (int n = 0; n < N; n++)
    {
        w[n] = (double) 1 / (double) N; // equally weighted
        for (int row=0; row < 2; row++)
        {
            cvmSet(pb[n], row, 0, 0.0);
            cvmSet(v[n], row, 0, 15 * uniform_random());
            cvmSet(state, row, 0, 0.0);           
        }
    }
   
    // set the system   
    CvMat* groundtruth = cvCreateMat(2, 1, CV_64FC1); // groundtruth of states   
    CvMat* measurement [3]; // measurement of states
    for (int k = 0; k < 3; k++) // 3 types of measurement
    {
        measurement[k] = cvCreateMat(2, 1, CV_64FC1);
    }
   
    cout << "start filtering... " << endl << endl;
    int step = 30; //30; // timestep
   
    for (int t = 0; t < step; t++) // for "step" steps
    {
//        cvZero(iplImg);
        cout << "step " << t << endl;
       
        // set groundtruth
        double gx = 10 * t;
        double gy = (-1.0 / width ) * (gx - width) * (gx - width) + height;
        set_groundtruth(groundtruth, gx, gy);

        // set measurement types
        double c1 = 1.0, c2 = 4.0;   
        // measured point 1
        cvmSet(measurement[0], 0, 0, gx + (c1 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
        cvmSet(measurement[0], 1, 0, gy + (c1 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
        // measured point 2
        cvmSet(measurement[1], 0, 0, gx + (c2 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
        cvmSet(measurement[1], 1, 0, gy + (c2 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
        // measured point 3 // clutter noise
        cvmSet(measurement[2], 0, 0, width*uniform_random()); // x-value
        cvmSet(measurement[2], 1, 0, height*uniform_random()); // y-value       

        // count the number of measurements       
        int count = count_detections(); // number of detections
        cout << "# of measured points = " << count << endl;
   
        // get measurement           
        for (int index = 0; index < count; index++)
        {
            cout << "measurement #" << index << " : "
                << cvmGet(measurement[index],0,0) << "  " << cvmGet(measurement[index],1,0) << endl;
           
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement[index],0,0)), cvRound(cvmGet(measurement[index],1,0))), 4, CV_RGB(200, 0, 255), 1);
           
        }
       
       
        double like[N]; // likelihood between measurement and prediction
        double like_sum = 0; // sum of likelihoods
       
        for (int n = 0; n < N; n++) // for "N" particles
        {
            // predict
            double prediction;
            for (int row = 0; row < 2; row++)
            {
               
                prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0) + cvmGet(transition_noise,row,row) * gaussian_random();
                cvmSet(pp[n], row, 0, prediction);
            }
           
            cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);           
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), 1, CV_RGB(255, 255, 0));

           
           
            // evaluate measurements
            double range = (double) width; // range to search measurements for each particle
//            cout << "range of distances = " << range << endl;
            int mselected;
            for (int index = 0; index < count; index++)
            {
                double d = distance(measurement[index], pp[n]);
               
                if ( d < range )
                {
                    range = d;
                    mselected = index; // selected measurement
                }
            }
///            cout << "selected measurement # = " << mselected << endl;
            like[n] = likelihood(measurement[mselected], measurement_noise, pp[n]);   
       
///            cout << "likelihood of #" << n << " = " << like[n] << endl;
           
            like_sum += like[n];
        }
       
///        cout << "sum of likelihoods = " << like_sum << endl;
       
        // estimate states       
        double state_x = 0.0;
        double state_y = 0.0;
   
        // estimate the state with predicted particles
        for (int n = 0; n < N; n++) // for "N" particles
        {
            w[n] = like[n] / like_sum; // update normalized weights of particles           
///            cout << "w" << n << "= " << w[n] << "  ";               
            state_x += w[n] * cvmGet(pp[n], 0, 0); // x-value
            state_y += w[n] * cvmGet(pp[n], 1, 0); // y-value
        }
        cvmSet(state, 0, 0, state_x);
        cvmSet(state, 1, 0, state_y);       
       
        cout << endl << "* * * * * *" << endl;       
        cout << "estimation = " << cvmGet(state,0,0) << "  " << cvmGet(state,1,0) << endl;
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 4, CV_RGB(0, 255, 200), 2);
   
        // update particles       
        cout << endl << "updating particles" << endl;
        double urn[N]; // uniform random number
        double a[N]; // portion between particles

        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
//        cout << "a" << N << "= " << a[N] << "  " << endl;           
       
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            urn[n] = uniform_random();
            int pselected;
            for (int k = 0; k < N; k++)
            {
                if (urn[n] < a[k] )
                {
                    pselected = k;
                    break;
                }
            }
///            cout << "particle " << n << " => " << pselected << "  ";       
            // retain the selection 
            cvmSet(pu[n], 0, 0, cvmGet(pp[pselected],0,0)); // x-value
            cvmSet(pu[n], 1, 0, cvmGet(pp[pselected],1,0)); // y-value
           
            cvSub(pp[pselected], pb[pselected], vu[n]); // pp - pb -> vu
   
        }
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            for (int row = 0; row < 2; row++)
            {
                cvmSet(pb[n], row, 0, cvmGet(pu[n],row,0));
                cvmSet(v[n], row , 0, cvmGet(vu[n],row,0));
            }
        }
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(groundtruth,0,0)), cvRound(cvmGet(groundtruth,1,0))), 4, cvScalarAll(255), 1);
       
        cout << endl << endl ;
       
        cvShowImage("ParticleFilter-2d", iplImg);
        cvWaitKey(1000);   
       
       
       
       
    } // for "t"
   
   
    cvWaitKey();   
   
    return 0;
}

// 2-D Particle filter algorithm exercise
// lym, VIP Lab, Sogang Univ.
// 2009-11-23
// ref. Probabilistic Robotics: 98p

#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <cstdlib> // RAND_MAX
#include <ctime> // time as a random seed
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.14159265
#define N 100 //number of particles

int width = 400; // width of image window
int height = 400; // height of image window   
IplImage *iplImg = cvCreateImage(cvSize(width, height), 8, 3);

// uniform random number generator
double uniform_random(void) {
   
    return (double) rand() / (double) RAND_MAX;
   
}

// Gaussian random number generator
double gaussian_random(void) {
   
    static int next_gaussian = 0;
    static double saved_gaussian_value;
   
    double fac, rsq, v1, v2;
   
    if(next_gaussian == 0) {
       
        do {
            v1 = 2.0 * uniform_random() - 1.0;
            v2 = 2.0 * uniform_random() - 1.0;
            rsq = v1 * v1 + v2 * v2;
        }
        while(rsq >= 1.0 || rsq == 0.0);
        fac = sqrt(-2.0 * log(rsq) / rsq);
        saved_gaussian_value = v1 * fac;
        next_gaussian = 1;
        return v2 * fac;
    }
    else {
        next_gaussian = 0;
        return saved_gaussian_value;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}
////////////////////////////////////////////////////////////////////////////

// set groundtruth
void get_groundtruth (CvMat* groundtruth, double x, double y)
{
    cvmSet(groundtruth, 0, 0, x); // x-value
    cvmSet(groundtruth, 1, 0, y); // y-value
   
    cout << "groundtruth = " << cvmGet(groundtruth,0,0) << "  " << cvmGet(groundtruth,1,0) << endl;
    cvCircle(iplImg, cvPoint(cvRound(cvmGet(groundtruth,0,0)), cvRound(cvmGet(groundtruth,1,0))), 2, cvScalarAll(255), 2);   
}


// count the number of detections in measurement process
int count_detections (void)
{
    // set cases of measurement results
    double mtype[4];
    mtype [0] = 0.0;
    mtype [1] = 0.5;
    mtype [2] = mtype[1] + 0.3;
    mtype [3] = mtype[2] + 0.2;
    //    cout << "check measurement type3 = " << mtype[3] << endl; // just to check
   
    // select a measurement case
    double mrn = uniform_random();       
    int type = 1;
    for ( int k = 0; k < 3; k++ )
    {   
        if ( mrn < mtype[k] )
        {
            type = k;
            break;
        }
    }
    return type;
}

// get measurements
int get_measurement (CvMat* measurement[], CvMat* measurement_noise, double x, double y)
{
    // set measurement types
    double c1 = 1.0, c2 = 4.0;   
    // measured point 1
    cvmSet(measurement[0], 0, 0, x + (c1 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
    cvmSet(measurement[0], 1, 0, y + (c1 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
    // measured point 2
    cvmSet(measurement[1], 0, 0, x + (c2 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
    cvmSet(measurement[1], 1, 0, y + (c2 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
    // measured point 3 // clutter noise
    cvmSet(measurement[2], 0, 0, width*uniform_random()); // x-value
    cvmSet(measurement[2], 1, 0, height*uniform_random()); // y-value       

    // count the number of measured points   
    int number = count_detections(); // number of detections
    cout << "# of measured points = " << number << endl;

    // get measurement           
    for (int index = 0; index < number; index++)
    {
        cout << "measurement #" << index << " : "
        << cvmGet(measurement[index],0,0) << "  " << cvmGet(measurement[index],1,0) << endl;
       
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement[index],0,0)), cvRound(cvmGet(measurement[index],1,0))), 4, CV_RGB(255, 0, 255), 2);           
    }

    return number;
}


// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
    double distance2 = 0;
    double differance = 0;
    for (int u = 0; u < 2; u++)
    {
        differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
        distance2 += differance * differance;
    }
    return sqrt(distance2);
}


// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
   
    CvMat* diff = cvCreateMat(2, 1, CV_64FC1);
    cvSub(sample, mean, diff); // sample - mean -> diff
    CvMat* diff_t = cvCreateMat(1, 2, CV_64FC1);
    cvTranspose(diff, diff_t); // transpose(diff) -> diff_t
    CvMat* cov_inv = cvCreateMat(2, 2, CV_64FC1);
    cvInvert(covariance, cov_inv); // transpose(covariance) -> cov_inv
    CvMat* tmp = cvCreateMat(2, 1, CV_64FC1);
    CvMat* dist = cvCreateMat(1, 1, CV_64FC1);
    cvMatMul(cov_inv, diff, tmp); // cov_inv * diff -> tmp   
    cvMatMul(diff_t, tmp, dist); // diff_t * tmp -> dist
   
    double likeliness = exp( -0.5 * cvmGet(dist, 0, 0) );
    double bound = 0.0000001;
    if ( likeliness < bound )
    {
        likeliness = bound;
    }
    return likeliness;
    //    return exp( -0.5 * cvmGet(dist, 0, 0) );
    //    return max(0.0000001, exp(-0.5 * cvmGet(dist, 0, 0)));   
}


int main (int argc, char * const argv[]) {
   
    srand(time(NULL));

    // set the system   
    CvMat* state = cvCreateMat(2, 1, CV_64FC1);    // state of the system to be estimated
    CvMat* groundtruth = cvCreateMat(2, 1, CV_64FC1); // groundtruth of states   
    CvMat* measurement [3]; // measurement of states
    for (int k = 0; k < 3; k++) // 3 types of measurement
    {
        measurement[k] = cvCreateMat(2, 1, CV_64FC1);
    }   

    // declare particles
    CvMat* pb [N]; // estimated particles
    CvMat* pp [N]; // predicted particles
    CvMat* pu [N]; // temporary variables to update a particle
    CvMat* v[N]; // estimated velocity of each particle
    CvMat* vu[N]; // temporary varialbe to update the velocity   
    double w[N]; // weight of each particle
    for (int n = 0; n < N; n++)
    {
        pb[n] = cvCreateMat(2, 1, CV_64FC1);
        pp[n] = cvCreateMat(2, 1, CV_64FC1);
        pu[n] = cvCreateMat(2, 1, CV_64FC1);   
        v[n] = cvCreateMat(2, 1, CV_64FC1);   
        vu[n] = cvCreateMat(2, 1, CV_64FC1);           
    }   
   
    // initialize the state and particles
    for (int n = 0; n < N; n++)
    {
        w[n] = (double) 1 / (double) N; // equally weighted
        for (int row=0; row < 2; row++)
        {
            cvmSet(state, row, 0, 0.0);   
            cvmSet(pb[n], row, 0, 0.0);
            cvmSet(v[n], row, 0, 15 * uniform_random());
        }
    }
   
    // set the process noise
    // covariance of Gaussian noise to control
    CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(transition_noise, 0, 0, 3); //set transition_noise(0,0) to 3.0
    cvmSet(transition_noise, 0, 1, 0.0);
    cvmSet(transition_noise, 1, 0, 0.0);
    cvmSet(transition_noise, 1, 1, 6);      
   
    // set the measurement noise
    // covariance of Gaussian noise to measurement
    CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(measurement_noise, 0, 0, 2); //set measurement_noise(0,0) to 2.0
    cvmSet(measurement_noise, 0, 1, 0.0);
    cvmSet(measurement_noise, 1, 0, 0.0);
    cvmSet(measurement_noise, 1, 1, 2);  
   
    // initialize the image window
    cvZero(iplImg);   
    cvNamedWindow("ParticleFilter-3d", 0);

    cout << "start filtering... " << endl << endl;
    int step = 30; //30; // timestep
   
    for (int t = 0; t < step; t++) // for "step" steps
    {
//        cvZero(iplImg);
        cout << "step " << t << endl;
       
        // get the groundtruth
        double gx = 10 * t;
        double gy = (-1.0 / width ) * (gx - width) * (gx - width) + height;
        get_groundtruth(groundtruth, gx, gy);
        // get measurements
        int count = get_measurement(measurement, measurement_noise, gx, gy);
       
        double like[N]; // likelihood between measurement and prediction
        double like_sum = 0; // sum of likelihoods
       
        for (int n = 0; n < N; n++) // for "N" particles
        {
            // predict
            double prediction;
            for (int row = 0; row < 2; row++)
            {
                prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0) + cvmGet(transition_noise,row,row) * gaussian_random();
                cvmSet(pp[n], row, 0, prediction);
            }
//            cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);           
//            cvCircle(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), 1, CV_RGB(255, 255, 0));
           
            // evaluate measurements
            double range = (double) width; // range to search measurements for each particle
//            cout << "range of distances = " << range << endl;
            int mselected;
            for (int index = 0; index < count; index++)
            {
                double d = distance(measurement[index], pp[n]);
               
                if ( d < range )
                {
                    range = d;
                    mselected = index; // selected measurement
                }
            }
//            cout << "selected measurement # = " << mselected << endl;
            like[n] = likelihood(measurement[mselected], measurement_noise, pp[n]);   
//            cout << "likelihood of #" << n << " = " << like[n] << endl;           
            like_sum += like[n];
        }
//        cout << "sum of likelihoods = " << like_sum << endl;
       
        // estimate states       
        double state_x = 0.0;
        double state_y = 0.0;
        // estimate the state with predicted particles
        for (int n = 0; n < N; n++) // for "N" particles
        {
            w[n] = like[n] / like_sum; // update normalized weights of particles           
//            cout << "w" << n << "= " << w[n] << "  ";               
            state_x += w[n] * cvmGet(pp[n], 0, 0); // x-value
            state_y += w[n] * cvmGet(pp[n], 1, 0); // y-value
        }
        cvmSet(state, 0, 0, state_x);
        cvmSet(state, 1, 0, state_y);       
       
        cout << endl << "* * * * * *" << endl;       
        cout << "estimation = " << cvmGet(state,0,0) << "  " << cvmGet(state,1,0) << endl;
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 4, CV_RGB(0, 255, 200), 2);
       
        // update particles       
        cout << endl << "updating particles" << endl;
        double a[N]; // portion between particles
       
        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
//        cout << "a" << N << "= " << a[N] << "  " << endl;           
       
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            int pselected;
            for (int k = 0; k < N; k++)
            {
                if ( uniform_random() < a[k] )               
                {
                    pselected = k;
                    break;
                }
            }
//            cout << "p " << n << " => " << pselected << "  ";       

            // retain the selection 
            cvmSet(pu[n], 0, 0, cvmGet(pp[pselected],0,0)); // x-value
            cvmSet(pu[n], 1, 0, cvmGet(pp[pselected],1,0)); // y-value               
            cvSub(pp[pselected], pb[pselected], vu[n]); // pp - pb -> vu
        }
       
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            for (int row = 0; row < 2; row++)
            {
                cvmSet(pb[n], row, 0, cvmGet(pu[n],row,0));
                cvmSet(v[n], row , 0, cvmGet(vu[n],row,0));
            }
        }
        cout << endl << endl ;
       
        cvShowImage("ParticleFilter-3d", iplImg);
        cvWaitKey(1000);   
       
    } // for "t"
   
    cvWaitKey();   
   
    return 0;
}

SLAM

mobile robotics

the problem of building a map of an unknown environment from a sequence of noisy landmark measurements obtained from a moving robot + a robot localization problem => SLAM

autonomous robots operating in environments where precise maps and positioning are not available

Extended Kalman Filter (EKF)
: used for incrementally estimating the joint posterior distribution over robot pose and landmark positions

limitations of EKF
1) Quadratic complexity (: sensor updates require time quadratic in the total number of landmarks in the map)
=> limiting the number of landmarks to only a few hundred (whereas natural environment models frequently contain millions of features
2) Data association / correspondence (: mapping of observations to landmarks)
=> associating a small numver of observations with incorrect landmarks to cause the filter to diverge

FastSLAM factors the SLAM posterior into a localization problem and K independent landmark estimation problems conditioned on the robot pose estimate.

> a modified particle filter to estimate the posterior over robot paths
> each particle possessing K independent Kalman filters that estimate the landmark locations conditioned on the particle's path
=> an instance of the Rao-Blackwellized particle filter

Representing particles as binary trees of Kalman Filters
-> Incorporating observations into FastSLAM in O(MlogK) time  (M, # of particles; K, # of landmarks)

Each particle represents a different robot pose hypothesis.
=> Data association can be considered separately for every particle.
=> 1) The noise of robot motion does not affect the accuracy of data association.
2) Incorrectly associated particls will receive lower probabilities and will be removed in future resampling steps.

On real-world data with ambiguous data association
Adding extra odometric noise
( Odometry is the use of data from the movement of actuators to estimate change in position over time. )
Estimating an accurate map without any odometry in the environment in which the Kalman Filter inevitably diverges.
How to incorporate negative information resulting in a measurable increase in the accuracy of the resulting map

monocular SLAM
particle filter + top-down search => real-time, large number  of landmarks

the first to apply this FastSLAM-type particle filter to single-camera SLAM