tracking one target in a pair of stereo images using kalman

KalmanFilterTracker.cpp

//tracking one target in a pair of stereo images 

#include "KalmanFilterTracker.h"

#include <iostream>
#include "opencv2/opencv.hpp"

using namespace cv;
using namespace std;

KalmanFilterTracker::KalmanFilterTracker()
{
	KF = KalmanFilter(16, 8, 0);
	state = Mat(16, 1, CV_32F);
	precessNoise = Mat(16, 1, CV_32F);
	measurement = Mat(8, 1, CV_32F);
	measurement.setTo(Scalar(0));
	init = true;
}

KalmanFilterTracker::~KalmanFilterTracker() {}

void KalmanFilterTracker::initilizeKF(Rect recl, Rect recr) 
{
	KF.statePre.at<float>(0) = recl.x;
	KF.statePre.at<float>(1) = recl.y;
	KF.statePre.at<float>(2) = recl.width;
	KF.statePre.at<float>(3) = recl.height;
	KF.statePre.at<float>(4) = recr.x;
	KF.statePre.at<float>(5) = recr.y;
	KF.statePre.at<float>(6) = recr.width;
	KF.statePre.at<float>(7) = recr.height;
	KF.statePre.at<float>(8) = 0;
	KF.statePre.at<float>(9) = 0;
	KF.statePre.at<float>(10) = 0;
	KF.statePre.at<float>(11) = 0;
	KF.statePre.at<float>(12) = 0;
	KF.statePre.at<float>(13) = 0;
	KF.statePre.at<float>(14) = 0;
	KF.statePre.at<float>(15) = 0;
	KF.transitionMatrix = *(Mat_<float>(16, 16) <<	1,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0, //cross-over control for x valve
							0,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,
							0,0,1,0,0,0,0,0,0,0,1,0,0,0,0,0,
							0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,0,
							0,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,
							0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,
							0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,
							0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,
							0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,
							0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,
							0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,
							0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,
							0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,
							0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,
							0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,
							0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
	);

	setIdentity(KF.measurementMatrix);
	setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
	setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
	setIdentity(KF.errorCovPost, Scalar::all(.1));

	predictl_vector.clear();
	predictr_vector.clear();
	init = false;
}

void KalmanFilterTracker::track(Rect recl, Rect recr)
{
    	if ( init )
        	initilizeKF(recl, recr);

	measurement.at<float>(0) = recl.x;
    	measurement.at<float>(1) = recl.y;
	measurement.at<float>(2) = recl.width;
	measurement.at<float>(3) = recl.height;
	measurement.at<float>(4) = recr.x;
    	measurement.at<float>(5) = recr.y;
	measurement.at<float>(6) = recr.width;
	measurement.at<float>(7) = recr.height;

    	Mat estimated = KF.correct(measurement);
	Mat prediction = KF.predict();

    	Rect predictlPt(prediction.at<float>(0), prediction.at<float>(1),prediction.at<float>(2),prediction.at<float>(3));
	Rect predictrPt(prediction.at<float>(4), prediction.at<float>(5),prediction.at<float>(6),prediction.at<float>(7));
	predictl_vector.push_back(predictlPt);
	predictr_vector.push_back(predictrPt);

	vector<Rect>::iterator itl = predictl_vector.begin();
	vector<Rect>::iterator itr = predictr_vector.begin();
	if (predictl_vector.end()-predictl_vector.begin() > 5)
	{
		predictl_vector.erase(itl);
		predictr_vector.erase(itr);
	}//release old data
}

void KalmanFilterTracker::draw(Mat img)
{
    	for (int i=0; i<predictl_vector.size()-1; i++) 
	{
		line(img, Point(predictl_vector[i].x + predictl_vector[i].width/2 ,predictl_vector[i].y + predictl_vector[i].height/2),  Point(predictl_vector[i+1].x + predictl_vector[i+1].width/2 ,predictl_vector[i+1].y + predictl_vector[i+1].height/2), Scalar(0,255,0), 1);
    	}
}

KalmanFilterTracker.h

#ifndef KALMANFILTERTRACKER_H
#define KALMANFILTERTRACKER_H

#include <iostream>
#include "opencv2/opencv.hpp"

using namespace cv;
using namespace std;

class KalmanFilterTracker
{
    public:
        KalmanFilter KF;
        Mat state;
        Mat precessNoise;
        Mat measurement;
        vector<Rect>predictr_vector, predictl_vector;
        bool init;

        KalmanFilterTracker();
        virtual ~KalmanFilterTracker();
        void track(Rect recr, Rect recl);
        void initilizeKF(Rect recr, Rect recl);
        void draw(Mat img);
    protected:
    private:
};

#endif // KALMANFILTERTRACKER_H