2-D visual SLAM with Extended Kalman Filter 연습

2-D visual SLAM with Extended Kalman Filter 연습

가정1: 2차원 공간의 랜드마크들에 대해 카메라로부터 1차원 인풋 이미지(랜드마크에 대한 관측)를 얻는다.

가정2: 우선, 매 프레임마다 모든 랜드마크를 관측한다.


다음은, EKF 알고리즘으로 실행되는 코드
슬램 창에 로봇의 방위 + 1-D 입력 이미지 라인 추가

// 2-D visual SLAM with Extended Kalman Filter
// lym, VIP lab, Sogang University
// 2010-01-29
// ref. Probabilistic Robotics 204p

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

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

#include "randomNumber.h"
#include "display.h"
#include "groundtruth.h"
#include "EKF.h"

#define num_landmarks 4
#define dof_rob 3 // DOF of a robot, (Rx, Ry, Ra)
#define dim_map 2 // 2 dimensional map
#define dim_sys ( dim_map * (num_landmarks + dof_rob) ) // defining the size of a state vector
#define dim_dom ( dim_map * (1 + dof_rob) ) // dimension of the domain of the observation model function
// number of variables to define the observation of one landmark
#define dim_obs 1 // 1 dimensional measurement from input images

#define mapWidth 1200 // width of the map on SLAM
#define mapHeight 600
#define imgWidth 400 // width of input image
#define imgHeight 200

#define focal 200 // focal length of the robot's camera
#define Step 100

// calculate the Jacobian of the observation model
CvMat* compute_Jacobian (CvMat* state)
{ /* Since the observation model is a non-linear function of elements of the state vector,
 H matrix is defined by its Jacobian matrix. */
   
    double Rx = mapHeight - cvmGet(state,0,0); // robot's x-position
    double Rz = cvmGet(state,1,0); // robot's z-position
    double Ra = cvmGet(state,2,0) * CV_PI / 180.0; // robot's angle (radian)
// double Vx = cvmGet(state,3,0); // robot's x-velocity
// double Vz = cvmGet(state,4,0); // robot's z-velocity
// double Va = cvmGet(state,5,0); // robot's angular velocity
    double Lx; // n-th landmark's x-position
    double Lz; // n-th landmark's z-position
   
    CvMat* H = cvCreateMat(num_landmarks, dim_sys, CV_64FC1);   
    cvZero(H); // initialize H matrix 
   
    // calculate the Jacobian of the observation model function, h
    // let h = -focal*h1/h2, when px = h + mapWidth/2   
    double cosRa = cos(Ra);
    double sinRa = sin(Ra);
    for (int n = 0; n < num_landmarks; n++)
    {
        Lx = cvmGet(state,6+2*n,0); // n-th landmark's x-position
        Lz = cvmGet(state,7+2*n,0); // n-th landmark's z-position

//        double h1 = (Lx - Rx)*cosRa - (Lz - Rz)*sinRa;
//        double h2 = (Lz - Rz)*cosRa + (Lx - Rx)*sinRa;
       
        double h1 = (Rx - Lx)*cosRa - (Lz - Rz)*sinRa;
        double h2 = (Lz - Rz)*cosRa + (Rx - Lx)*sinRa;  
       
       
        // partial derivatives of h
        double dh[dim_dom]; // "dim_dom = 8"
 /*       dh[0] = focal/h2 * ( cosRa - sinRa*h1/h2 ); // der(h) to Rx; partial derivative of h with respect to Rx
        dh[1] = focal/h2 * ( cosRa*h1/h2 - sinRa ); // der(h) to Rz       
        dh[2] = focal * ( 1 + (h1*h1)/(h2*h2) ); // der(h) to Ra
        dh[3] = dh[4] = dh[5] = 0.0; // der(h) to Vx, Vz, and Va are zero ; h is independent on them
        dh[6] = focal/h2 * ( sinRa*h1/h2 - cosRa); // der(h) to Lx
        dh[7] = focal/h2 * ( sinRa + cosRa*h1/h2 ); // der(h) to Lz     
    */   
       
        dh[0] = -focal / h2 * (sinRa - h1 * cosRa / h2); // der(h) to Rz                                                 
        dh[1] = -focal / h2 * (cosRa - h1 * sinRa / h2); // der(h) to Rx                          \       
        dh[2] = (CV_PI/180.0)*(focal/h2) * ( sinRa * (Rx-Lx) + cosRa * (Lz-Rz) + (h1/h2)*(cosRa * (Rx-Lx) - sinRa * (Lz-Rz)) ); // der(h) to Ra
        dh[3] = dh[4] = dh[5] = 0.0; // der(h) to Vx, Vz, and Va are zero ; h is independent on them
        dh[6] = focal / h2 * (sinRa + cosRa * h1 / h2 ); // der(h) to Lz   
        dh[7] = focal / h2 * (cosRa - sinRa * h1 / h2); // der(h) to Lx
       
        // set H matrix
        for (int col = 0; col < 5; col++)
        {
            cvmSet(H, n, col, dh[col]); // H(0,0) = dh[0] = dh/dRx
        }
        cvmSet(H, n, 6+2*n, dh[6]); // dh/dLx; der(h) to "row"-th landmark's x-position unit
        cvmSet(H, n, 7+2*n, dh[7]); // dh/dLz; der(h) to "row"-th landmark's z-position unit
    }
    displayMatrix(H, "H matrix");
    return H;
}

