Ctracker.cpp

#include "Ctracker.h"
using namespace cv;
using namespace std;

size_t CTrack::NextTrackID=0;
// ---------------------------------------------------------------------------
// Êîíñòðóêòîð òðåêà.
// Ïðè ñîçäàíèè, òðåê íà÷èíàåòñÿ ñ êàêîé òî òî÷êè,
// ýòà òî÷êà è ïåðåäàåòñÿ êîíñòðóêòîðó â êà÷åñòâå àðãóìåíòà.
// ---------------------------------------------------------------------------
CTrack::CTrack(Point2f pt, float dt, float Accel_noise_mag)
{
	track_id=NextTrackID;

	NextTrackID++;
	// Êàæäûé òðåê èìååò ñâîé ôèëüòð Êàëüìàíà,
	// ïðè ïîìîùè êîòîðîãî äåëàåòñÿ ïðîãíîç, ãäå äîëæíà áûòü ñëåäóþùàÿ òî÷êà.
	KF = new TKalmanFilter(pt,dt,Accel_noise_mag);
	// Çäåñü õðàíÿòñÿ êîîðäèíàòû òî÷êè, â êîòîðîé òðåê ïðîãíîçèðóåò ñëåäóþùåå íàáëþäåíèå (äåòåêò).
	prediction=pt;
	skipped_frames=0;
}
// ---------------------------------------------------------------------------
//
// ---------------------------------------------------------------------------
CTrack::~CTrack()
{
	// Îñâîáîæäàåì ôèëüòð Êàëüìàíà.
	delete KF;
}

