돛단배

void cvGEMM(const CvArr* src1, const CvArr* src2, double alpha, const CvArr* src3, double beta, CvArr* dst, int tABC=0)

define cvMatMulAdd(src1, src2, src3, dst ) cvGEMM(src1, src2, 1, src3, 1, dst, 0 )define cvMatMul(src1, src2, dst ) cvMatMulAdd(src1, src2, 0, dst)¶

// 1-D SLAM with Kalman Filter
// VIP lab, Sogang University
// 2010-01-14
// ref. Probabilistic Robotics 42p

#include <OpenCV/OpenCV.h> // matrix operations

#include <iostream>
#include <iomanip>
#include <cmath>
using namespace std;

#define num_landmarks 10
#define num_dim (num_landmarks + 2)

#define step 100
#define width 1000
#define height 200

int main (int argc, char * const argv[]) {
   
    srand(time(NULL));
   
    // ground truth of num_landmarks landmarks in the world coordinate
    double landmark[num_landmarks];//    = { 200, 600, 400 }; // x-position
    for( int n = 0; n < num_landmarks; n++ )
    {
        landmark[n] = width * uniform_random();
    }
   
    // set the initial state
    double rob_pos = 25.0; // initial robot position
    double rob_vel = 10.0; // initial robot velocity
    // set the initial covariance of the state
    double rob_pos_cov = 0.01; // covariance of noise to robot position
    double rob_vel_cov = 0.01; // covariance of noise to robot velocity
    double obs_cov = 900; // covarriance of noise to measurement of landmarks
    double xGroundTruth, vGroundTruth;
    xGroundTruth = rob_pos;
    vGroundTruth = rob_vel;
   

   
    IplImage *iplImg = cvCreateImage(cvSize(width, height) , 8, 3);
    cvZero(iplImg);
   
    cvNamedWindow("SLAM-1d", 0);
   
    // H matrix
    int Hrow = num_landmarks;
    int Hcol = num_landmarks + 2;
    CvMat* H = cvCreateMat(Hrow, Hcol, CV_64FC1);
    cvZero(H); // initialize H matrix   
    // set H matrix
    for (int row = 0; row < Hrow; row++)
    {
        cvmSet(H, row, 0, -1.0);
        cvmSet(H, row, row+2, 1.0);
    }   
    displayMatrix(H, "H matrix");
   
    // Q matrix ; covariance of noise to H ; uncertainty of control
    CvMat* Q = cvCreateMat(Hrow, Hrow, CV_64FC1);    
    cvZero(Q); // initialize Q matrix
    // set Q matrix
    for (int row = 0; row < Q->rows; row++)
    {
        cvmSet(Q, row, row, obs_cov);
    }
    displayMatrix(Q, "Q matrix");   
   
    // G matrix // transition
    int Grow = num_landmarks + 2;
    int Gcol = Grow;
    CvMat* G = cvCreateMat(Grow, Gcol, CV_64FC1);
    cvZero(G); // initialize G matrix
    // set G matrix
    cvmSet(G, 0, 0, 1.0); // previous position
    cvmSet(G, 0, 1, 1.0); // velocity   
    for (int row = 1; row < Grow; row++)
    {
        cvmSet(G, row, row, 1.0); // constance of velocity
    }
    displayMatrix(G, "G matrix");
   
    // R matrix ; covariance of noise to H ; uncertainty of observation
    CvMat* R = cvCreateMat(Grow, Grow, CV_64FC1); // 5x5
    cvZero(R); // initialize R matrix
    // set R matrix
    cvmSet(R, 0, 0, rob_pos_cov);
    cvmSet(R, 1, 1, rob_vel_cov);   
    displayMatrix(R, "R matrix");   
   
    CvMat* mu = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
    CvMat* rob_ground = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
    CvMat* mu_p = cvCreateMat(num_dim, 1, CV_64FC1); // state to be predicted
    CvMat* u = cvCreateMat(1, 1, CV_64FC1); // control vector    
    cvmSet(u, 0, 0, 1.0); // set u(0,0) to 1.0, the constant velocity here
    CvMat* sigma = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
    CvMat* sigma_p = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
    CvMat* z = cvCreateMat(num_landmarks, 1, CV_64FC1); // measurement vector
    CvMat* K = cvCreateMat(num_dim, num_landmarks, CV_64FC1); // K matrix // Kalman gain
   
    CvMat* delta = cvCreateMat(z->rows, 1, CV_64FC1); // measurement noise (ref. 42p: (3.5))   
    CvMat* obs = cvCreateMat(num_landmarks, 1, CV_64FC1); // observation for each landmark
   
    // initialize "mu" vector
    cvmSet(mu, 0, 0, rob_pos + sqrt(rob_pos_cov)*gaussian_random()); // set mu(0,0) to "rob_pos"
    cvmSet(mu, 1, 0, rob_vel + sqrt(rob_vel_cov)*gaussian_random()); // set mu(0,0) to "rob_vel"   
    for(int n = 0; n < num_landmarks; n++)
    {
//        cvmSet(mu, n+2, 0, landmark[n] + sqrt(obs_cov)*gaussian_random());
        cvmSet(mu, n+2, 0, landmark[n]);       
    }   
    displayMatrix(mu, "mu vector");
   
    // initialize "sigma" matrix <-- This is the most critical point in tuning
    cvSetIdentity(sigma, cvRealScalar(obs_cov));       
    displayMatrix(sigma, "sigma matrix");
   
    // matrices to be used in calculation
    CvMat* Hx = cvCreateMat(H->rows, mu->cols, CV_64FC1); // num_landmarksx5 * 5x1
    CvMat* Gt = cvCreateMat(G->cols, G->rows, CV_64FC1); // 5x5
    cvTranspose(G, Gt); // transpose(G) -> Gt   
    CvMat* sigmaGt = cvCreateMat(sigma->rows, G->rows, CV_64FC1); // 5x5 * 5x5
    CvMat* GsigmaGt = cvCreateMat(G->rows, G->rows, CV_64FC1); // 5x5
   
    CvMat* Ht = cvCreateMat(H->cols, H->rows, CV_64FC1); // 5xnum_landmarks
    cvTranspose(H, Ht); // transpose(H) -> Ht
    CvMat* sigmaHt = cvCreateMat(sigma->rows, H->rows, CV_64FC1);    // 5x5 * 5xnum_landmarks
    CvMat* HsigmaHt = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks   
    CvMat* HsigmaHtplusQ = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks   
   
    CvMat* invGain = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks   
    CvMat* sigmapHt = cvCreateMat(sigma_p->rows, Ht->cols, CV_64FC1); // 5x5 * 5xnum_landmarks    
   
    CvMat* Hmu = cvCreateMat(H->rows, mu->cols, CV_64FC1); // num_landmarksx5 * 5x1
    CvMat* miss = cvCreateMat(Hmu->rows, 1, CV_64FC1); // num_landmarksx1
    CvMat* adjust = cvCreateMat(mu->rows, 1, CV_64FC1); // 5x1
   
    CvMat* KH = cvCreateMat(K->rows, H->cols, CV_64FC1); // 5xnum_landmarks * num_landmarksx5
    CvMat* I = cvCreateMat(KH->rows, KH->cols, CV_64FC1); // 5x5 identity matrix
    cvSetIdentity(I);       
    CvMat* change = cvCreateMat(I->rows, I->cols, CV_64FC1); // 5x5

   
    for (int t = 0; t < step; t++)
    {
        cout << endl << "step " << t << endl;       
        cvZero(iplImg);
   
        // predict
        // predict the state (ref. L2, KF algorithm, 42p)
        cvMatMul(G, mu, mu_p); // G * mu -> mu_p
//        displayMatrix(mu_p, "mu_p vector");   
       
        // predict the covariance of the state (ref. L3, KF algorithm, 42p)
        cvMatMul(sigma, Gt, sigmaGt); // sigma * Gt -> sigmaGt
        cvMatMul(G, sigmaGt, GsigmaGt); // G * sigmaGt -> GsigmaGt
        cvAdd(GsigmaGt, R, sigma_p); // GsigmaGt + R -> sigma_p
//        displayMatrix(sigma_p, "sigma_p matrix");
       
        // estimate Kalman gain (ref. L4, KF algorithm, 42p)
        cvMatMul(sigma_p, Ht, sigmaHt); // sigma_p * Ht -> sigmaHt
        cvMatMul(H, sigmaHt, HsigmaHt); // H * sigmaHt -> HsigmaHt
        cvAdd(HsigmaHt, Q, HsigmaHtplusQ); // HsigmaHt + Q -> HsigmaHtplusQ
    //    displayMatrix(HsigmaHtplusQ, "H*sigma*Ht + Q matrix");
       
        cvInvert(HsigmaHtplusQ, invGain); // inv(HsigmaHtplusQ) -> invGain
        displayMatrix(invGain, "invGain matrix");
       
        cvMatMul(sigma_p, Ht, sigmapHt); // sigma_p * Ht -> sigmapHt
        cvMatMul(sigmapHt, invGain, K); // sigmapHt * invGain -> K
    //    displayMatrix(K, "K matrix");       

        // measure
        xGroundTruth += vGroundTruth;
        cvZero(rob_ground);
        cvmSet(rob_ground, 0, 0, xGroundTruth);
        cvmSet(rob_ground, 1, 0, vGroundTruth);
        for( int n = 0; n < num_landmarks; n++ )
        {
            cvmSet(rob_ground, n + 2, 0, landmark[n]);
        }

        for(int n = 0; n < num_landmarks; n++)
        {
            double rn = sqrt(obs_cov) * gaussian_random();
            cvmSet(delta, n, 0, rn);
        }
    //    displayMatrix(delta, "delta vector; measurement noise");
        displayMatrix(rob_ground, "rob_ground");

        cvMatMul(H, rob_ground, Hx); // H * rob_ground -> Hx
        cvAdd(Hx, delta, z); // Hx + delta -> z
        displayMatrix(z, "z vector");
       
        // update the state with Kalman gain (ref. L5, KF algorithm, 42p)
        cvMatMul(H, mu_p, Hmu); // H * mu_p -> Hmu
        cvSub(z, Hmu, miss); // z - Hmu -> miss
        cvMatMul(K, miss, adjust); // K * miss -> adjust
        cvAdd(mu_p, adjust, mu); // mu_p + adjust -> mu
        displayMatrix(mu, "mu vector");
       
        // update the coariance of the state (ref. L6, KF algorith, 42p)
        cvMatMul(K, H, KH); // K * H -> KH
        cvSub(I, KH, change); // I - KH -> change
        cvMatMul(change, sigma_p, sigma); // change * sigma_p -> sigma
        displayMatrix(sigma, "sigma matrix");

        // result in console
        cout << "landmarks  = " << landmark[0] << setw(10) << landmark[1] << setw(10) << landmark[2] << setw(10) << endl;
        cout << "robot position = " << cvmGet(mu, 0, 0) << endl;
//        cout << "measurement = " << cvmGet(z,0,0) << setw(10) << cvmGet(z,1,0) << setw(10) << cvmGet(z,2,0) << endl;   
        for( int n = 0; n < num_landmarks; n++ )
        {
            cvmSet(obs, n, 0, cvmGet(mu,0,0) + cvmGet(z,n,0));
        }
        cout << "observation = " << cvmGet(obs,0,0) << setw(10) << cvmGet(obs,1,0) << setw(10) << cvmGet(obs,2,0) << endl;
        cout<< "estimation = " << cvmGet(mu,2,0) << setw(10) << cvmGet(mu,3,0) << setw(10) << cvmGet(mu,4,0) << endl;

        // result in image
        // ground truth of robot position       
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(rob_ground,0,0)), cvRound(height/2)), 1, cvScalar(100, 0, 255));
        // robot position, purple
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,0,0)), cvRound(height/2)), 3, cvScalar(255, 0, 100));
        // uncertainty of robot position, purple line
        cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,0,0))-sqrt(cvmGet(sigma,0,0)), cvRound(height/2)),
                       cvPoint(cvRound(cvmGet(mu,0,0))+sqrt(cvmGet(sigma,0,0)), cvRound(height/2)), cvScalar(255, 0, 100), 1);
       
        for( int index = 0; index < num_landmarks; index++    )
        { 
            // landmarks, white
            cvCircle(iplImg, cvPoint(cvRound(landmark[index]), cvRound(height/2)), 3, cvScalarAll(255));
            // observation, yellow
//            cvCircle(iplImg, cvPoint(cvRound(cvmGet(obs,index,0)), cvRound(height/2)), 2, cvScalar(0, 200, 255));
            // estimation, green
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0)), cvRound(height/2)), 2, cvScalar(50, 255, 0));
            // uncertainty of estimation, green line
            cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0))-sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)),
                   cvPoint(cvRound(cvmGet(mu,index+2,0))+sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)), cvScalar(50, 255, 0), 1);

        }
   
        cvShowImage("SLAM-1d", iplImg);
        cvWaitKey(0);
       
    }
    cvWaitKey();   
   
    return 0;
}