void update_measurement (CvMat* output, CvMat* input, CvMat* measurement_noise)
{
//      CvMat* z = cvCreateMat(num_landmarks, 1, CV_64FC1);
   
    double Rz = cvmGet(input,0,0);   
    double Rx = mapHeight - cvmGet(input,1,0);   
    double Ra = cvmGet(input,2,0) * CV_PI / 180.0; // radian
   
    double cosRa = cos(Ra);
    double sinRa = sin(Ra);
   
    // set the non-linear observation model function, h
    // let h = -focal*h1/h2, when px = h + mapWidth/2   
    for( int n = 0; n < num_landmarks; n++ )
    {
        double Lz = cvmGet(input,6+2*n,0);
        double Lx = mapHeight - cvmGet(input,7+2*n,0);
        double h1 = (Lx - Rx)*cosRa - (Lz - Rz)*sinRa;
        double h2 = (Lz - Rz)*cosRa + (Lx - Rx)*sinRa;
        double h = -focal * h1 / h2;
        cvmSet(output, n, 0, h + cvmGet(measurement_noise, n, 0));
    }
   
    displayMatrix(output, "z vector");
    //      return z;
   
} 


// set the transition matrix to define the motion model
CvMat* define_motion_model( int state_dimension )
{
    CvMat* G = cvCreateMat(state_dimension, state_dimension, CV_64FC1);
    cvZero(G); // initialize G matrix
   
    // set G matrix
    cvmSet(G, 0, 0, 1.0); // Rx
    cvmSet(G, 0, 3, 1.0); // Vx
    cvmSet(G, 1, 1, 1.0); // Rz
    cvmSet(G, 1, 4, 1.0); // Vz
    cvmSet(G, 2, 2, 1.0); // Ra
    cvmSet(G, 2, 5, 1.0); // Va
    // assuming robot's constant velocity and motionless landmarks
    for (int row = 3; row < G->rows; row++)
    {
        cvmSet(G, row, row, 1.0);
    }
   
    displayMatrix(G, "G matrix");
   
    return G;
}

// set the covariance matrix of the process noise to define the uncertainty of control
CvMat* define_process_noise( int state_dimension, int Rx_cov, int Rz_cov, int Ra_cov, int Vx_cov, int Vz_cov, int Va_cov )
{                   
    // R matrix ; covariance of process noise to G; uncertainty of control   
    CvMat* R = cvCreateMat(dim_sys, dim_sys, CV_64FC1);
    cvZero(R); // initialize R matrix
   
    // set R matrix
    cvmSet(R, 0, 0, Rx_cov);
    cvmSet(R, 1, 1, Rz_cov);
    cvmSet(R, 2, 2, Ra_cov);
    cvmSet(R, 3, 3, Vx_cov);  
    cvmSet(R, 4, 4, Vz_cov);
    cvmSet(R, 5, 5, Va_cov);  
   
    displayMatrix(R, "R matrix"); 
   
    return R;
}

