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 "../mobile_robot.h" #include using namespace MatrixWrapper; /* The purpose of this program is to construct a kalman filter for the problem The necessary SYSTEM MODEL is: x_k = x_{k-1} + v_{k-1} * cos(theta) * delta_t The used MEASUREMENT MODEL: set WALL_CT = 1/sqrt(pow(a,2) + 1) where H = [ WALL_CT * a - WALL_CT 0 ] */ int main(int argc, char** argv) char path1[] = "C:\\Temp\\Research\\Input\\IKONOS\\ikonos_xy00_d4.bmp"; IplImage *Yk; cvNamedWindow( "window_Yk", CV_WINDOW_AUTOSIZE); /**************************** // Create the matrices A and B for the linear system model vector // create gaussian SymmetricMatrix sysNoise_Cov(2); Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov); // create the model /********************************* // create matrix H for linear measurement model // Construct the measurement noise (a scalar in this case) SymmetricMatrix measNoise_Cov(1); // create the model /**************************** /****************************** /*************************** /******************* ofstream SaveFile("Output.txt"); // DO ONE MEASUREMENT // UPDATE FILTER Pdf SaveFile < //return 0; } // estimation loop SaveFile.close(); Pdf cout << "======================================================" << endl getchar();
#include
#include
using namespace BFL;
using namespace std;
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)
y_k = y_{k-1} + v_{k-1} * sin(theta) * delta_t
measuring the (perpendicular) distance z to the wall y = ax + b
z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
or Z = H * X_k + J * U_k
and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)
// Implementation of the Paper "Three State Extended Kalman Filter for Mobile Robot
// Localization"
{
cerr << "==================================================" << endl
<< "Test of kalman filter" << endl
<< "Mobile robot localisation example" << endl
<< "==================================================" << endl;
Yk = cvLoadImage(path1);
cvShowImage("window_Yk",Yk);
* 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;
AB[0] = A;
AB[1] = B;
ColumnVector sysNoise_Mu(2);
sysNoise_Mu(1) = 0.0;
sysNoise_Mu(2) = 0.0;
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);
LinearAnalyticConditionalGaussian sys_pdf(AB, system_Uncertainty);
LinearAnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
* Initialise measurement model *
********************************/
Matrix H(1,2);
H(1,1) = 0;
H(1,2) = 2;
ColumnVector measNoise_Mu(1);
measNoise_Mu(1) = 0.0;
measNoise_Cov(1,1) = pow(0.05,2);
Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);
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;
for (time_step = 0; time_step < 200; time_step++)
{
// DO ONE STEP WITH MOBILE ROBOT
mobile_robot.Move(input);
ColumnVector measurement = mobile_robot.Measure();
filter.Update(&sys_model,input,&meas_model,measurement);
// cout << "After " << time_step+1 << " timesteps " << endl;
//cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
// << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
//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;
<< "End of the Kalman filter for mobile robot localisation" << endl
<< "======================================================"
<< endl;
cvDestroyWindow("window_Yk");
cvReleaseImage(&Yk);
return 0;
}
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