// 2-D SLAM with Kalman Filter
// VIP lab, Sogang University
// 2010-01-18
// ref. Probabilistic Robotics 42p

#include <OpenCV/OpenCV.h> // matrix operations

#include <iostream>
#include <iomanip>
using namespace std;

#define num_landmarks 20
#define num_dim ( 2 * (num_landmarks + 2) )

#define step 200
#define width 800
#define height 600


// 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;
    }
}


void displayMatrix(CvMat *mat, char *title = NULL) {
    if(title) cout << title << endl;
    for(int iR = 0; iR < mat->rows; iR++) {
        for(int iC = 0; iC < mat->cols; iC++) {
            printf("%.2f ", cvmGet(mat, iR, iC));
        }
        printf("\n");
    }
    printf("\n");
    return;
} 


void draw2DEllipseFromCovariance
(CvMat* cov, CvPoint* cnt, IplImage *iplImg, CvScalar* curveColor /*= cvScalarAll(255)*/, CvScalar* centerColor /*= cvScalarAll(128) */, int thickness /*= 1*/)
{
   
    if(NULL == cov || 2 != cov->rows || 2 != cov->cols) {
        printf("covariance matrix is not 2x2 !! \n");
        exit(0);
    }
    double eigenvalues[6], eigenvectors[36]; 
    float ev1, ev2, vx, vy, angle;
   
    CvSize axes;
    CvMat evals = cvMat(1, 2, CV_64F, eigenvalues), evecs = cvMat(2, 2, CV_64F, eigenvectors);
   
    cvSVD(cov, &evals, &evecs, 0, CV_SVD_MODIFY_A + CV_SVD_U_T ); 
   
    ev1 = cvmGet(&evals, 0, 0);        ev2 = cvmGet(&evals, 0, 1);
   
    if( ev1 < 0 && ev2 < 0 ) {
        ev1 = -ev1;
        ev2 = -ev2;
    }
    if( ev1 < ev2 ) {
        float tmp = ev1;
        ev1 = ev2;
        ev2 = tmp;
    }
    if( ev1 <= 0 || ev2 <= 0 ) {
        printf("COV Eigenvalue is negativ or zero(!)\n");
        exit(0);
    }
   
    // calc angle 
    angle = (float)(180 - atan2(eigenvectors[2], eigenvectors[3]) * 180 / CV_PI); 
   
    axes = cvSize(cvRound(sqrt(ev1)), cvRound(sqrt(ev2)));
    (float)(180 - atan2(eigenvectors[2], eigenvectors[3]) * 180 / CV_PI);
    cvEllipse(iplImg, *cnt, axes, angle, 0, 360, *curveColor, thickness);
   
    cvLine(iplImg, cvPoint(cnt->x - 1, cnt->y - 1), cvPoint(cnt->x + 2, cnt->y + 1), *centerColor, 1);
    cvLine(iplImg, cvPoint(cnt->x - 1, cnt->y + 1), cvPoint(cnt->x + 2, cnt->y - 1), *centerColor, 1);
   
}