// set the covariance matrix of measurement noise to define the uncertainty of observation
CvMat* define_measurement_noise( int measurement_dimension, double obs_cov )
{
    CvMat* Q = cvCreateMat(measurement_dimension, measurement_dimension, CV_64FC1);
    cvZero(Q); // initialize Q matrix
   
    // set Q matrix
    for (int row = 0; row < measurement_dimension; row++)
    {
        cvmSet(Q, row, row, obs_cov);
    }
   
    displayMatrix(Q, "Q matrix"); 
   
    return Q;
}

// predict the covariance matrix of the estimated state
CvMat* predict_covariance_of_state( CvMat* G, CvMat* sigma, CvMat* R )
{
    CvMat* sigma_p = cvCreateMat(dim_sys, dim_sys, CV_64FC1);
   
    // create the temporary matrices to be needed in calculation below
    CvMat* Gt = cvCreateMat(G->cols, G->rows, CV_64FC1);
    CvMat* sigmaGt = cvCreateMat(sigma->rows, G->rows, CV_64FC1);
    CvMat* GsigmaGt = cvCreateMat(G->rows, G->rows, CV_64FC1);
   
    // predict the covariance of the state (ref. L3, EKF algorithm, 59p)
    cvTranspose(G, Gt); // transpose(G) -> Gt
    cvMatMul(sigma, Gt, sigmaGt); // sigma * Gt -> sigmaGt
    cvMatMul(G, sigmaGt, GsigmaGt); // G * sigmaGt -> GsigmaGt
    cvAdd(GsigmaGt, R, sigma_p); // GsigmaGt + R -> sigma_p
   
    // release the temporary matrices in calculation above
    cvReleaseMat(&Gt);
    cvReleaseMat(&sigmaGt);
    cvReleaseMat(&GsigmaGt);
   
    return sigma_p;
}


// compute Kalman Gain in EKF algorithm (ref. L4, EKF algorithm, 59p)
CvMat* compute_KalmanGain( CvMat* H, CvMat* sigma_p, CvMat* Q )
{
    CvMat* K = cvCreateMat(dim_sys, num_landmarks, CV_64FC1); // K matrix // Kalman gain
   
    // create temporary matrices to be used in calculation below
    CvMat* Ht = cvCreateMat(H->cols, H->rows, CV_64FC1);
    CvMat* sigmaHt = cvCreateMat(sigma_p->rows, H->rows, CV_64FC1); 
    CvMat* HsigmaHt = cvCreateMat(H->rows, H->rows, CV_64FC1);
    CvMat* HsigmaHtplusQ = cvCreateMat(H->rows, H->rows, CV_64FC1);  
    CvMat* invGain = cvCreateMat(H->rows, H->rows, CV_64FC1);
    CvMat* sigmapHt = cvCreateMat(sigma_p->rows, Ht->cols, CV_64FC1);  
   
    // compute Kalman gain
    cvTranspose(H, Ht); // transpose(H) -> Ht
    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
   
    // release temporary matrices to be used in calculation above
    cvReleaseMat(&Ht);
    cvReleaseMat(&sigmaHt);
    cvReleaseMat(&HsigmaHt);
    cvReleaseMat(&HsigmaHtplusQ);
    cvReleaseMat(&invGain);
    cvReleaseMat(&sigmapHt);
   
    displayMatrix(K, "K matrix");
   
    return K;
}

// update the state with Kalman gain (ref. L5, EKF algorithm, 59p)
CvMat* update_state_mean( CvMat* mu, CvMat* mu_p, CvMat* K, CvMat* z, CvMat* H )
{
   
    // create temporary matrices to be used in calculation below
    CvMat* Hmu = cvCreateMat(H->rows, mu->cols, CV_64FC1);
    CvMat* miss = cvCreateMat(Hmu->rows, 1, CV_64FC1);
    CvMat* adjust = cvCreateMat(mu->rows, 1, CV_64FC1);
   
    // update the state with Kalman gain (ref. L5, EKF algorithm, 59p)
    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
   
    // release temporary matrices to be used in calculation above
    cvReleaseMat(&Hmu);
    cvReleaseMat(&miss);
    cvReleaseMat(&adjust);
   
    displayMatrix(mu, "mu vector");
   
    return mu;
}

