Hey there :) I currently started working with the Kalmanfilter in OpenCv. I encountered 2 Problems.
First one is, that i used the Example from http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/ . In the video it seems that the noise is reduced by the Kalmanfilter, but if i run my code the noise is not reduced , so if i move my mouse the prediction behaves nearly exactly like the mouse movement without reducing the noise. Is it due to my high sampling rate or are the values of measurementMatrix, processNoiseCov, measurementNoiseCov and errorCovPost set bad? I hope you can understand what i mean.
My other problem is that if i press "n" i want to disable new measurements and i want Kalmanfilter to still guess the new position of the mouse. In http://code.opencv.org/issues/1380 Mircea Popa said:
"but set the measurement matrix to 0 before the kalman.correct() call (it is the duty of the programmer). Doing this, the correction factor (the gain) becomes 0, and the state of the filter is updated based on what it was predicted."
So i tried to do sth like this: measurement = Mat::zeros(2,1, CV_32F) but then just the position of the prediction had been at position (0,0) so not what i expected. Is there something i understood wrong? Isn't measurment the "measurement matrix" that Mircea Popa talked about? Or is there another way to let KalmanFilter predict the new position without measurment?
To make clear what i would expect Kalmanfilter to do: If there is no measurement the movement of the estimated position should be uniform and the direction of movement should be on a straight line that is determinated by the last two Positions.
Here is my Code:
#include <iostream>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
using namespace cv;
using namespace std;
int m_X = 0;
int m_Y = 0;
bool cursorSet = false;
bool noDetection = false;
Mat_<float> lastPosition(2,1);
void drawCross(Mat &img, Point ¢er, Scalar color, int d) {
line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 2, CV_AA, 0);
line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 2, CV_AA, 0);
};
void CallBackFunc(int event, int x, int y, int flags, void* userdata) {
m_X = x;
m_Y = y;
cursorSet = true;
cout << "Mouse move over the window - position (" << x << ", " << y << ")" << endl;
};
void GetCursorPos(Point &mousepos){
mousepos = Point(m_X, m_Y);
};
int main( ) {
//create window and set callback for mouse movement
namedWindow("window", 1);
setMouseCallback("window", CallBackFunc, NULL);
// Image to show mouse tracking
Mat img(600, 800, CV_8UC3);
vector<Point> mousev,kalmanv;
mousev.clear();
kalmanv.clear();
//wait until mouse has an initial position inside the window
while(!cursorSet) {
imshow("window", img);
waitKey(10);
};
//create kalman Filter
KalmanFilter KF(6, 2, 0);
//position of the mouse will be observed
Point mousePos;
GetCursorPos(mousePos);
// intialization of KF...
KF.transitionMatrix = *(Mat_<float>(6, 6) << 1,0,1,0,.5,0, // x + v_x + 1/2 a_x
0,1,0,1,0,0.5, // y + v_Y + 1/2 a_y
0,0,1,0,1,0, // v_x + a_x
0,0,0,1,0,1, // v_y + a_y
0,0,0,0,1,0, // a_x
0,0,0,0,0,1); // a_y
Mat_<float> measurement(2,1); measurement.setTo(Scalar(0));
//initialize the pre state
KF.statePost.at<float>(0) = mousePos.x;
KF.statePost.at<float>(1) = mousePos.y;
KF.statePost.at<float>(2) = 0;
KF.statePost.at<float>(3) = 0;
KF.statePost.at<float>(4) = 0;
KF.statePost.at<float>(5) = 0;
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-1));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-5));
setIdentity(KF.errorCovPost, Scalar::all(1e-3));
while(1) {
img = Scalar::all(0);
// First predict, to update the internal statePre variable
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
// Get mouse point
if (!noDetection) {
GetCursorPos(mousePos);
measurement(0) = mousePos.x;
measurement(1) = mousePos.y;
}else {
measurement(0) = prediction.at<float>(0);
measurement(1) = prediction.at<float>(1);
}
// The update phase3
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at<float>(0),estimated.at<float>(1));
Point measPt(measurement(0),measurement(1));
// draw cross for actual mouse position and kalman guess
mousev.push_back(measPt);
kalmanv.push_back(statePt);
drawCross(img, statePt, Scalar(255,255,255), 5);
drawCross(img, measPt, Scalar(0,0,255), 5 );
// draw lines of movement
for (int i = 0; i < mousev.size()-1; i++)
line(img, mousev[i], mousev[i+1], Scalar(0,0,255), 1);
for (int i = 0; i < kalmanv.size()-1; i++)
line(img, kalmanv[i], kalmanv[i+1], Scalar(255,255,255), 1);
imshow("window", img);
char key = waitKey(10);
if (key == 'c') {
mousev.clear();
kalmanv.clear(); // press c to clear screen
}if (key == 'n') {
noDetection = true; //press n to simulate that no measurement is made
}if (key == 'd') {
noDetection = false;//press d to allow measurements again
}else if(key == 'x') {
break; // press x to exit program
};
}
return 0;
}
Speaking from a more generalized PoV... if you wish to have a Kalman Filter that softens the measurement noise, you have to rely more on the process model and less on the measurement update. So in your case, tweaking the "measurementMatrix, processNoiseCov, measurementNoiseCov" may lead you to a softer output.