// ---------------------------------------------------------------------------
// Òðåêåð. Ïðîèçâîäèò óïðàâëåíèå òðåêàìè. Ñîçäàåò, óäàëÿåò, óòî÷íÿåò.
// ---------------------------------------------------------------------------
CTracker::CTracker(float _dt, float _Accel_noise_mag, double _dist_thres, int _maximum_allowed_skipped_frames,int _max_trace_length)
{
dt=_dt;
Accel_noise_mag=_Accel_noise_mag;
dist_thres=_dist_thres;
maximum_allowed_skipped_frames=_maximum_allowed_skipped_frames;
max_trace_length=_max_trace_length;
}
// ---------------------------------------------------------------------------
//
// ---------------------------------------------------------------------------
void CTracker::Update(vector<Point2d>& detections)
{
	// -----------------------------------
	// Åñëè òðåêîâ åùå íåò, òî íà÷íåì äëÿ êàæäîé òî÷êè ïî òðåêó
	// -----------------------------------
	if(tracks.size()==0)
	{
		// Åñëè åùå íåò íè îäíîãî òðåêà
		for(int i=0;i<detections.size();i++)
		{
			CTrack* tr=new CTrack(detections[i],dt,Accel_noise_mag);
			tracks.push_back(tr);
		}	
	}

	// -----------------------------------
	// Çäåñü òðåêè óæå åñòü â ëþáîì ñëó÷àå
	// -----------------------------------
	int N=tracks.size();		// òðåêè
	int M=detections.size();	// äåòåêòû

	// Ìàòðèöà ðàññòîÿíèé îò N-íîãî òðåêà äî M-íîãî äåòåêòà.
	vector< vector<double> > Cost(N,vector<double>(M));
	vector<int> assignment; // íàçíà÷åíèÿ

	// -----------------------------------
	// Òðåêè óæå åñòü, ñîñòàâèì ìàòðèöó ðàññòîÿíèé
	// -----------------------------------
	double dist;
	for(int i=0;i<tracks.size();i++)
	{	
		// Point2d prediction=tracks[i]->prediction;
		// cout << prediction << endl;
		for(int j=0;j<detections.size();j++)
		{
			Point2d diff=(tracks[i]->prediction-detections[j]);
			dist=sqrtf(diff.x*diff.x+diff.y*diff.y);
			Cost[i][j]=dist;
		}
	}
	// -----------------------------------
	// Ðåøàåì çàäà÷ó î íàçíà÷åíèÿõ (òðåêè è ïðîãíîçû ôèëüòðà)
	// -----------------------------------
	AssignmentProblemSolver APS;
	APS.Solve(Cost,assignment,AssignmentProblemSolver::optimal);

	// -----------------------------------
	// ïî÷èñòèì assignment îò ïàð ñ áîëüøèì ðàññòîÿíèåì
	// -----------------------------------
	// Íå íàçíà÷åííûå òðåêè
	vector<int> not_assigned_tracks;

	for(int i=0;i<assignment.size();i++)
	{
		if(assignment[i]!=-1)
		{
			if(Cost[i][assignment[i]]>dist_thres)
			{
				assignment[i]=-1;
				// Îòìå÷àåì íåíàçíà÷åííûå òðåêè, è óâåëè÷èâàåì ñ÷åò÷èê ïðîïóùåíûõ êàäðîâ,
				// êîãäà êîëè÷åñòâî ïðîïóùåííûõ êàäðîâ ïðåâûñèò ïîðîãîâîå çíà÷åíèå, òðåê ñòèðàåòñÿ.
				not_assigned_tracks.push_back(i);
			}
		}
		else
		{			
			// Åñëè òðåêó íå íàçíà÷åí äåòåêò, òî óâåëè÷èâàåì ñ÷åò÷èê ïðîïóùåíûõ êàäðîâ.
			tracks[i]->skipped_frames++;
		}

	}

	// -----------------------------------
	// Åñëè òðåê äîëãî íå ïîëó÷àåò äåòåêòîâ, óäàëÿåì
	// -----------------------------------
	for(int i=0;i<tracks.size();i++)
	{
		if(tracks[i]->skipped_frames>maximum_allowed_skipped_frames)
		{
			delete tracks[i];
			tracks.erase(tracks.begin()+i);
			assignment.erase(assignment.begin()+i);
			i--;
		}
	}
	// -----------------------------------
	// Âûÿâëÿåì íåíàçíà÷åííûå äåòåêòû
	// -----------------------------------
	vector<int> not_assigned_detections;
	vector<int>::iterator it;
	for(int i=0;i<detections.size();i++)
	{
		it=find(assignment.begin(), assignment.end(), i);
		if(it==assignment.end())
		{
			not_assigned_detections.push_back(i);
		}
	}

	// -----------------------------------
	// è íà÷èíàåì äëÿ íèõ íîâûå òðåêè
	// -----------------------------------
	if(not_assigned_detections.size()!=0)
	{
		for(int i=0;i<not_assigned_detections.size();i++)
		{
			CTrack* tr=new CTrack(detections[not_assigned_detections[i]],dt,Accel_noise_mag);
			tracks.push_back(tr);
		}	
	}

	// Àïäåéòèì ñîñòîÿíèå ôèëüòðîâ

	for(int i=0;i<assignment.size();i++)
	{
		// Åñëè òðåê àïäåéòèëñÿ ìåíüøå îäíîãî ðàçà, òî ñîñòîÿíèå ôèëüòðà íåêîððåêòíî.

		tracks[i]->KF->GetPrediction();

		if(assignment[i]!=-1) // Åñëè íàçíà÷åíèå åñòü òî àïäåéòèì ïî íåìó
		{
			tracks[i]->skipped_frames=0;
			tracks[i]->prediction=tracks[i]->KF->Update(detections[assignment[i]],1);
		}else				  // Åñëè íåò, òî ïðîäîëæàåì ïðîãíîçèðîâàòü
		{
			tracks[i]->prediction=tracks[i]->KF->Update(Point2f(0,0),0);	
		}
		
		if(tracks[i]->trace.size()>max_trace_length)
		{
			tracks[i]->trace.erase(tracks[i]->trace.begin(),tracks[i]->trace.end()-max_trace_length);
		}

		tracks[i]->trace.push_back(tracks[i]->prediction);
		tracks[i]->KF->LastResult=tracks[i]->prediction;
	}

}
// ---------------------------------------------------------------------------
//
// ---------------------------------------------------------------------------
CTracker::~CTracker(void)
{
	for(int i=0;i<tracks.size();i++)
	{
	delete tracks[i];
	}
	tracks.clear();
}