Kalman.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include <iostream>
00025 #include <algorithm> // for std::max
00026 #include "cxcore.h"
00027 #include "cv.h"
00028 #include "highgui.h"
00029 #include "ar_track_alvar/Kalman.h"
00030 #include "ar_track_alvar/Util.h"
00031 #include "ar_track_alvar/Alvar.h"
00032 
00033 namespace alvar {
00034 
00035 KalmanSensorCore::KalmanSensorCore(const KalmanSensorCore &k) {
00036         m = k.m;
00037         n = k.n;
00038         z = cvCloneMat(k.z);
00039         H = cvCloneMat(k.H);
00040         H_trans = cvCloneMat(k.H_trans);
00041         K = cvCloneMat(k.K);
00042         z_pred = cvCloneMat(k.z_pred);
00043         z_residual = cvCloneMat(k.z_residual);
00044         x_gain = cvCloneMat(k.x_gain);
00045 }
00046 
00047 KalmanSensorCore::KalmanSensorCore(int _n, int _m) {
00048         n = _n;
00049         m = _m;
00050         z = cvCreateMat(m,1,CV_64FC1); cvSetZero(z);
00051         H = cvCreateMat(m,n,CV_64FC1); cvSetZero(H);
00052         H_trans = cvCreateMat(n,m,CV_64FC1); cvSetZero(H_trans);
00053         K = cvCreateMat(n,m,CV_64FC1); cvSetZero(K);
00054         z_pred = cvCreateMat(m,1,CV_64FC1); cvSetZero(z_pred);
00055         z_residual = cvCreateMat(m,1,CV_64FC1); cvSetZero(z_residual);
00056         x_gain = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_gain);
00057 }
00058 
00059 KalmanSensorCore::~KalmanSensorCore() {
00060         cvReleaseMat(&z);
00061         cvReleaseMat(&H);
00062         cvReleaseMat(&H_trans);
00063         cvReleaseMat(&K);
00064         cvReleaseMat(&z_pred);
00065         cvReleaseMat(&z_residual);
00066         cvReleaseMat(&x_gain);
00067 }
00068 
00069 void KalmanSensorCore::update_x(CvMat *x_pred, CvMat *x) {
00070         // x = x_pred + K * (z - H*x_pred)
00071         cvMatMul(H, x_pred, z_pred);
00072         cvScaleAdd(z_pred, cvScalar(-1), z, z_residual);
00073         cvMatMul(K, z_residual, x_gain);
00074         cvScaleAdd(x_pred, cvScalar(1), x_gain, x);
00075 }
00076 
00077 void KalmanCore::predict_x(unsigned long tick) {
00078         // x_pred = F * x;
00079         cvMatMul(F, x, x_pred);
00080 }
00081 
00082 KalmanCore::KalmanCore(const KalmanCore &s) {
00083         n = s.n;
00084         x = cvCloneMat(s.x);
00085         F = cvCloneMat(s.F);
00086         x_pred = cvCloneMat(s.x_pred);
00087         F_trans = cvCloneMat(s.F_trans);
00088 }
00089 
00090 KalmanCore::KalmanCore(int _n) {
00091         n = _n;
00092         x = cvCreateMat(n,1,CV_64FC1); cvSetZero(x);
00093         F = cvCreateMat(n,n,CV_64FC1); cvSetIdentity(F);
00094         F_trans = cvCreateMat(n,n,CV_64FC1); cvSetIdentity(F_trans);
00095         x_pred = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_pred);
00096 }
00097 
00098 KalmanCore::~KalmanCore() {
00099         cvReleaseMat(&x);
00100         cvReleaseMat(&F);
00101         cvReleaseMat(&F_trans);
00102         cvReleaseMat(&x_pred);
00103 }
00104 
00105 CvMat *KalmanCore::predict() {
00106         predict_x(0);
00107         return x_pred;
00108 }
00109 
00110 CvMat *KalmanCore::predict_update(KalmanSensorCore *sensor) {
00111         predict();
00112         sensor->update_x(x_pred, x);
00113         return x;
00114 }
00115 
00116 KalmanSensor::KalmanSensor(const KalmanSensor &k) : KalmanSensorCore(k) {
00117         R = cvCloneMat(k.R);
00118         R_tmp = cvCloneMat(k.R_tmp);
00119         P_tmp = cvCloneMat(k.P_tmp);
00120 }
00121 
00122 KalmanSensor::KalmanSensor(int n, int _m) : KalmanSensorCore(n, _m) {
00123         R = cvCreateMat(m,m,CV_64FC1); cvSetZero(R);
00124         R_tmp = cvCreateMat(m,m,CV_64FC1); cvSetZero(R);
00125         P_tmp = cvCreateMat(n,n,CV_64FC1); cvSetZero(P_tmp);
00126 }
00127 
00128 KalmanSensor::~KalmanSensor() {
00129         cvReleaseMat(&R);
00130         cvReleaseMat(&R_tmp);
00131         cvReleaseMat(&P_tmp);
00132 }
00133 
00134 void KalmanSensor::update_K(CvMat *P_pred) {
00135         // K = P * trans(H) * inv(H*P*trans(H) + R)
00136         cvTranspose(H, H_trans);
00137         cvMatMul(P_pred, H_trans, K);
00138         cvMatMul(H, K, R_tmp);
00139         cvScaleAdd(R_tmp, cvScalar(1), R, R_tmp);
00140         cvInvert(R_tmp, R_tmp);
00141         cvMatMul(H_trans, R_tmp, K);
00142         cvMatMul(P_pred, K, K);
00143 }
00144 
00145 void KalmanSensor::update_P(CvMat *P_pred, CvMat *P) {
00146         //P = (I - K*H) * P_pred
00147         cvMatMul(K, H, P_tmp);
00148         cvSetIdentity(P);
00149         cvScaleAdd(P_tmp, cvScalar(-1), P, P);
00150         cvMatMul(P, P_pred, P);
00151 }
00152 
00153 void Kalman::predict_P() {
00154         // P_pred = F*P*trans(F) + Q
00155         cvTranspose(F, F_trans);
00156         cvMatMul(P, F_trans, P_pred);
00157         cvMatMul(F, P_pred, P_pred);
00158         cvScaleAdd(P_pred, cvScalar(1), Q, P_pred);
00159 }
00160 
00161 Kalman::Kalman(int _n) : KalmanCore(_n) {
00162         prev_tick = 0;
00163         Q = cvCreateMat(n,n,CV_64FC1); cvSetZero(Q);
00164         P = cvCreateMat(n,n,CV_64FC1); cvSetZero(P);
00165         P_pred = cvCreateMat(n,n,CV_64FC1); cvSetZero(P_pred);
00166 }
00167 
00168 Kalman::~Kalman() {
00169         cvReleaseMat(&Q);
00170         cvReleaseMat(&P);
00171         cvReleaseMat(&P_pred);
00172 }
00173 
00174 void Kalman::update_F(unsigned long tick) {
00175         //cvSetIdentity(F);
00176 }
00177 
00178 CvMat *Kalman::predict(unsigned long tick) {
00179         update_F(tick);
00180         predict_x(tick);
00181         predict_P();
00182         return x_pred;
00183 }
00184 
00185 CvMat *Kalman::predict_update(KalmanSensor *sensor, unsigned long tick) {
00186         predict(tick);
00187         sensor->update_H(x_pred);
00188         sensor->update_K(P_pred);
00189         sensor->update_x(x_pred, x);
00190         sensor->update_P(P_pred, P);
00191         prev_tick = tick;
00192         return x;
00193 }
00194 
00195 double Kalman::seconds_since_update(unsigned long tick) {
00196         unsigned long tick_diff = (prev_tick ? tick-prev_tick : 0);
00197         return ((double)tick_diff/1000.0);
00198 }
00199 
00200 void KalmanSensorEkf::update_H(CvMat *x_pred) {
00201         // By default we update the H by calculating Jacobian numerically
00202         const double step = 0.000001;
00203         cvZero(H);
00204         for (int i=0; i<n; i++) {
00205                 CvMat H_column;
00206                 cvGetCol(H, &H_column, i);
00207 
00208                 cvZero(delta); 
00209                 cvmSet(delta, i, 0, step);
00210                 cvAdd(x_pred, delta, x_plus);
00211                 cvmSet(delta, i, 0, -step);
00212                 cvAdd(x_pred, delta, x_minus);
00213 
00214                 h(x_plus, z_tmp1);  
00215                 h(x_minus, z_tmp2);     
00216                 cvSub(z_tmp1, z_tmp2, &H_column);
00217                 cvScale(&H_column, &H_column, 1.0/(2*step));
00218         }
00219 }
00220 
00221 void KalmanSensorEkf::update_x(CvMat *x_pred, CvMat *x) {
00222         // x = x_pred + K * (z - h(x_pred))
00223         h(x_pred, z_pred);
00224         cvScaleAdd(z_pred, cvScalar(-1), z, z_residual);
00225         cvMatMul(K, z_residual, x_gain);
00226         cvScaleAdd(x_pred, cvScalar(1), x_gain, x);
00227 }
00228 
00229 KalmanSensorEkf::KalmanSensorEkf(const KalmanSensorEkf &k) : KalmanSensor(k) {
00230         delta   = cvCloneMat(k.delta);
00231         x_plus  = cvCloneMat(k.x_plus);
00232         x_minus = cvCloneMat(k.x_minus);
00233         z_tmp1  = cvCloneMat(k.z_tmp1);
00234         z_tmp2  = cvCloneMat(k.z_tmp2);
00235 }
00236 
00237 KalmanSensorEkf::KalmanSensorEkf(int _n, int _m) : KalmanSensor(_n, _m) {
00238         delta = cvCreateMat(n,1,CV_64FC1); cvSetZero(delta);
00239         x_plus = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_plus);
00240         x_minus = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_minus);
00241         z_tmp1 = cvCreateMat(m,1,CV_64FC1); cvSetZero(z_tmp1);
00242         z_tmp2 = cvCreateMat(m,1,CV_64FC1); cvSetZero(z_tmp2);
00243 }
00244 
00245 KalmanSensorEkf::~KalmanSensorEkf() {
00246         cvReleaseMat(&delta);
00247         cvReleaseMat(&x_plus);
00248         cvReleaseMat(&x_minus);
00249         cvReleaseMat(&z_tmp1);
00250         cvReleaseMat(&z_tmp2);
00251 }
00252 
00253 void KalmanEkf::update_F(unsigned long tick) {
00254         // By default we update the F by calculating Jacobian numerically
00255         // TODO
00256         double dt = (tick-prev_tick)/1000.0;
00257         const double step = 0.000001;
00258         cvZero(F);
00259         for (int i=0; i<n; i++) {
00260                 CvMat F_column;
00261                 cvGetCol(F, &F_column, i);
00262 
00263                 cvZero(delta); 
00264                 cvmSet(delta, i, 0, step);
00265                 cvAdd(x, delta, x_plus);
00266                 cvmSet(delta, i, 0, -step);
00267                 cvAdd(x, delta, x_minus);
00268 
00269                 f(x_plus, x_tmp1, dt);  
00270                 f(x_minus, x_tmp2, dt); 
00271                 cvSub(x_tmp1, x_tmp2, &F_column);
00272                 cvScale(&F_column, &F_column, 1.0/(2*step));
00273         }
00274 }
00275 
00276 void KalmanEkf::predict_x(unsigned long tick) {
00277         double dt = (tick-prev_tick)/1000.0;
00278         f(x, x_pred, dt);
00279 }
00280 
00281 KalmanEkf::KalmanEkf(int _n) : Kalman(_n) {
00282         delta = cvCreateMat(n,1,CV_64FC1); cvSetZero(delta);
00283         x_plus = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_plus);
00284         x_minus = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_minus);
00285         x_tmp1 = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_tmp1);
00286         x_tmp2 = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_tmp2);
00287 }
00288 
00289 KalmanEkf::~KalmanEkf() {
00290         cvReleaseMat(&delta);
00291         cvReleaseMat(&x_plus);
00292         cvReleaseMat(&x_minus);
00293         cvReleaseMat(&x_tmp1);
00294         cvReleaseMat(&x_tmp2);
00295 }
00296 
00297 void KalmanVisualize::img_matrix(CvMat *mat, int top, int left) {
00298         cvSetImageROI(img, cvRect(top, left, mat->cols, mat->rows));
00299         for (int j=0; j<mat->rows; j++) {
00300                 for (int i=0; i<mat->cols; i++) {
00301                         double d = cvGet2D(mat, j, i).val[0];
00302                         if (d < 0) d=-d;
00303                         double c1=0, c2=0, c3=0;
00304                         if (d < 0.1) {
00305                                 c1 = 0 + ((d - 0.0)/(0.1 - 0.0)*(127 - 0));
00306                         } else if(d < 1.0) {
00307                                 c1 = 127 + ((d - 0.1)/(1.0 - 0.1)*(255 - 127));
00308                         } else if (d < 10.0) {
00309                                 c1 = 255;
00310                                 c2 = 0 + ((d - 1.0)/(10.0 - 1.0)*(255 - 0));
00311                         } else if (d < 100.0) {
00312                                 c1 = 255;
00313                                 c2 = 255;
00314                                 c3 = 0 + ((d - 10.0)/(100.0 - 10.0)*(255 - 0));
00315                         } else {
00316                                 c1 = 255; c2 = 255;     c3 = 255;
00317                         }
00318                         if (d < 0) {
00319                                 cvSet2D(img, j, i, cvScalar(c3, c2, c1)); // BRG
00320                         } else {
00321                                 cvSet2D(img, j, i, cvScalar(c2, c1, c3)); // BGR
00322                         }
00323                 }
00324         }
00325         cvResetImageROI(img);
00326 }
00327 
00328 void KalmanVisualize::Init() {
00329         n = kalman->get_n();
00330         m = sensor->get_m();
00331         int img_width = std::max(3+n+3+n+5+m+5, 1+n+1+n+1+n+1+m+1+n+1);
00332         int img_height = 1+n+1+std::max(n, m+1+m)+1;
00333         img = cvCreateImage(cvSize(img_width, img_height), IPL_DEPTH_8U, 3);
00334         cvSet(img, cvScalar(64,64,64));
00335         img_legend = cvLoadImage("Legend.png");
00336         if (img_legend) {
00337                 for (img_scale=1; img_scale<50; img_scale++) {
00338                         if (img_scale*img_width > img_legend->width) {
00339                                 break;
00340                         }
00341                 }
00342                 img_show = cvCreateImage(cvSize(img_width*img_scale, img_legend->height + img_height*img_scale), IPL_DEPTH_8U, 3);
00343                 cvSet(img_show, cvScalar(64,64,64));
00344                 cvSetImageROI(img_show, cvRect(0, 0, img_legend->width, img_legend->height));
00345                 cvCopy(img_legend, img_show);
00346                 cvResetImageROI(img_show);
00347                 cvNamedWindow("KalmanVisualize");
00348         } else {
00349                 img_scale = 1;
00350                 img_show = cvCreateImage(cvSize(img_width*img_scale, img_height*img_scale), IPL_DEPTH_8U, 3);
00351                 cvSet(img_show, cvScalar(64,64,64));
00352                 cvNamedWindow("KalmanVisualize",0);
00353         }
00354 }
00355 
00356 void KalmanVisualize::out_matrix(CvMat *m, char *name) {
00357         if (m->cols == 1) {
00358                 std::cout<<name<<" = [";
00359                 for (int j=0; j<m->rows; j++) {
00360                         std::cout<<" "<<cvGet2D(m, j, 0).val[0];
00361                 }
00362                 std::cout<<"]^T"<<std::endl;
00363         } else if (m->rows == 1) {
00364                 std::cout<<name<<" = [";
00365                 for (int i=0; i<m->cols; i++) {
00366                         std::cout<<" "<<cvGet2D(m, 0, i).val[0];
00367                 }
00368                 std::cout<<"]^T"<<std::endl;
00369         } else {
00370                 std::cout<<name<<" = ["<<std::endl;
00371                 for (int j=0; j<m->rows; j++) {
00372                         for (int i=0; i<m->cols; i++) {
00373                                 std::cout<<" "<<cvGet2D(m, j, i).val[0];
00374                         }
00375                         std::cout<<std::endl;
00376                 }
00377                 std::cout<<"]"<<std::endl;
00378         }
00379 }
00380 
00381 KalmanVisualize::KalmanVisualize(Kalman *_kalman, KalmanSensor *_sensor) {
00382         kalman = _kalman;
00383         sensor = _sensor;
00384         kalman_ext = _kalman;
00385         sensor_ext = _sensor;
00386         Init();
00387 }
00388 
00389 KalmanVisualize::KalmanVisualize(KalmanCore *_kalman, KalmanSensorCore *_sensor) {
00390         kalman = _kalman;
00391         sensor = _sensor;
00392         kalman_ext = NULL;
00393         sensor_ext = NULL;
00394         Init();
00395 }
00396 
00397 KalmanVisualize::~KalmanVisualize() {
00398         cvReleaseImage(&img);
00399 }
00400 
00401 void KalmanVisualize::update_pre() {
00402         img_matrix(kalman->x, 1, 1); // 1
00403         if (kalman_ext && sensor_ext) {
00404                 int y = std::max(2+n, 3+m+m);
00405                 img_matrix(kalman_ext->P, 1, y); // n
00406         }
00407 }
00408 
00409 void KalmanVisualize::update_post() {
00410         img_matrix(kalman->F, 3, 1); // n
00411         img_matrix(kalman->x_pred, 4+n, 1); // 1
00412         img_matrix(sensor->H, 6+n, 1); // n
00413         img_matrix(sensor->z_pred, 7+n+n, 1); // 1
00414         img_matrix(sensor->z, 7+n+n, 2 + m);
00415         img_matrix(sensor->z_residual, 9+n+n, 1); // 1
00416         img_matrix(sensor->K, 11+n+n, 1); // m
00417         img_matrix(sensor->x_gain, 12+n+n+m, 1); // 1
00418         img_matrix(kalman->x, 14+n+n+m, 1); // 1
00419         if (kalman_ext && sensor_ext) {
00420                 int y = std::max(2+n, 3+m+m);
00421                 img_matrix(kalman_ext->Q, 2+n, y); // n
00422                 img_matrix(kalman_ext->P_pred, 3+n+n, y); // n
00423                 img_matrix(sensor_ext->R, 4+n+n+n, y); // m
00424                 img_matrix(kalman_ext->P, img->width - 1 - n, y); // n
00425         }
00426         if (img_legend) {
00427                 cvSetImageROI(img_show, cvRect(0, img_legend->height, img->width * img_scale, img->height * img_scale));
00428                 cvResize(img, img_show, CV_INTER_NN);
00429                 cvResetImageROI(img_show);
00430         } else {
00431                 cvResize(img, img_show, CV_INTER_NN);
00432         }
00433 }
00434 
00435 void KalmanVisualize::show() {
00436         //out_matrix(sensor->K, "K");
00437         cvShowImage("KalmanVisualize", img_show);
00438 }
00439 
00440 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02