int main (int argc, char * const argv[]) {

    srand(time(NULL));
   
    // set the initial state
    double rob_x = width * 0.1; // robot's initial x-position
    double rob_y = height * 0.4; // robot's initial y-position   
    double rob_vx = 10.0; // robot's initial x-velocity
    double rob_vy = 10.0; // robot's initial y-velocity   
   
    // set the initial covariance of the state uncertainty
    double rob_px_cov = 0.01; // covariance of noise to robot's x-position
    double rob_py_cov = 0.01; // covariance of noise to robot's y-position   
    double rob_vx_cov = 0.01; // covariance of noise to robot's x-velocity
    double rob_vy_cov = 0.01; // covariance of noise to robot's y-velocity   
   
    // set the initial covariance of the measurement noise
    double obs_x_cov = 900; // covarriance of noise to x-measurement of landmarks
    double obs_y_cov = 900; // covarriance of noise to y-measurement of landmarks
   
    // ground truth of state
    double xGroundTruth = rob_x;
    double yGroundTruth = rob_y;
    double vxGroundTruth = rob_vx;
    double vyGroundTruth = rob_vy;
   
    // ground truth of num_landmarks landmarks in the world coordinate
    double landmark[2*num_landmarks];  
    for( int n = 0; n < num_landmarks; n++ )
    {
        landmark[2*n] = width * uniform_random();
        landmark[2*n+1] = height * uniform_random();
    }   
   
    IplImage *iplImg = cvCreateImage(cvSize(width, height) , 8, 3);
    cvZero(iplImg);
   
    cvNamedWindow("SLAM-2d");
   
    // H matrix, measurement matrix
    CvMat* H = cvCreateMat(2*num_landmarks, num_dim, CV_64FC1);
    cvZero(H); // initialize H matrix   
    // set H matrix
    for (int r = 0; r < num_landmarks; r++)
    {
        cvmSet(H, 2*r, 0, -1.0); // robot's x-position
        cvmSet(H, 2*r, 2*r+4, 1.0); // landmark's x-position
        cvmSet(H, 2*r+1, 1, -1.0); // robot's y-position
        cvmSet(H, 2*r+1, 2*r+5, 1.0); // landmarks's y-position        
    }   
    displayMatrix(H, "H matrix");
   
    // Q matrix ; covariance of noise to H; uncertainty of control
    CvMat* Q = cvCreateMat(2*num_landmarks, 2*num_landmarks, CV_64FC1);    
    cvZero(Q); // initialize Q matrix
    // set Q matrix
    for (int row = 0; row < Q->rows; row++)
    {
        cvmSet(Q, row, row, obs_x_cov);
    }
    displayMatrix(Q, "Q matrix");   
   
    // G matrix // transition
    CvMat* G = cvCreateMat(num_dim, num_dim, CV_64FC1);
    cvZero(G); // initialize G matrix
    // set G matrix
    cvmSet(G, 0, 0, 1.0); // previous x-position
    cvmSet(G, 0, 2, 1.0); // x-velocity
    cvmSet(G, 1, 1, 1.0); // previous y-position
    cvmSet(G, 1, 3, 1.0); // y-velocity   
    for (int row = 2; row < G->rows; row++)
    {
        cvmSet(G, row, row, 1.0); // constance of velocity
    }
    displayMatrix(G, "G matrix");
   
    // R matrix ; covariance of noise to G; uncertainty of observation
    CvMat* R = cvCreateMat(num_dim, num_dim, CV_64FC1);
    cvZero(R); // initialize R matrix
    // set R matrix
    cvmSet(R, 0, 0, rob_px_cov);
    cvmSet(R, 1, 1, rob_py_cov);
    cvmSet(R, 2, 2, rob_vx_cov);
    cvmSet(R, 3, 3, rob_vy_cov);   
    displayMatrix(R, "R matrix");   
       
   
    CvMat* rob_ground = cvCreateMat(num_dim, 1, CV_64FC1); // ground truth of state        
    CvMat* mu = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
    CvMat* mu_p = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be predicted

    CvMat* sigma = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
    CvMat* sigma_p = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
    CvMat* z = cvCreateMat(2*num_landmarks, 1, CV_64FC1); // measurement vector
    CvMat* K = cvCreateMat(num_dim, 2*num_landmarks, CV_64FC1); // K matrix // Kalman gain
   
    CvMat* delta = cvCreateMat(z->rows, 1, CV_64FC1); // measurement noise (ref. 42p: (3.5))  
    CvMat* obs = cvCreateMat(2*num_landmarks, 1, CV_64FC1); // observation for each landmark

    // initialize "mu" vector
    cvmSet(mu, 0, 0, rob_x); // set mu(0,0) to "rob_x"
    cvmSet(mu, 1, 0, rob_y); // set mu(1,0) to "rob_y"
    cvmSet(mu, 2, 0, rob_vx); // set mu(2,0) to "rob_vx"
    cvmSet(mu, 3, 0, rob_vy); // set mu(3,0) to "rob_vy"
    for (int n = 0; n < 2*num_landmarks; n++)
    {
        cvmSet(mu, n+4, 0, landmark[n]); // set mu(4,0) to "landmark[0]", ...
    }
    displayMatrix(mu, "mu vector");
   
/*    // initialize "sigma" matrix
    cvmSet(sigma, 0, 0, rob_px_cov);
    cvmSet(sigma, 1, 1, rob_py_cov);
    cvmSet(sigma, 2, 2, rob_vx_cov);
    cvmSet(sigma, 3, 3, rob_vy_cov);
    for (int r = 4; r < sigma->rows; r=r*2)
    {
        cvmSet(sigma, r, r, obs_x_cov);
        cvmSet(sigma, r+1, r+1, obs_y_cov);        
    }
*/    // initialize "sigma" matrix <-- This is the most critical point in tuning
    cvSetIdentity(sigma, cvRealScalar(obs_x_cov));       
    displayMatrix(sigma, "sigma matrix");
   
    // matrices to be used in calculation
    CvMat* Hx = cvCreateMat(H->rows, mu->cols, CV_64FC1);
    CvMat* Gt = cvCreateMat(G->cols, G->rows, CV_64FC1);
    cvTranspose(G, Gt); // transpose(G) -> Gt
    CvMat* sigmaGt = cvCreateMat(sigma->rows, G->rows, CV_64FC1);
    CvMat* GsigmaGt = cvCreateMat(G->rows, G->rows, CV_64FC1); // 10x10
   
    CvMat* Ht = cvCreateMat(H->cols, H->rows, CV_64FC1); // 10x6
    cvTranspose(H, Ht); // transpose(H) -> Ht       
    CvMat* sigmaHt = cvCreateMat(sigma->rows, H->rows, CV_64FC1);    // 10x10 * 10x6
    CvMat* HsigmaHt = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6   
    CvMat* HsigmaHtplusQ = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6   
   
    CvMat* invGain = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6   
    CvMat* sigmapHt = cvCreateMat(sigma_p->rows, Ht->cols, CV_64FC1); // 10x10 * 10x6    
   
    CvMat* Hmu = cvCreateMat(H->rows, mu->cols, CV_64FC1); // 6x10 * 10x1
    CvMat* miss = cvCreateMat(Hmu->rows, 1, CV_64FC1); // 6x1
    CvMat* adjust = cvCreateMat(mu->rows, 1, CV_64FC1); // 10x1
   
    CvMat* KH = cvCreateMat(K->rows, H->cols, CV_64FC1); // 10x6 * 6x10
    CvMat* I = cvCreateMat(KH->rows, KH->cols, CV_64FC1); // 10x10 identity matrix
    cvSetIdentity(I); // does not seem to be working properly      
    CvMat* change = cvCreateMat(I->rows, I->cols, CV_64FC1); // 10x10
   
    CvPoint trajectory[step];
    CvPoint robot_ground[step];
   
    int frame = int(0.9*step);
   
    for (int t = 0; t < step; t++)
    {
        cout << endl << "step " << t << endl;      
        cvZero(iplImg);
       
        // predict
        // predict the state (ref. L2, KF algorithm, 42p)
        cvMatMul(G, mu, mu_p); // G * mu -> mu_p
       
        // predict the covariance of the state (ref. L3, EKF algorithm, 42p)
        cvMatMul(sigma, Gt, sigmaGt); // sigma * Gt -> sigmaGt
        cvMatMul(G, sigmaGt, GsigmaGt); // G * sigmaGt -> GsigmaGt
        cvAdd(GsigmaGt, R, sigma_p); // GsigmaGt + R -> sigma_p
               
        // estimate Kalman gain (ref. L4, EKF algorithm, 42p)   
        cvMatMul(sigma_p, Ht, sigmaHt); // sigma_p * Ht -> sigmaHt
        cvMatMul(H, sigmaHt, HsigmaHt); // H * sigmaHt -> HsigmaHt
        cvAdd(HsigmaHt, Q, HsigmaHtplusQ); // HsigmaHt + Q -> HsigmaHtplusQ
        cvInvert(HsigmaHtplusQ, invGain); // inv(HsigmaHtplusQ) -> invGain
        cvMatMul(sigma_p, Ht, sigmapHt); // sigma_p * Ht -> sigmapHt
        cvMatMul(sigmapHt, invGain, K); // sigmapHt * invGain -> K
        displayMatrix(K, "K matrix");  
   
       
        // measure
        // set ground truths
        if ( xGroundTruth >= width || xGroundTruth <= 0)
        {
            vxGroundTruth = - vxGroundTruth;
        }   
        if ( yGroundTruth >= height || yGroundTruth <= 0 )
        {
            vyGroundTruth = - vyGroundTruth;
        }   
        xGroundTruth += vxGroundTruth;
        yGroundTruth += vyGroundTruth;
        cvZero(rob_ground);
        cvmSet(rob_ground, 0, 0, xGroundTruth);
        cvmSet(rob_ground, 1, 0, yGroundTruth);
        cvmSet(rob_ground, 2, 0, vxGroundTruth);
        cvmSet(rob_ground, 3, 0, vyGroundTruth);
       
        robot_ground[t] = cvPoint(cvRound(xGroundTruth),cvRound(yGroundTruth)); 
       
        for (int dim = 0; dim < 2*num_landmarks; dim++)
        {
            cvmSet(rob_ground, dim+4, 0, landmark[dim]);
        }
        displayMatrix(rob_ground, "rob_ground");
        // set measurement noise
        for(int n = 0; n < num_landmarks; n++)
        {
            double rn_x = sqrt(obs_x_cov) * gaussian_random();
            double rn_y = sqrt(obs_y_cov) * gaussian_random();           
            cvmSet(delta, 2*n, 0, rn_x);
            cvmSet(delta, 2*n+1, 0, rn_y);
           
        }
//      displayMatrix(delta, "delta vector; measurement noise");
       
        // define z, measurement, vector
        cvMatMul(H, rob_ground, Hx); // H * rob_ground -> Hx
        cvAdd(Hx, delta, z); // Hx + delta -> z
        displayMatrix(z, "z vector");
       
        // observation relative to robot's position
        for( int n = 0; n < 2*num_landmarks; n++ )
        {
            cvmSet(obs, n, 0, cvmGet(mu,0,0) + cvmGet(z,n,0));
        }
       
        // update the state with Kalman gain (ref. L5, EKF algorithm, 42p)
        cvMatMul(H, mu, Hmu); // H * mu -> Hmu
        cvSub(z, Hmu, miss); // z - Hmu -> miss
        cvMatMul(K, miss, adjust); // K * miss -> adjust
        cvAdd(mu_p, adjust, mu); // mu_p + adjust -> mu
        displayMatrix(mu, "mu vector");
       
        trajectory[t] = cvPoint(cvRound(cvmGet(mu,0,0)),cvRound(cvmGet(mu,1,0)));
       
       
        // update the covariance of the state
        cvMatMul(K, H, KH); // K * H -> KH
        cvSub(I, KH, change); // I - KH -> change
        cvMatMul(change, sigma_p, sigma); // change * sigma_p -> sigma
        displayMatrix(sigma, "sigma matrix");
       
        // result in console
        cout << "robot position: " << "px = " << cvmGet(mu, 0, 0) << "  py = " << cvmGet(mu, 1, 0) << endl;
        for (int n = 0; n < num_landmarks; n++)
        {
            cout << setw(10) << "landmark" << n+1 << " (" << landmark[2*n] << ", " << landmark[2*n+1] << ") "
            << setw(10) << "observation" << n+1 << " (" << cvmGet(obs,2*n,0) << ", " << cvmGet(obs,2*n+1,0) << ") "
            << setw(10) << "estimation" << n+1 << " (" << cvmGet(mu,4+2*n,0) << ", " << cvmGet(mu,4+2*n+1,0) << ") " << endl;
        }       
       
       
        CvMat* local_uncertain = cvCreateMat(2, 2, CV_64FC1);
        CvMat* map_uncertain [num_landmarks];
        for (int n = 0; n < num_landmarks; n++)
        {
            map_uncertain [n] = cvCreateMat(2, 2, CV_64FC1);
        }
        cvmSet(local_uncertain, 0, 0, cvmGet(sigma,0,0));
        cvmSet(local_uncertain, 0, 1, cvmGet(sigma,0,1));
        cvmSet(local_uncertain, 1, 0, cvmGet(sigma,1,0));
        cvmSet(local_uncertain, 1, 1, cvmGet(sigma,1,1));
       
        displayMatrix(local_uncertain, "local_uncertain");       
       
        for (int n = 0; n < num_landmarks; n++)
        {
            cvmSet(map_uncertain[n], 0, 0, cvmGet(sigma,n+4,n+4));
            cvmSet(map_uncertain[n], 0, 1, cvmGet(sigma,n+4,n+5));
            cvmSet(map_uncertain[n], 1, 0, cvmGet(sigma,n+5,n+4));
            cvmSet(map_uncertain[n], 1, 1, cvmGet(sigma,n+5,n+5));

            displayMatrix(map_uncertain[n], "map_uncertain");
        } 
       
        // result in image
        // ground truth of robot position, red       
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(rob_ground,0,0)), cvRound(cvmGet(rob_ground,1,0))), 2, cvScalar(60, 0, 255), 2);
        // estimated robot position, purple
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,0,0)), cvRound(cvmGet(mu,1,0))), 5, cvScalar(255, 0, 100), 2);
        // uncertainty of robot position, purple line