// update the covariance of the state with Kalman gain (ref. L6, EKF algorithm, 59p)
CvMat* update_state_covariance( CvMat* sigma, CvMat* K, CvMat* H, CvMat* sigma_p )
{
    // create temporary matrices
    CvMat* KH = cvCreateMat(K->rows, H->cols, CV_64FC1);   
    CvMat* I = cvCreateMat(KH->rows, KH->cols, CV_64FC1);
    CvMat* change = cvCreateMat(I->rows, I->cols, CV_64FC1);
   
    cvMatMul(K, H, KH); // K * H -> KH
    cvSetIdentity(I);
    cvSub(I, KH, change); // I - KH -> change
    cvMatMul(change, sigma_p, sigma); // change * sigma_p -> sigma
   
    cvReleaseMat(&KH);
    cvReleaseMat(&I);
    cvReleaseMat(&change);
   
    displayMatrix(sigma, "sigma matrix");
    return sigma;
}


// set the ground truth of the robot's state
CvMat* set_groundtruth_of_robot( double x, double z, double a, double vx, double vz, double va )  
{                                   
//    CvMat* rob_ground = cvCreateMat(dim_sys, 1, CV_64FC1);
    CvMat* rob_ground = cvCreateMat(6, 1, CV_64FC1);
   
    if( a < -20.0 || a > 20.0 )
    {
        va = (-1) * va ;
    }
    x += vx;
    //      z += vzGroundTruth; // steady       
    a += va;
   
    // temporary variable to display the robot's position in "window1"
    //            robot_ground[t] = cvPoint(cvRound(xGroundTruth),cvRound(zGroundTruth));
   
    cvZero(rob_ground);
    cvmSet(rob_ground, 0, 0, x); cvmSet(rob_ground, 1, 0, z); cvmSet(rob_ground, 2, 0, a);
    cvmSet(rob_ground, 3, 0, vx); cvmSet(rob_ground, 4, 0, vz); cvmSet(rob_ground, 5, 0, va);   
   
/*    for (int dim = 0; dim < num_landmarks*dim_map; dim++)
    {
        cvmSet(rob_ground, dim+6, 0, landmark[dim]);
    }
*/   
    displayMatrix(rob_ground, "rob_ground");
    return rob_ground;
}



void drawObservedImage( IplImage *iplImg, CvMat *observed, double imgCenter, double imageHeight, CvScalar* colorLandmark, CvMat *noise = NULL)
{
   
    for(int n = 0; n < num_landmarks; n++ )
    {
        //              cout << cvRound(cvmGet(z,n,0)) << endl;
        //             cout << cvRound(cvmGet(z,n,0)+ imgWidth/2) << ", " << cvRound(imgHeight/2) <<endl;
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(observed, n, 0) + imgCenter), cvRound(imageHeight / 2)), 2, colorLandmark[n], 1);
        if(noise) {
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(observed, n, 0) - cvmGet(noise, n, 0) + imgCenter), cvRound(imageHeight / 2) ), 2, colorLandmark[n], -1);
        }
    }
    cvLine(iplImg, cvPoint(imgWidth / 2.0, 0), cvPoint(imgWidth / 2.0, imageHeight - 1), cvScalar(255, 255, 255));
}



