Problems with BF and OpenCV

Dear Friends:

I was trying to insert a few commands of OpenCV, to the linear kalman filter example, in orden to read and show up an image but the image is not showed, only appears a white windows.

Here is the code that I am using,

Thanks a lot in advance for your help,

Aldo
Note: I am using Visual C++ 2005, Window XP

// $Id: kalman_mobile.cpp 5925 2006-03-14 21:23:49Z tdelaet $
// Copyright (C) 2006 Tinne De Laet
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

/* Demonstration program for the Bayesian Filtering Library.
Mobile robot localization with respect to wall with different possibilities for filter
*/
#include
#include
#include

#include

#include
#include

#include
#include

#include "../mobile_robot.h"

#include
#include
#include

using namespace MatrixWrapper;
using namespace BFL;
using namespace std;

/* The purpose of this program is to construct a kalman filter for the problem
of localisation of a mobile robot equipped with an ultrasonic sensor.
In this case the orientation is known, which simplifies the model considerably:
The system model will become linear.
The ultrasonic measures the distance to the wall (it can be switched off:
see mobile_robot_wall_cts.h)

The necessary SYSTEM MODEL is:

x_k = x_{k-1} + v_{k-1} * cos(theta) * delta_t
y_k = y_{k-1} + v_{k-1} * sin(theta) * delta_t

The used MEASUREMENT MODEL:
measuring the (perpendicular) distance z to the wall y = ax + b

set WALL_CT = 1/sqrt(pow(a,2) + 1)
z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
or Z = H * X_k + J * U_k

where

H = [ WALL_CT * a - WALL_CT 0 ]
and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)

*/
// Implementation of the Paper "Three State Extended Kalman Filter for Mobile Robot
// Localization"

int main(int argc, char** argv)
{
cerr << "==================================================" << endl
<< "Test of kalman filter" << endl
<< "Mobile robot localisation example" << endl
<< "==================================================" << endl;

char path1[] = "C:\\Temp\\Research\\Input\\IKONOS\\ikonos_xy00_d4.bmp";

IplImage *Yk;
Yk = cvLoadImage(path1);

cvNamedWindow( "window_Yk", CV_WINDOW_AUTOSIZE);
cvShowImage("window_Yk",Yk);

/****************************
* Linear system model *
***************************/

// Create the matrices A and B for the linear system model
Matrix A(2,2);
A(1,1) = 1.0;
A(1,2) = 0.0;
A(2,1) = 0.0;
A(2,2) = 1.0;
Matrix B(2,2);
B(1,1) = cos(0.8);
B(1,2) = 0.0;
B(2,1) = sin(0.8);
B(2,2) = 0.0;

vector AB(2);
AB[0] = A;
AB[1] = B;

// create gaussian
ColumnVector sysNoise_Mu(2);
sysNoise_Mu(1) = 0.0;
sysNoise_Mu(2) = 0.0;

SymmetricMatrix sysNoise_Cov(2);
sysNoise_Cov(1,1) = pow(0.01,2);
sysNoise_Cov(1,2) = 0.0;
sysNoise_Cov(2,1) = 0.0;
sysNoise_Cov(2,2) = pow(0.01,2);

Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);

// create the model
LinearAnalyticConditionalGaussian sys_pdf(AB, system_Uncertainty);
LinearAnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);

/*********************************
* Initialise measurement model *
********************************/

// create matrix H for linear measurement model
Matrix H(1,2);
H(1,1) = 0;
H(1,2) = 2;

// Construct the measurement noise (a scalar in this case)
ColumnVector measNoise_Mu(1);
measNoise_Mu(1) = 0.0;

SymmetricMatrix measNoise_Cov(1);
measNoise_Cov(1,1) = pow(0.05,2);
Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);

// create the model
LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);

/****************************
* Linear prior DENSITY *
***************************/
// Continuous Gaussian prior (for Kalman filters)
ColumnVector prior_Mu(2);
prior_Mu(1) = -1.0;
prior_Mu(2) = 1.0;
SymmetricMatrix prior_Cov(2);
prior_Cov(1,1) = 1.0;
prior_Cov(1,2) = 0.0;
prior_Cov(2,1) = 0.0;
prior_Cov(2,2) = 1.0;
Gaussian prior_cont(prior_Mu,prior_Cov);

/******************************
* Construction of the Filter *
******************************/
ExtendedKalmanFilter filter(&prior_cont);

/***************************
* initialise MOBILE ROBOT *
**************************/
// Model of mobile robot in world with one wall
// The model is used to simultate the distance measurements.
MobileRobot mobile_robot;
ColumnVector input(2);
input(1) = 0.1;
input(2) = 0.0;