//        cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,0,0))-sqrt(cvmGet(sigma,0,0)), cvRound(height/2)),
//               cvPoint(cvRound(cvmGet(mu,0,0))+sqrt(cvmGet(sigma,0,0)), cvRound(height/2)), cvScalar(255, 0, 100), 1);
   
        CvPoint local = cvPoint(cvRound(cvmGet(mu,0,0)),cvRound(cvmGet(mu,1,0)));
        CvScalar local_center_color = cvScalar(255, 0, 100);
        CvScalar local_curve_color = cvScalarAll(128);
       
        draw2DEllipseFromCovariance(local_uncertain, &local, iplImg, &local_center_color, &local_curve_color, 1);
       
       
        for( int index = 0; index < num_landmarks; index++    )
        { 
            // landmarks, white
            cvCircle(iplImg, cvPoint(cvRound(landmark[2*index]), cvRound(landmark[2*index+1])), 4, cvScalarAll(255), 2);
            // observation, yellow
//            cvCircle(iplImg, cvPoint(cvRound(cvmGet(obs,2*index,0)), cvRound(cvmGet(obs,2*index+1,0))), 4, cvScalar(0, 200, 255), 1);
            // estimation, green
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,4+2*index,0)), cvRound(cvmGet(mu,4+2*index+1,0))), 3, cvScalar(50, 255, 0), 1);
            // uncertainty of estimation, green line
