I am trying to track a detected object and the code I am using as follows:
// if there is a detection
if (detectedObjectRectanglesPreviousFrame.size() != 0)
{
cout << "#: " << detectedObjectRectanglesPreviousFrame.size() << endl;
//-----------------------------------------------------------
KalmanFilter KF(4, 2, 0);
Mat_<float> state(4, 1); /* (x, y, Vx, Vy) */
Mat processNoise(4, 1, CV_32F);
Mat_<float> measurement(2, 1);
measurement.setTo(Scalar(0));
//-----------------------------------------------------------
for (int i = 0; i < detectedObjectRectanglesPreviousFrame.size(); i++)
{
// Get the center coordinates of the detected object
int trackedX = detectedObjectRectanglesPreviousFrame[i].x + detectedObjectRectanglesPreviousFrame[i].width / 2;
int trackedY = detectedObjectRectanglesPreviousFrame[i].y + detectedObjectRectanglesPreviousFrame[i].height / 2;
cout << "P" << Point(trackedX, trackedY) << endl;
// if this is the first detection, initialize kalman filter
if (firstDetection == true)
{
//---------------------------------------------------------------------------------------------
KF.statePre.at<float>(0) = trackedX;
KF.statePre.at<float>(1) = trackedY;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
KF.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
//KF.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
setIdentity(KF.transitionMatrix);
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-4));
setIdentity(KF.errorCovPost, Scalar::all(.1));
points_vector.clear();
kalman_vector.clear();
//---------------------------------------------------------------------------------------------
cout << "First time-> Initialize: " << trackedX << "," << trackedY << endl;
firstDetection = false;
}
// if not, start tracking
else
{
//---------------------------------------------------------------------------------------------
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0), prediction.at<float>(1));
KF.statePre.copyTo(KF.statePost);
KF.errorCovPre.copyTo(KF.errorCovPost);
measurement(0) = trackedX;
measurement(1) = trackedY;
Point measPt(measurement(0), measurement(1));
points_vector.push_back(measPt);
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at<float>(0), estimated.at<float>(1));
kalman_vector.push_back(statePt);
for (int i = 0; i < kalman_vector.size() - 1; i++)
{
line(sourceFrame, kalman_vector[i], kalman_vector[i + 1], Scalar(0, 255, 0), 1);
}
for (int j = 0; j < kalman_vector.size(); j++)
{
cout << "Kalman Vector Element: " << kalman_vector[j] << endl;
}
//---------------------------------------------------------------------------------------------
}
}
}
And unfortunately, the output I am getting is :
#: 1
P[261, 259]
First time-> Initialize: 261,259
#: 1
P[260, 259]
Kalman Vector Element: [0, 0]
#: 1
P[258, 260]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[254, 263]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[255, 263]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[254, 262]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[252, 262]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[252, 263]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
As you can see the kalman vector is always filled up with [0,0], why doesn't it properly predict? Is there something wrong with the loops or?