/*******************
* ESTIMATION LOOP *
*******************/
cout << "MAIN: Starting estimation" << endl;
cout << " The Estimated Value of the Position is: " << endl;
unsigned int time_step;

ofstream SaveFile("Output.txt");
for (time_step = 0; time_step < 200; time_step++)
{
// DO ONE STEP WITH MOBILE ROBOT
mobile_robot.Move(input);

// DO ONE MEASUREMENT
ColumnVector measurement = mobile_robot.Measure();

// UPDATE FILTER
filter.Update(&sys_model,input,&meas_model,measurement);

Pdf * posterior = filter.PostGet();
// cout << "After " << time_step+1 << " timesteps " << endl;
//cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
// << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;

SaveFile <
ExpectedValueGet()(1)<<"\t"<
ExpectedValueGet()(2)<< endl;

//return 0;

} // estimation loop

SaveFile.close();

Pdf * posterior = filter.PostGet();
//ColumnVector test(2);
//test(1) = 0.0;
//test(2) = 0.0;
cout << "After " << time_step+1 << " timesteps " << endl;
cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
<< " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
cout << " X = " << posterior->ExpectedValueGet()(1) << endl;
cout << " Y = " << posterior->ExpectedValueGet()(2) << endl;

cout << "======================================================" << endl
<< "End of the Kalman filter for mobile robot localisation" << endl
<< "======================================================"
<< endl;

getchar();
cvDestroyWindow("window_Yk");
cvReleaseImage(&Yk);
return 0;
}

bernhardkleiner's picture

Example for a working project which includes BFL and OPENCV

Hello,
Currently I am workin with both libraries and I guess your problem is the implementation of the OPENCV code.
I use a linux system and installed both libraries as devian packages.
I guess you forget to implement the cvWaitKey(..) function in the loop. Otherwise your Image will not be drawn in the Window.
Here is part of my code:

{
#include
#include
#include

cvNamedWindow("Kalman Tracking");
cvNamedWindow("Feature");

/* INITIALIZATION OF KALMANFILTER */
......
......
/* ESTIMATION LOOP */

int c;
for (;;)
{
// UPDATE FILTER with SYSTEMMODEL
filter.Update(&sys_model);

pred_cont = filter.PostGet();

exp_pred_Mu = pred_cont->ExpectedValueGet();
exp_pred_Cov = pred_cont->CovarianceGet();

_first.x = (int)exp_pred_Mu(1) - (int) sqrt(exp_pred_Cov(1,1)) - (int)(BOXSIZE * 2);// * 0.5);
_first.y = (int)exp_pred_Mu(2) - (int) sqrt(exp_pred_Cov(2,2)) - (int)(BOXSIZE * 2);// * 0.5);
_pt.x = (int)exp_pred_Mu(1) + (int) sqrt(exp_pred_Cov(1,1)) + (int)(BOXSIZE * 2);// * 0.5);
_pt.y = (int)exp_pred_Mu(2) + (int) sqrt(exp_pred_Cov(2,2)) + (int)(BOXSIZE * 2);// * 0.5);

//K = exp_pred_Cov * H.transpose() * (H * exp_pred_Cov * H.transpose() + measNoise_Cov).inverse();

measurement(&newImg);

CvPoint onecorner; // temporally memory for each point

onecorner.x = (int)(corners[0].x);
onecorner.y = (int)(corners[0].y);

// Mark the best found features with circles
cvCircle(newImg,cvPoint(onecorner.x,onecorner.y),BOXSIZE,CV_RGB(0,255,0),2,8,0);

cvRectangle(newImg,_first,_pt,CV_RGB(255,0,0),2,8,0);

cvShowImage("Kalman Tracking",newImg);
cvShowImage("Feature",resizeFeatureImg);

ColumnVector measurement(2);
measurement(1) = (int)(corners[0].x);
measurement(2) = (int)(corners[0].y);

// UPDATE FILTER with MEASUREMENT
filter.Update(&meas_model,measurement);

post_cont = filter.PostGet();

exp_post_Mu = post_cont->ExpectedValueGet();
exp_post_Cov = post_cont->CovarianceGet();

c = cvWaitKey(5); // WAIT 5ms for KEYEVENT
// THIS IS NECESSARY, OTHERWISE THE PICTURE IN THE WINDOW WILL NOT BE DRAWN !
if (c == 27) break;
} // estimation loop

}

Best regards,

Bernhard Kleiner, KU-Leuven