//            cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0))-sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)),
//                   cvPoint(cvRound(cvmGet(mu,index+2,0))+sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)), cvScalar(50, 255, 0), 1);
       
            CvPoint observed = cvPoint(cvRound(cvmGet(mu,4+2*index,0)), cvRound(cvmGet(mu,4+2*index+1,0)));
            CvScalar observed_center_color = cvScalar(50, 255, 0);
            CvScalar observed_curve_color = cvScalar(50, 255, 0);
           
            draw2DEllipseFromCovariance(map_uncertain[index], &observed, iplImg, &observed_center_color, &observed_curve_color, 1);    
        }
       
        for( int p = 1; p <= t; p++ )
        {
            cvLine(iplImg, robot_ground[p-1], robot_ground[p], cvScalar(60, 0, 255), 1);           
            cvLine(iplImg, trajectory[p-1], trajectory[p], cvScalar(255, 0, 100), 1);
        }

        if ( t == frame )
        {
            cvSaveImage("2D SLAM test.bmp", iplImg);
        }
       
        cvShowImage("SLAM-2d", iplImg);
        cvWaitKey(100);   
       
    }
    cout << endl << endl << "process finished" << endl;
    cvWaitKey();   
   
    return 0;
}

References

[1] J. Civera, A.J. Davison, and J.M.M Montiel. Inverse depth parametrization for monocular SLAM. IEEE Trans. on Robotics, 24(5), 2008.

