I wanted to put up a quick note on how to use Kalman Filters in OpenCV 2.2 with the C++ API, because all I could find online was using the old C API. Plus the kalman.cpp example that ships with OpenCV is kind of crappy and really doesn’t explain how to use the Kalman Filter.
I’m no expert on Kalman filters though, this is just a quick hack I got going as a test for a project. It worked, so I’m posting the results.
So I wanted to do a 2D tracker that is more immune to noise. For that I set up a Kalman filter with 4 dynamic parameters and 2 measurement parameters (no control), where my measurement is: 2D location of object, and dynamic is: 2D location and 2D velocity. Pretty simple, and it makes the transition matrix also simple.
KalmanFilter KF(4, 2, 0); KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); Mat_<float> measurement(2,1); measurement.setTo(Scalar(0)); // init... KF.statePre.at<float>(0) = mouse_info.x; KF.statePre.at<float>(1) = mouse_info.y; KF.statePre.at<float>(2) = 0; KF.statePre.at<float>(3) = 0; setIdentity(KF.measurementMatrix); setIdentity(KF.processNoiseCov, Scalar::all(1e-4)); setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); setIdentity(KF.errorCovPost, Scalar::all(.1));
Cool, moving on to the dynamic part.
So I set up a mouse callback to get the mouse position every “frame” (a 100ms wait), and feed that into the filter:
// 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 measurement(0) = mouse_info.x; measurement(1) = mouse_info.y; Point measPt(measurement(0),measurement(1)); // The "correct" phase that is going to use the predicted value and our measurement Mat estimated = KF.correct(measurement); Point statePt(estimated.at<float>(0),estimated.at<float>(1));
All the rest is garnish (see the code)..
The important bit is to see that Predict() happens before Correct(). This is according to the excellent Kalman filter tutorial I found. Look carefully at Figure 1-2!! It will sort you out. Also take a look at OpenCV’s internal impl of Kalman, see that it follows these steps closely. Especially
Mat& KalmanFilter::predict(const Mat& control) and
Mat& KalmanFilter::correct(const Mat& measurement).
Another good place I found that helped me formulate the parameters for the filter is this place. Again, take everything with a grain of salt, because Kalman Filters are very versatile you just need to know how to formulate them right.
As usual, grab the code off the SVN:
svn co http://morethantechnical.googlecode.com/svn/trunk/mouse_kalman/main.cpp