I'm trying to track pedestrians on a video using Kalman Filter. First, I initialize Kalman with the top-left coordinate (x0,y0) of a pedestrian's bounding box, which is stored within a sample. Next, I call kalmanPredict(), but it returns the same (x0,y0) coordinate I passed.
for (int i = 0; i < samples->size(); i++) {
// retrieves person
sample = samples->at(i);
// Gets the bounding box of a person
Rect rectSample = sample->GetWindow();
// x0 and y0 coordinates of the bounding box
Point2f aux(rectSample.x0,rectSample.y0);
// initializes Kalman with this coordinate
initKalman(aux.x,aux.y);
for (int j = 0; j < 10; j++) {
//Predict
p = kalmanPredict();
s = kalmanCorrect(p.x,p.y);
//crop image and send for shared memory
SSFRect ret(p.x,p.y,rectSample.w,rectSample.h);
Point pt1, pt2, pt3;
pt1.x = p.x;
pt1.y = p.y;
pt2.x = p.x + rectSample.w;
pt2.y = p.y + rectSample.h;
pt3.x = s.x;
pt3.y = s.y;
rectangle(data, pt1, pt2, cv::Scalar(0,255,0), 5);
rectangle(data, pt3, pt2, cv::Scalar(255,0,0), 1);
// show result
imshow("janela", data);
waitKey(30);
aux = pt1;
cout<<i;
}
}
The following code is the setup of Kalman I'm using. I have tried different values for KF.transitionMatrix and KF.measurementMatrix, but prediction kept returning the same value (x0,y0). I'm missing something, but I don't know what.
void initKalman(float x, float y)
{
// Instantate Kalman Filter with
// 4 dynamic parameters and 2 measurement parameters,
// where my measurement is: 2D location of object,
// and dynamic is: 2D location and 2D velocity.
KF.init(4, 2, 0);
measurement = Mat_<float>::zeros(2,1);
measurement.at<float>(0, 0) = x;
measurement.at<float>(1, 0) = y;
KF.statePre.setTo(0);
KF.statePre.at<float>(0, 0) = x;
KF.statePre.at<float>(1, 0) = y;
KF.statePost.setTo(0);
KF.statePost.at<float>(0, 0) = x;
KF.statePost.at<float>(1, 0) = y;
KF.transitionMatrix = (Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); // Including velocity
KF.measurementMatrix = (Mat_<float>(2, 4) << 1,0,0,0, 0,1,0,0);
setIdentity(KF.processNoiseCov, Scalar::all(.005)); //adjust this for faster convergence - but higher noise
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(KF.errorCovPost, Scalar::all(.1));
}
Point kalmanPredict()
{
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
/*This prevents the filter from getting stuck in a state when no corrections are executed.*/
KF.statePre.copyTo(KF.statePost);
KF.errorCovPre.copyTo(KF.errorCovPost);
return predictPt;
}
Point2f kalmanCorrect(float x, float y)
{
measurement(0) = x;
measurement(1) = y;
Mat estimated = KF.correct(measurement);
Point2f statePt(estimated.at<float>(0),estimated.at<float>(1));
return statePt;
}
Why Kalman keeps returning the same prediction value to me?