int main (int argc, char * const argv[]) {
   
    srand(time(NULL));
   
    // set the initial state
    double Rxi = mapWidth * 0.01; // robot's initial x-position
    double Rzi = mapHeight * 0.5; // robot's initial z-position to be steady
    double Rai = 0.0; // robot's initial angle rotated around y-axis
    double Vxi = 2.0; // = mapWidth * 0.6 / (Step+2); // robot's initial x-velocity
    double Vzi = 0.0; // robot's initial z-velocity that is zero
    double Vai = 0.2; // = 20.0 * uniform_random() - 10.0; // robot's initial angular velocity around y-axis

    // set the initial covariance of the state uncertainty
    double Rx_cov = 0.001; // covariance of noise to robot's x-position
    double Rz_cov = 0.001; // covariance of noise to robot's z-position
    double Ra_cov = 0.001; // covariance of noise to robot's angle around y-axis
    double Vx_cov = 0.001; // covariance of noise to robot's x-velocity
    double Vz_cov = 0.001; // covariance of noise to robot's z-velocity
    double Va_cov = 0.001; // covariance of noise to robot's angular velocity
   
    // set the initial covariance of the measurement noise
    double obs_x_cov = 900.0; // covariance of noise to x-measurement of landmarks
    double obs_z_cov = 900.0; // covariance of noise to z-measurement of landmarks
   
    // ground truths of the state   
    double xGroundTruth = Rxi;
    double zGroundTruth = Rzi;
    double aGroundTruth = Rai;
    double vxGroundTruth = Vxi;
    double vzGroundTruth = Vzi;
    double vaGroundTruth = Vai;
   
    // ground truths of "num_landmarks" landmarks in the world coordinate
    double landmark[num_landmarks*dim_map]; // "dim_map" = 2; dimension of the map
   
    // ground truths of "num_landmarks" landmarks in the world coordinate
    set_groundtruths_of_landmarks( landmark ); // "groundtruth.h"
   
    // G matrix; transition matrix to define the motion model
    CvMat* G = define_motion_model(dim_sys);
   
    // R matrix; covariance matrix of the process noise added to G, due to the uncertainty of control   
    CvMat* R = define_process_noise(dim_sys, Rx_cov, Rz_cov, Ra_cov, Vx_cov, Vz_cov, Va_cov);
   
    // H matrix; measurement matrix to define the observation model   
 //   CvMat* H = cvCreateMat(num_landmarks, dim_sys, CV_64FC1);
   
    // Q matrix; covariance matrix of the measurement noise to H, due to the uncertainty of observation   
    CvMat* Q = define_measurement_noise(num_landmarks, obs_x_cov);
   

   
    // define the system
       
    CvMat* mu = cvCreateMat(dim_sys, 1, CV_64FC1); // state vector to be estimated
    CvMat* mu_p = cvCreateMat(dim_sys, 1, CV_64FC1); // state vector to be predicted
    CvMat* sigma = cvCreateMat(dim_sys, dim_sys, CV_64FC1); // covariance to be updated
   
    CvMat* z = cvCreateMat(num_landmarks, 1, CV_64FC1); // measurement vector
    CvMat* delta = cvCreateMat(num_landmarks, 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, Rxi);
    cvmSet(mu, 1, 0, Rzi);
    cvmSet(mu, 2, 0, Rai);
    cvmSet(mu, 3, 0, Vxi);
    cvmSet(mu, 4, 0, Vzi);
    cvmSet(mu, 5, 0, Vai);
   
    for (int n = 0; n < num_landmarks*dim_map; n++)
    {
        cvmSet(mu, n+6, 0, landmark[n]); // set mu(6,0) to "landmark[0]", ...
    }
    displayMatrix(mu, "mu vector");
   
    // initialize "sigma" matrix <-- This is the most critical factor in tuning
    cvZero(sigma);
    cvmSet(sigma, 0, 0, Rx_cov);
//  cvmSet(sigma, 1, 1, Rz_cov);   
    cvmSet(sigma, 2, 2, Ra_cov);
    cvmSet(sigma, 3, 3, Vx_cov);
//  cvmSet(sigma, 4, 4, Vz_cov);   
    cvmSet(sigma, 5, 5, Va_cov);
    for( int r = 6; r < sigma->rows; r += 2 )
    {
        cvmSet(sigma, r, r, obs_x_cov * 10);
        cvmSet(sigma, r+1, r+1, obs_z_cov * 10);           
    }
     
   
 
    CvPoint trajectory[Step];
    CvPoint robot_ground[Step];
   
    // define IPL image windows
    char window1[256], window2[256], captured[256];
    sprintf(window1, "2D vSLAM with EKF");
    cvNamedWindow(window1);
    cvMoveWindow(window1, 10  , imgHeight + 200);
    IplImage *iplImg = cvCreateImage(cvSize(mapWidth, mapHeight) , 8, 3); // process of vSLAM algorithm
    cvZero(iplImg);
   
    sprintf(window2, "observation of 1D input image");
    cvNamedWindow(window2);
    IplImage *inputImg[Step]; // measured input images
    for( int t = 0; t < Step; t++ )
    {
        inputImg[t] = cvCreateImage(cvSize(imgWidth, imgHeight) , 8, 3);
        cvZero(inputImg[t]);
    }
   
    // set to display the groundtruth of each landmark's position
    char txt[200];
    CvPoint pLM;
    CvScalar colorLandmark[num_landmarks];
    for(int iL = 0; iL < num_landmarks; iL++)
    {
        colorLandmark[iL] = generateRandomColor(50, 50, 50);
    }
   
    sprintf(captured, "vSLAM2D_test.bmp"); // captured one from frames in "iplImg"
   
    int frame = int(0.9*Step); // frame Nunmber of a image to be saved as "captured"
   
    cout << endl << "process starting" << endl;
   
    for (int t = 0; t < Step; t++)
    {
        cout << endl << "step " << t << endl;          
        cvZero(iplImg);
       
        //* prediction step
       
        // predict the state (ref. L2, KF algorithm, 59p)
        cvMatMul(G, mu, mu_p); // G * mu -> mu_p
       
        // predict the covariance of the state (ref. L3, EKF algorithm, 59p)
        CvMat* sigma_p = predict_covariance_of_state( G, sigma, R);
   
       
        //* measurement step
       
        // ground truth of state
//        CvMat* rob_ground = set_groundtruth_of_robot( xGroundTruth, zGroundTruth, aGroundTruth, vxGroundTruth, vzGroundTruth, vaGroundTruth );
        /////###
        // set ground truths       
       
        if( aGroundTruth < -20.0 || aGroundTruth > 20.0 )
         {
             vaGroundTruth = (-1) * vaGroundTruth ;
         }
         xGroundTruth += vxGroundTruth;
//      zGroundTruth += vzGroundTruth; // steady       
         aGroundTruth += vaGroundTruth;
         
         // temporary variable to display the robot's position in "window1"
         robot_ground[t] = cvPoint(cvRound(xGroundTruth),cvRound(zGroundTruth));
        CvMat* rob_ground = cvCreateMat(dim_sys, 1, CV_64FC1);
        cvZero(rob_ground);
         cvmSet(rob_ground, 0, 0, xGroundTruth);
         cvmSet(rob_ground, 1, 0, zGroundTruth);
         cvmSet(rob_ground, 2, 0, aGroundTruth);   
         cvmSet(rob_ground, 3, 0, vxGroundTruth);
         cvmSet(rob_ground, 4, 0, vzGroundTruth);
         cvmSet(rob_ground, 5, 0, vaGroundTruth);   
         
         for (int dim = 0; dim < num_landmarks*dim_map; dim++)
         {
         cvmSet(rob_ground, dim+6, 0, landmark[dim]);
         }
         displayMatrix(rob_ground, "rob_ground");
         ////###
         
       
       
       
        for(int n = 0; n < num_landmarks; n++)
        {
            double rn = sqrt(obs_x_cov) * gaussian_random();
            cvmSet(delta, n, 0, rn);        
        }
       
       
        update_measurement(z, rob_ground, delta);
        // remember px = z + mapWidth/2
       

       
        //* correction step
       
        // update H matrix H; measurement matrix to define the observation model
        // returned by CvMat* compute_Jacobian (CvMat* state)
        CvMat* H = compute_Jacobian( mu ); 
       
        // update Kalman gain (ref. L4, EKF algorithm, 59p)
        CvMat* K = compute_KalmanGain( H, sigma_p, Q );
       
        // update the state with Kalman gain (ref. L5, EKF algorithm, 59p)
        mu = update_state_mean( mu, mu_p, K, z, H );
           
        trajectory[t] = cvPoint(cvRound(cvmGet(mu,0,0)),cvRound(cvmGet(mu,1,0)));
       
        // update the covariance of the state (ref. L6, EKF algorithm, 59p)
        sigma = update_state_covariance( sigma, K, H, sigma_p );
       
           
        // result in console
        cout << "robot position & pose: " << "Rx = " << cvmGet(mu, 0, 0) << "  Rz = " << cvmGet(mu, 1, 0)
        << "  Ra = " << cvmGet(mu, 2, 0) << " degree" << 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,n,0) << ", " << "unknown depth" << ") "
            << setw(10) << "estimation" << n+1 << " (" << cvmGet(mu,n*dim_map+6,0) << ", " << cvmGet(mu,n*dim_map+7,0) << ") " << endl;
        }     
       
        // 2x2 matrix to display an ellipse of the uncertainty of the robot's position
        CvMat* local_uncertain = 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");     
       
        // 2x2 matrix to display an ellipse of the uncertainty of the landmarks' positions       
        CvMat* map_uncertain [num_landmarks];

        for (int n = 0; n < num_landmarks; n++)           
        {
            map_uncertain [n] = cvCreateMat(2, 2, CV_64FC1);
        }   
       
        for (int n = 0; n < num_landmarks; n++)
        {
            cvmSet(map_uncertain[n], 0, 0, cvmGet(sigma,n+6,n+6));
            cvmSet(map_uncertain[n], 0, 1, cvmGet(sigma,n+6,n+7));
            cvmSet(map_uncertain[n], 1, 0, cvmGet(sigma,n+7,n+6));
            cvmSet(map_uncertain[n], 1, 1, cvmGet(sigma,n+7,n+7));
           
        //   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(10, 0, 255), 1);
       
        // estimated robot position, purple
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,0,0)), cvRound(cvmGet(mu,1,0))), 5, cvScalar(255, 0, 100), 5);
       
       
       
        // draw the camera's position and pose in the global map
        drawCamState(iplImg, cvPoint2D32f(cvmGet(mu,0,0), cvmGet(mu,1,0)), cvmGet(mu,2,0), focal, imgWidth / 2.0, cvScalar(255, 0, 100), cvScalar(60, 255, 255), cvPoint(-100, 30));
       
        drawCamState(iplImg, cvPoint2D32f(cvmGet(rob_ground,0,0), cvmGet(rob_ground, 1, 0)), cvmGet(rob_ground, 2, 0), focal, imgWidth / 2.0, cvScalar(0, 255, 0), cvScalar(255, 0, 0), cvPoint(10, 30));
       
        drawObservedImage(inputImg[t], z, imgWidth / 2.0, imgHeight, colorLandmark, delta);
       
       
        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
            pLM = cvPoint(cvRound(landmark[2 * index]), cvRound(landmark[2 * index+1]));
 //           cvCircle(iplImg, pLM, 4, cvScalarAll(255), 1);
           
            //    라벨링
            sprintf(txt, "L%d", index + 1);
           cvPutText(iplImg, txt, cvPoint(pLM.x - 5, pLM.y - 7), &cvFont(0.7), colorLandmark[index]);
       
            //    GT랑 camera center랑 연결
            cvLine(iplImg, pLM, robot_ground[t], colorLandmark[index]);
       
            // 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,6+2*index,0)), cvRound(cvmGet(mu,6+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,6+2*index,0)), cvRound(cvmGet(mu,6+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(captured, iplImg);
        }
       
        cvShowImage(window1, iplImg);       
        cvShowImage(window2, inputImg[t]);    
       
//        cvWaitKey(1000);       
        cvWaitKey();         
    } // end for t
   
    cout << endl << endl << "process finished" << endl;
   
    cvWaitKey(); 
   
    return 0;
   
}

 

다음에 할 일:
1. 1대의 카메라로부터 랜드마크들을 초기화하는 문제
2. 프레임마다 관측되는 랜드마크들과 그 수가 달라지는 문제 --> dynamic data structure