[2] J. Civera, Andrew Davison, and J. Montiel. Inverse Depth to Depth Conversion for Monocular SLAM. In IEEE Int. Conf. on Robotics and Automation, pages 2778 –2783, April 2007.

[3] A. J. Davison. Real-time simultaneous localisation and mapping with a single camera. In Int. Conf. on Computer Vision, volume 2, pages 1403–1410, Nice, October 2003.

[4] Andrew J. Davison. Active search for real-time vision. Int. Conf. on Computer Vision, 1:66–73, 2005.

[5] Andrew J. Davison, Ian D. Reid, Nicholas D. Molton, and Olivier Stasse. MonoSLAM: Real-time single camera SLAM. Trans. on Pattern Analysis and Machine Intelligence, 29(6):1052–1067, June 2007.

[6] Ethan Eade and Tom Drummond. Scalable monocular SLAM. IEEE Int. Conf. on Computer Vision and Pattern Recognition, 1:469–476, 2006.

[7] Thomas Lemaire and Simon Lacroix. Monocular-vision based SLAM using line segments. In IEEE Int. Conf. on Robotics and Automation, pages 2791–2796, Rome, Italy, 2007.

[8] Nicholas Molton, Andrew J. Davison, and Ian Reid. Locally planar patch features for real-time structure from motion. In British Machine Vision Conference, 2004.

[9] J. Montiel, J. Civera, and A. J. Davison. Unified inverse depth parametrization for monocular SLAM. In Robotics: Science and Systems, Philadelphia, USA, August 2006.

[10] L. M. Paz, P. Pini´es, J. Tard´os, and J. Neira. Large scale 6DOF SLAM with stereo-in-hand. IEEE Trans. on Robotics, 24(5), 2008.

[11] J. Sol`a, Andr´e Monin, Michel Devy, and T. Vidal-Calleja. Fusing monocular information in multi-camera SLAM. IEEE Trans. on Robotics, 24(5):958–968, 2008.

[12] Joan Sol`a. Towards Visual Localization, Mapping and Moving Objects Tracking by a Mobile Robot: a Geometric and Probabilistic Approach. PhD thesis, Institut National Polytechnique de Toulouse, 2007.

[13] Joan Sol`a, Andr´e Monin, and Michel Devy. BiCamSLAM: Two times mono is more than stereo. In IEEE Int. Conf. on Robotics and Automation, pages 4795–4800, Rome, Italy, April 2007.

[14] Joan Sol`a, Andr´e Monin, Michel Devy, and Thomas Lemaire. Undelayed initialization in bearing only SLAM. In IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pages 2499–2504, Edmonton, Canada, 2005.

[15] Joan Sol`a, Teresa Vidal-Calleja, and Michel Devy. Undelayed initialization of line segments in monocular SLAM. In IEEE Int. Conf. on Intelligent Robots and Systems, Saint Louis, USA, 2009. To appear.

OS: 32bit Windows Vista Home Basic K Service Pack 1
processor: Intel(R) Core(TM)2 Duo CPU T5750 @ 2.00GHz 2.00 GHz
RAM: 2.00GM

Microsoft Visual Studio 2005 버전 8.050727.867 (vsvista.050727-8600
Microsoft.Net Framework 버전 2.0.50727 서비스 팩

// 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;
}

http://wiki.answers.com/Q/What_is_a_feedback_system
A feedback system, in general engineering terms, is a system whose output if fed back to the input, and depending on the output, your input is adjusted so as to reach a steady-state. In colloquial language, you adjust your input based on the output of your system so as to achieve a certain end, like minimizing disturbance, cancelling echo (in a speech system) and so on.

http://en.wikipedia.org/wiki/Three_sigma_rule
In statistics, the 68-95-99.7 rule, or three-sigma rule, or empirical rule, states that for a normal distribution, nearly all values lie within 3 standard deviations of the mean.

// 1-D Kalman filter algorithm exercise
// VIP lab, Sogang University
// 2009-11-05
// ref. Probabilistic Robotics: 42p

#include <iostream>
using namespace std;

int main (int argc, char * const argv[]) {
   
    double groundtruth[] = {1.0, 2.0, 3.5, 5.0, 7.0, 8.0, 10.0};
    double measurement[] = {1.0, 2.1, 3.2, 5.3, 7.4, 8.1, 9.6};
    double transition_noise = 0.1; // covariance of Gaussian noise to control
    double measurement_noise = 0.3; // covariance of Gaussian noise to measurement
   
    double x = 0.0, v = 1.0;    double cov = 0.5;
   
    double x_p, c_p; // prediction of x and cov
    double gain; // Kalman gain
    double x_pre, m;
   
    for (int t=0; t<7; t++)
    {
        // prediction
        x_pre = x;
        x_p = x + v;
        c_p = cov + transition_noise;
        m = measurement[t];
        // update
        gain = c_p / (c_p + measurement_noise);
        x = x_p + gain * (m - x_p);
        cov = ( 1 - gain ) * c_p;
        v = x - x_pre;

        cout << t << endl;
        cout << "estimation  = " << x << endl;
        cout << "measurement = " << measurement[t] << endl;   
        cout << "groundtruth = " << groundtruth[t] << endl;
    }
    return 0;
}

0
estimation  = 1
measurement = 1
groundtruth = 1
1
estimation  = 2.05
measurement = 2.1
groundtruth = 2
2
estimation  = 3.14545
measurement = 3.2
groundtruth = 3.5
3
estimation  = 4.70763
measurement = 5.3
groundtruth = 5
4
estimation  = 6.76291
measurement = 7.4
groundtruth = 7
5
estimation  = 8.50584
measurement = 8.1
groundtruth = 8
6
estimation  = 9.9669
measurement = 9.6
groundtruth = 10
logout

[Process completed]

// 2-D Kalman filter algorithm exercise
// lym, VIP lab, Sogang University
// 2009-11-05
// ref. Probabilistic Robotics: 42p

#include <OpenCV/OpenCV.h> // matrix operations

#include <iostream>
#include <iomanip>
using namespace std;

int main (int argc, char * const argv[]) {
    int step = 7;
   
    IplImage *iplImg = cvCreateImage(cvSize(150, 150), 8, 3);
    cvZero(iplImg);
   
    cvNamedWindow("Kalman-2d", 0);
   
    //ground truth of real states
    double groundtruth[] = {10.0, 20.0, 35, 50.0, 70.0, 80.0, 100.0, //x-value
                            10.0, 20.0, 40.0, 55, 65, 80.0, 90.0}; //y-value
    //measurement of observed states
    double measurement_set[] = {10.0, 21, 32, 53, 74, 81, 96,  //x-value
                            10.0, 19, 42, 56, 66, 78, 88};    //y-value
    //covariance of Gaussian noise to control
//    double transition_noise[] = { 0.1, 0.0, 
//                                  0.0, 0.1 }; 
    CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1); 
    cvmSet(transition_noise, 0, 0, 0.1); //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, 0.1);    
    //covariance of Gaussian noise to measurement
//    double measurement_noise[] = { 0.3, 0.0, 
//                                   0.0, 0.2 };
    CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1); 
    cvmSet(measurement_noise, 0, 0, 0.3); //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, 0.2);        
   
    CvMat* state = cvCreateMat(2, 1, CV_64FC1);    //states to be estimated   
    CvMat* state_p = cvCreateMat(2, 1, CV_64FC1);  //states to be predicted
    CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); //motion controls to change states
    CvMat* measurement = cvCreateMat(2, 1, CV_64FC1); //measurement of states
   
    CvMat* cov = cvCreateMat(2, 2, CV_64FC1);     //covariance to be updated
    CvMat* cov_p = cvCreateMat(2, 2, CV_64FC1); //covariance to be predicted
    CvMat* gain = cvCreateMat(2, 2, CV_64FC1);     //Kalman gain to be updated
   
    // temporary matrices to be used for estimation
    CvMat* Kalman = cvCreateMat(2, 2, CV_64FC1); //
    CvMat* invKalman = cvCreateMat(2, 2, CV_64FC1); //

    CvMat* I = cvCreateMat(2,2,CV_64FC1);
    cvSetIdentity(I); // does not seem to be working properly   
//  cvSetIdentity (I, cvRealScalar (1));   
    // check matrix
    for(int i=0; i<2; i++)
    {
        for(int j=0; j<2; j++)
        {
            cout << cvmGet(I, i, j) << "\t";           
        }
        cout << endl;
    }
 
    // set the initial state
    cvmSet(state, 0, 0, 0.0); //x-value //set state(0,0) to 0.0
    cvmSet(state, 1, 0, 0.0); //y-value //set state(1,0) to 0.0
    // set the initital covariance of state
    cvmSet(cov, 0, 0, 0.5); //set cov(0,0) to 0.5
    cvmSet(cov, 0, 1, 0.0); //set cov(0,1) to 0.0
    cvmSet(cov, 1, 0, 0.0); //set cov(1,0) to 0.0
    cvmSet(cov, 1, 0, 0.4); //set cov(1,1) to 0.4   
    // set the initial control
    cvmSet(velocity, 0, 0, 10.0); //x-direction //set velocity(0,0) to 1.0
    cvmSet(velocity, 1, 0, 10.0); //y-direction //set velocity(0,0) to 1.0
   
    for (int t=0; t<step; t++)
    {
        // retain the current state
        CvMat* state_out = cvCreateMat(2, 1, CV_64FC1); // temporary vector   
        cvmSet(state_out, 0, 0, cvmGet(state,0,0)); 
        cvmSet(state_out, 1, 0, cvmGet(state,1,0));        
        // predict
        cvAdd(state, velocity, state_p); // state + velocity -> state_p
        cvAdd(cov, transition_noise, cov_p); // cov + transition_noise -> cov_p
        // measure
        cvmSet(measurement, 0, 0, measurement_set[t]); //x-value
        cvmSet(measurement, 1, 0, measurement_set[step+t]); //y-value
        // estimate Kalman gain
        cvAdd(cov_p, measurement_noise, Kalman); // cov_p + measure_noise -> Kalman
        cvInvert(Kalman, invKalman); // inv(Kalman) -> invKalman
        cvMatMul(cov_p, invKalman, gain); // cov_p * invKalman -> gain       
        // update the state
        CvMat* err = cvCreateMat(2, 1, CV_64FC1); // temporary vector
        cvSub(measurement, state_p, err); // measurement - state_p -> err
        CvMat* adjust = cvCreateMat(2, 1, CV_64FC1); // temporary vector
        cvMatMul(gain, err, adjust); // gain*err -> adjust
        cvAdd(state_p, adjust, state); // state_p + adjust -> state
        // update the covariance of states
        CvMat* cov_up = cvCreateMat(2, 2, CV_64FC1); // temporary matrix   
        cvSub(I, gain, cov_up); // I - gain -> cov_up       
        cvMatMul(cov_up, cov_p, cov); // cov_up *cov_p -> cov
        // update the control
        cvSub(state, state_out, velocity); // state - state_p -> velocity
       
        // result in colsole
        cout << "step " << t << endl;
        cout << "estimation  = " << cvmGet(state,0,0) << setw(10) << cvmGet(state,1,0) << endl;
        cout << "measurement = " << cvmGet(measurement,0,0) << setw(10) << cvmGet(measurement,1,0) << endl;   
        cout << "groundtruth = " << groundtruth[t] << setw(10) << groundtruth[t+step] << endl;
        // result in image
        cvCircle(iplImg, cvPoint(cvRound(groundtruth[t]), cvRound(groundtruth[t + step])), 3, cvScalarAll(255));
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement,0,0)), cvRound(cvmGet(measurement,1,0))), 2, cvScalar(255, 0, 0));
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 2, cvScalar(0, 0, 255));
       
        cvShowImage("Kalman-2d", iplImg);
        cvWaitKey(500);   
   
    }
    cvWaitKey();   
   
    return 0;
}

step 0
estimation  = 10        10
measurement = 10        10
groundtruth = 10        10
step 1
estimation  = 20.5   19.6263
measurement = 21        19
groundtruth = 20        20
step 2
estimation  = 31.4545   35.5006
measurement = 32        42
groundtruth = 35        40
step 3
estimation  = 47.0763   53.7411
measurement = 53        56
groundtruth = 50        55
step 4
estimation  = 67.6291   69.0154
measurement = 74        66
groundtruth = 70        65
step 5
estimation  = 85.0584   81.1424
measurement = 81        78
groundtruth = 80        80
step 6
estimation  = 99.669    90.634
measurement = 96        88
groundtruth = 100        90

Camera.Parameters=[ 3.02629 6.17916 0.524049 0.291111 2.1234 ]

exec camera.cfg

calibrator_settings.cfg -> CameraCalibrator -> camera.cfg -> settings.cfg -> PTAM

1) settings.cfg 파일 로드 GUI.LoadFile("settings.cfg");

2) 사용자 입력 parsing GUI.StartParserThread();

3) 클래스 system (system.h) 실행 s.Run();

atexit
Set function to be executed on exit
The function pointed by the function pointer argument is called when the program terminates normally.

try-if 구문
1) Deitel 823p "catch handler"
2) theuhm@naver: "에러가 발생한 객체는 예외를 발생시킴과 동시에 try블럭 안의 모든 객체는 스코프를 벗어나 참조할 수 없게 되므로 예외를 처리하는 동안 try블럭 안에서 예외를 발생시켰을 수 있는 객체의 참조를 원천적으로 막아 더 안전하고 깔끔한 예외처리를 할 수 있는 환경을 만들어줍니다. 그리고 예외를 던질 때에 예외 객체의 클래스를 적절히 구성하면, 예외 객체에 예외를 처리하는 방법을 담아서 던질 수도 있습니다. 그렇게 구성하면 굉장히 깔끔한 코드를 얻을 수 있죠.

1) PTAM에서 핵심적 기능을 하는 클래스들과 클래스 "System"을 선언
// Defines the System class
// This stores the main functional classes of the system
class ATANCamera;
class Map;
class MapMaker;
class Tracker;
class ARDriver;
class MapViewer;
class System;

cvd  -  documentation 0.8  -  tutorial  - 


CVD: CVD::ImageRef Class Reference

Kai Arras
The CAS Robot Navigation Toolbox, a MATLAB simulation toolbox for robot localization and mapping
http://www.cas.kth.se/toolbox/index.html

Tim Bailey
MATLAB simulators for EKF-SLAM, UKF-SLAM, and FastSLAM 1.0 and 2.0. http://www.acfr.usyd.edu.au/homepages/academic/tbailey/software/index.html

Mark Paskin
Java library with several SLAM variants, including Kalman filter, information filter, and thin junction tree forms. Includes a MATLAB interface.
http://www.stanford.edu/~paskin/slam/

Andrew Davison
Scene, a C++ library for map-building and localization. Facilitates real-time single camera SLAM.
http://www.doc.ic.ac.uk/~ajd/Scene/ index.html

José Neira
MATLAB EKF-SLAM simulator that demonstrates joint compatibility branch-and-bound data association.
http://webdiis.unizar.es/~neira/software/slam/slamsim.htm

Dirk Hähnel
C language grid-based version of FastSLAM.
http://www.informatik.uni-freiburg.de/~haehnel/old/download.html

Various
MATLAB code from the 2002 SLAM summer school.
http://www.cas.kth.se/slam/toc.html

Eduardo Nebot
Numerous large-scale outdoor datasets, notably the popular Victoria Park data.
http://www.acfr.usyd.edu.au/homepages/academic/enebot/dataset.htm

Chieh-Chih Wang
Three large-scale outdoor datasets collected by the Navlab11 testbed.
http://www.cs.cmu.edu/~bobwang/datasets.html

Radish (The Robotics Many and varied indoor datasets, including large-area Data Set Repository) data from the CSU Stanislaus Library, the Intel Research Lab in Seattle, the Edmonton Convention Centre, and more.
http://radish.sourceforge.net/

IJRR (The International Journal of Robotics Research)
IJRR maintains a Web page for each article, often containing data and video of results. A good paper example is by Bosse et al. [3], which has data from Killian Court at MIT.
http://www.ijrr.org/contents/23\_12/abstract/1113.html

1)
state-space model + additive Gaussian noise
EKF = extended Kalman filter

2)
a set of samples of a more general non-Gaussian probability distribution to describe vehicle motion
Rao-Blackwellized particle filter or FastSLAM algorithm

3)
information-state form

ref. Sebastian Thrun, Yufeng Liu, Daphne Koller, Andrew Y. Ng, Zoubin Ghahramani, Hugh Durrant-Whyte
Simultaneous Localization and Mapping With Sparse Extended Information Filters

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