first commit

nodes/track/deep_sort/README.md

@@ -0,0 +1,6 @@
+
+
+summary
+-------------
+
+deep sort algorithm for tarcking

nodes/track/sort/Hungarian.cpp

@@ -0,0 +1,392 @@
+///////////////////////////////////////////////////////////////////////////////
+// Hungarian.cpp: Implementation file for Class HungarianAlgorithm.
+// 
+// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren.
+// The original implementation is a few mex-functions for use in MATLAB, found here:
+// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem
+// 
+// Both this code and the orignal code are published under the BSD license.
+// by Cong Ma, 2016
+// 
+
+#include "Hungarian.h"
+#include <cmath>
+#include <cfloat>
+HungarianAlgorithm::HungarianAlgorithm(){}
+HungarianAlgorithm::~HungarianAlgorithm(){}
+
+
+//********************************************************//
+// A single function wrapper for solving assignment problem.
+//********************************************************//
+double HungarianAlgorithm::Solve(vector<vector<double>>& DistMatrix, vector<int>& Assignment)
+{
+	unsigned int nRows = DistMatrix.size();
+	unsigned int nCols = DistMatrix[0].size();
+
+	double *distMatrixIn = new double[nRows * nCols];
+	int *assignment = new int[nRows];
+	double cost = 0.0;
+
+	// Fill in the distMatrixIn. Mind the index is "i + nRows * j".
+	// Here the cost matrix of size MxN is defined as a double precision array of N*M elements. 
+	// In the solving functions matrices are seen to be saved MATLAB-internally in row-order.
+	// (i.e. the matrix [1 2; 3 4] will be stored as a vector [1 3 2 4], NOT [1 2 3 4]).
+	for (unsigned int i = 0; i < nRows; i++)
+		for (unsigned int j = 0; j < nCols; j++)
+			distMatrixIn[i + nRows * j] = DistMatrix[i][j];
+	
+	// call solving function
+	assignmentoptimal(assignment, &cost, distMatrixIn, nRows, nCols);
+
+	Assignment.clear();
+	for (unsigned int r = 0; r < nRows; r++)
+		Assignment.push_back(assignment[r]);
+
+	delete[] distMatrixIn;
+	delete[] assignment;
+	return cost;
+}
+
+
+//********************************************************//
+// Solve optimal solution for assignment problem using Munkres algorithm, also known as Hungarian Algorithm.
+//********************************************************//
+void HungarianAlgorithm::assignmentoptimal(int *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
+{
+	double *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue;
+	bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix;
+	int nOfElements, minDim, row, col;
+
+	/* initialization */
+	*cost = 0;
+	for (row = 0; row<nOfRows; row++)
+		assignment[row] = -1;
+
+	/* generate working copy of distance Matrix */
+	/* check if all matrix elements are positive */
+	nOfElements = nOfRows * nOfColumns;
+	distMatrix = (double *)malloc(nOfElements * sizeof(double));
+	distMatrixEnd = distMatrix + nOfElements;
+
+	for (row = 0; row<nOfElements; row++)
+	{
+		value = distMatrixIn[row];
+		if (value < 0)
+			cerr << "All matrix elements have to be non-negative." << endl;
+		distMatrix[row] = value;
+	}
+
+
+	/* memory allocation */
+	coveredColumns = (bool *)calloc(nOfColumns, sizeof(bool));
+	coveredRows = (bool *)calloc(nOfRows, sizeof(bool));
+	starMatrix = (bool *)calloc(nOfElements, sizeof(bool));
+	primeMatrix = (bool *)calloc(nOfElements, sizeof(bool));
+	newStarMatrix = (bool *)calloc(nOfElements, sizeof(bool)); /* used in step4 */
+
+	/* preliminary steps */
+	if (nOfRows <= nOfColumns)
+	{
+		minDim = nOfRows;
+
+		for (row = 0; row<nOfRows; row++)
+		{
+			/* find the smallest element in the row */
+			distMatrixTemp = distMatrix + row;
+			minValue = *distMatrixTemp;
+			distMatrixTemp += nOfRows;
+			while (distMatrixTemp < distMatrixEnd)
+			{
+				value = *distMatrixTemp;
+				if (value < minValue)
+					minValue = value;
+				distMatrixTemp += nOfRows;
+			}
+
+			/* subtract the smallest element from each element of the row */
+			distMatrixTemp = distMatrix + row;
+			while (distMatrixTemp < distMatrixEnd)
+			{
+				*distMatrixTemp -= minValue;
+				distMatrixTemp += nOfRows;
+			}
+		}
+
+		/* Steps 1 and 2a */
+		for (row = 0; row<nOfRows; row++)
+			for (col = 0; col<nOfColumns; col++)
+				if (fabs(distMatrix[row + nOfRows*col]) < DBL_EPSILON)
+					if (!coveredColumns[col])
+					{
+						starMatrix[row + nOfRows*col] = true;
+						coveredColumns[col] = true;
+						break;
+					}
+	}
+	else /* if(nOfRows > nOfColumns) */
+	{
+		minDim = nOfColumns;
+
+		for (col = 0; col<nOfColumns; col++)
+		{
+			/* find the smallest element in the column */
+			distMatrixTemp = distMatrix + nOfRows*col;
+			columnEnd = distMatrixTemp + nOfRows;
+
+			minValue = *distMatrixTemp++;
+			while (distMatrixTemp < columnEnd)
+			{
+				value = *distMatrixTemp++;
+				if (value < minValue)
+					minValue = value;
+			}
+
+			/* subtract the smallest element from each element of the column */
+			distMatrixTemp = distMatrix + nOfRows*col;
+			while (distMatrixTemp < columnEnd)
+				*distMatrixTemp++ -= minValue;
+		}
+
+		/* Steps 1 and 2a */
+		for (col = 0; col<nOfColumns; col++)
+			for (row = 0; row<nOfRows; row++)
+				if (fabs(distMatrix[row + nOfRows*col]) < DBL_EPSILON)
+					if (!coveredRows[row])
+					{
+						starMatrix[row + nOfRows*col] = true;
+						coveredColumns[col] = true;
+						coveredRows[row] = true;
+						break;
+					}
+		for (row = 0; row<nOfRows; row++)
+			coveredRows[row] = false;
+
+	}
+
+	/* move to step 2b */
+	step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+
+	/* compute cost and remove invalid assignments */
+	computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
+
+	/* free allocated memory */
+	free(distMatrix);
+	free(coveredColumns);
+	free(coveredRows);
+	free(starMatrix);
+	free(primeMatrix);
+	free(newStarMatrix);
+
+	return;
+}
+
+/********************************************************/
+void HungarianAlgorithm::buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns)
+{
+	int row, col;
+
+	for (row = 0; row<nOfRows; row++)
+		for (col = 0; col<nOfColumns; col++)
+			if (starMatrix[row + nOfRows*col])
+			{
+#ifdef ONE_INDEXING
+				assignment[row] = col + 1; /* MATLAB-Indexing */
+#else
+				assignment[row] = col;
+#endif
+				break;
+			}
+}
+
+/********************************************************/
+void HungarianAlgorithm::computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows)
+{
+	int row, col;
+
+	for (row = 0; row<nOfRows; row++)
+	{
+		col = assignment[row];
+		if (col >= 0)
+			*cost += distMatrix[row + nOfRows*col];
+	}
+}
+
+/********************************************************/
+void HungarianAlgorithm::step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
+{
+	bool *starMatrixTemp, *columnEnd;
+	int col;
+
+	/* cover every column containing a starred zero */
+	for (col = 0; col<nOfColumns; col++)
+	{
+		starMatrixTemp = starMatrix + nOfRows*col;
+		columnEnd = starMatrixTemp + nOfRows;
+		while (starMatrixTemp < columnEnd){
+			if (*starMatrixTemp++)
+			{
+				coveredColumns[col] = true;
+				break;
+			}
+		}
+	}
+
+	/* move to step 3 */
+	step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+}
+
+/********************************************************/
+void HungarianAlgorithm::step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
+{
+	int col, nOfCoveredColumns;
+
+	/* count covered columns */
+	nOfCoveredColumns = 0;
+	for (col = 0; col<nOfColumns; col++)
+		if (coveredColumns[col])
+			nOfCoveredColumns++;
+
+	if (nOfCoveredColumns == minDim)
+	{
+		/* algorithm finished */
+		buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
+	}
+	else
+	{
+		/* move to step 3 */
+		step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+	}
+
+}
+
+/********************************************************/
+void HungarianAlgorithm::step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
+{
+	bool zerosFound;
+	int row, col, starCol;
+
+	zerosFound = true;
+	while (zerosFound)
+	{
+		zerosFound = false;
+		for (col = 0; col<nOfColumns; col++)
+			if (!coveredColumns[col])
+				for (row = 0; row<nOfRows; row++)
+					if ((!coveredRows[row]) && (fabs(distMatrix[row + nOfRows*col]) < DBL_EPSILON))
+					{
+						/* prime zero */
+						primeMatrix[row + nOfRows*col] = true;
+
+						/* find starred zero in current row */
+						for (starCol = 0; starCol<nOfColumns; starCol++)
+							if (starMatrix[row + nOfRows*starCol])
+								break;
+
+						if (starCol == nOfColumns) /* no starred zero found */
+						{
+							/* move to step 4 */
+							step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col);
+							return;
+						}
+						else
+						{
+							coveredRows[row] = true;
+							coveredColumns[starCol] = false;
+							zerosFound = true;
+							break;
+						}
+					}
+	}
+
+	/* move to step 5 */
+	step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+}
+
+/********************************************************/
+void HungarianAlgorithm::step4(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col)
+{
+	int n, starRow, starCol, primeRow, primeCol;
+	int nOfElements = nOfRows*nOfColumns;
+
+	/* generate temporary copy of starMatrix */
+	for (n = 0; n<nOfElements; n++)
+		newStarMatrix[n] = starMatrix[n];
+
+	/* star current zero */
+	newStarMatrix[row + nOfRows*col] = true;
+
+	/* find starred zero in current column */
+	starCol = col;
+	for (starRow = 0; starRow<nOfRows; starRow++)
+		if (starMatrix[starRow + nOfRows*starCol])
+			break;
+
+	while (starRow<nOfRows)
+	{
+		/* unstar the starred zero */
+		newStarMatrix[starRow + nOfRows*starCol] = false;
+
+		/* find primed zero in current row */
+		primeRow = starRow;
+		for (primeCol = 0; primeCol<nOfColumns; primeCol++)
+			if (primeMatrix[primeRow + nOfRows*primeCol])
+				break;
+
+		/* star the primed zero */
+		newStarMatrix[primeRow + nOfRows*primeCol] = true;
+
+		/* find starred zero in current column */
+		starCol = primeCol;
+		for (starRow = 0; starRow<nOfRows; starRow++)
+			if (starMatrix[starRow + nOfRows*starCol])
+				break;
+	}
+
+	/* use temporary copy as new starMatrix */
+	/* delete all primes, uncover all rows */
+	for (n = 0; n<nOfElements; n++)
+	{
+		primeMatrix[n] = false;
+		starMatrix[n] = newStarMatrix[n];
+	}
+	for (n = 0; n<nOfRows; n++)
+		coveredRows[n] = false;
+
+	/* move to step 2a */
+	step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+}
+
+/********************************************************/
+void HungarianAlgorithm::step5(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
+{
+	double h, value;
+	int row, col;
+
+	/* find smallest uncovered element h */
+	h = DBL_MAX;
+	for (row = 0; row<nOfRows; row++)
+		if (!coveredRows[row])
+			for (col = 0; col<nOfColumns; col++)
+				if (!coveredColumns[col])
+				{
+					value = distMatrix[row + nOfRows*col];
+					if (value < h)
+						h = value;
+				}
+
+	/* add h to each covered row */
+	for (row = 0; row<nOfRows; row++)
+		if (coveredRows[row])
+			for (col = 0; col<nOfColumns; col++)
+				distMatrix[row + nOfRows*col] += h;
+
+	/* subtract h from each uncovered column */
+	for (col = 0; col<nOfColumns; col++)
+		if (!coveredColumns[col])
+			for (row = 0; row<nOfRows; row++)
+				distMatrix[row + nOfRows*col] -= h;
+
+	/* move to step 3 */
+	step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+}

nodes/track/sort/Hungarian.h

@@ -0,0 +1,34 @@
+///////////////////////////////////////////////////////////////////////////////
+// Hungarian.h: Header file for Class HungarianAlgorithm.
+// 
+// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren.
+// The original implementation is a few mex-functions for use in MATLAB, found here:
+// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem
+// 
+// Both this code and the orignal code are published under the BSD license.
+// by Cong Ma, 2016
+// 
+
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+
+class HungarianAlgorithm
+{
+public:
+	HungarianAlgorithm();
+	~HungarianAlgorithm();
+	double Solve(vector<vector<double>>& DistMatrix, vector<int>& Assignment);
+
+private:
+	void assignmentoptimal(int *assignment, double *cost, double *distMatrix, int nOfRows, int nOfColumns);
+	void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns);
+	void computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows);
+	void step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
+	void step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
+	void step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
+	void step4(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col);
+	void step5(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
+};

nodes/track/sort/KalmanTracker.cpp

@@ -0,0 +1,176 @@
+///////////////////////////////////////////////////////////////////////////////
+// KalmanTracker.cpp: KalmanTracker Class Implementation Declaration
+
+#include "KalmanTracker.h"
+
+
+int KalmanTracker::kf_count = 0;
+
+
+// initialize Kalman filter
+void KalmanTracker::init_kf(StateType stateMat)
+{
+	int stateNum = 7;
+	int measureNum = 4;
+	kf = KalmanFilter(stateNum, measureNum, 0);
+
+	measurement = cv::Mat::zeros(measureNum, 1, CV_32F);
+
+	kf.transitionMatrix = (cv::Mat_<float>(stateNum, stateNum) << 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1);
+
+	setIdentity(kf.measurementMatrix);
+	setIdentity(kf.processNoiseCov, Scalar::all(1e-2));
+	setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));
+	setIdentity(kf.errorCovPost, Scalar::all(1));
+	
+	// initialize state vector with bounding box in [cx,cy,s,r] style
+	kf.statePost.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
+	kf.statePost.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
+	kf.statePost.at<float>(2, 0) = stateMat.area();
+	kf.statePost.at<float>(3, 0) = stateMat.width / stateMat.height;
+}
+
+
+// Predict the estimated bounding box.
+StateType KalmanTracker::predict()
+{
+	// predict
+	Mat p = kf.predict();
+	m_age += 1;
+
+	if (m_time_since_update > 0)
+		m_hit_streak = 0;
+	m_time_since_update += 1;
+
+	StateType predictBox = get_rect_xysr(p.at<float>(0, 0), p.at<float>(1, 0), p.at<float>(2, 0), p.at<float>(3, 0));
+
+	m_history.push_back(predictBox);
+	return m_history.back();
+}
+
+
+// Update the state vector with observed bounding box.
+void KalmanTracker::update(StateType stateMat)
+{
+	m_time_since_update = 0;
+	m_history.clear();
+	m_hits += 1;
+	m_hit_streak += 1;
+
+	// measurement
+	measurement.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
+	measurement.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
+	measurement.at<float>(2, 0) = stateMat.area();
+	measurement.at<float>(3, 0) = stateMat.width / stateMat.height;
+
+	// update
+	kf.correct(measurement);
+}
+
+
+// Return the current state vector
+StateType KalmanTracker::get_state()
+{
+	Mat s = kf.statePost;
+	return get_rect_xysr(s.at<float>(0, 0), s.at<float>(1, 0), s.at<float>(2, 0), s.at<float>(3, 0));
+}
+
+
+// Convert bounding box from [cx,cy,s,r] to [x,y,w,h] style.
+StateType KalmanTracker::get_rect_xysr(float cx, float cy, float s, float r)
+{
+	float w = sqrt(s * r);
+	float h = s / w;
+	float x = (cx - w / 2);
+	float y = (cy - h / 2);
+
+	if (x < 0 && cx > 0)
+		x = 0;
+	if (y < 0 && cy > 0)
+		y = 0;
+
+	return StateType(x, y, w, h);
+}
+
+
+
+/*
+// --------------------------------------------------------------------
+// Kalman Filter Demonstrating, a 2-d ball demo
+// --------------------------------------------------------------------
+
+const int winHeight = 600;
+const int winWidth = 800;
+
+Point mousePosition = Point(winWidth >> 1, winHeight >> 1);
+
+// mouse event callback
+void mouseEvent(int event, int x, int y, int flags, void *param)
+{
+	if (event == CV_EVENT_MOUSEMOVE) {
+		mousePosition = Point(x, y);
+	}
+}
+
+void TestKF();
+
+void main()
+{
+	TestKF();
+}
+
+
+void TestKF()
+{
+	int stateNum = 4;
+	int measureNum = 2;
+	KalmanFilter kf = KalmanFilter(stateNum, measureNum, 0);
+
+	// initialization
+	Mat processNoise(stateNum, 1, CV_32F);
+	Mat measurement = Mat::zeros(measureNum, 1, CV_32F);
+
+	kf.transitionMatrix = *(Mat_<float>(stateNum, stateNum) <<
+		1, 0, 1, 0,
+		0, 1, 0, 1,
+		0, 0, 1, 0,
+		0, 0, 0, 1);
+
+	setIdentity(kf.measurementMatrix);
+	setIdentity(kf.processNoiseCov, Scalar::all(1e-2));
+	setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));
+	setIdentity(kf.errorCovPost, Scalar::all(1));
+
+	randn(kf.statePost, Scalar::all(0), Scalar::all(winHeight));
+
+	namedWindow("Kalman");
+	setMouseCallback("Kalman", mouseEvent);
+	Mat img(winHeight, winWidth, CV_8UC3);
+
+	while (1)
+	{
+		// predict
+		Mat prediction = kf.predict();
+		Point predictPt = Point(prediction.at<float>(0, 0), prediction.at<float>(1, 0));
+
+		// generate measurement
+		Point statePt = mousePosition;
+		measurement.at<float>(0, 0) = statePt.x;
+		measurement.at<float>(1, 0) = statePt.y;
+
+		// update
+		kf.correct(measurement);
+
+		// visualization
+		img.setTo(Scalar(255, 255, 255));
+		circle(img, predictPt, 8, CV_RGB(0, 255, 0), -1); // predicted point as green
+		circle(img, statePt, 8, CV_RGB(255, 0, 0), -1); // current position as red
+
+		imshow("Kalman", img);
+		char code = (char)waitKey(100);
+		if (code == 27 || code == 'q' || code == 'Q')
+			break;
+	}
+	destroyWindow("Kalman");
+}
+*/

nodes/track/sort/KalmanTracker.h

@@ -0,0 +1,72 @@
+///////////////////////////////////////////////////////////////////////////////
+// KalmanTracker.h: KalmanTracker Class Declaration
+
+#ifndef KALMAN_H
+#define KALMAN_H 2
+
+#include "opencv2/video/tracking.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+using namespace std;
+using namespace cv;
+
+#define StateType Rect_<float>
+
+
+// This class represents the internel state of individual tracked objects observed as bounding box.
+class KalmanTracker
+{
+public:
+	KalmanTracker()
+	{
+		init_kf(StateType());
+		m_time_since_update = 0;
+		m_hits = 0;
+		m_hit_streak = 0;
+		m_age = 0;
+		m_id = kf_count;
+		//kf_count++;
+	}
+	KalmanTracker(StateType initRect)
+	{
+		init_kf(initRect);
+		m_time_since_update = 0;
+		m_hits = 0;
+		m_hit_streak = 0;
+		m_age = 0;
+		m_id = kf_count;
+		kf_count++;
+	}
+
+	~KalmanTracker()
+	{
+		m_history.clear();
+	}
+
+	StateType predict();
+	void update(StateType stateMat);
+	
+	StateType get_state();
+	StateType get_rect_xysr(float cx, float cy, float s, float r);
+
+	static int kf_count;
+
+	int m_time_since_update;
+	int m_hits;
+	int m_hit_streak;
+	int m_age;
+	int m_id;
+
+private:
+	void init_kf(StateType stateMat);
+
+	cv::KalmanFilter kf;
+	cv::Mat measurement;
+
+	std::vector<StateType> m_history;
+};
+
+
+
+
+#endif

nodes/track/sort/README.md

@@ -0,0 +1,6 @@
+
+
+summary
+-----------
+
+sort algorithm for tracking

nodes/track/vp_dsort_track_node.cpp

@@ -0,0 +1,21 @@
+#include "vp_dsort_track_node.h"
+
+namespace vp_nodes {
+        
+    vp_dsort_track_node::vp_dsort_track_node(std::string node_name, 
+                                            vp_track_for track_for):
+                                            vp_track_node(node_name, track_for) {
+        this->initialized();
+    }
+    
+    vp_dsort_track_node::~vp_dsort_track_node() {
+        deinitialized();
+    }
+
+    void vp_dsort_track_node::track(int channel_index, const std::vector<vp_objects::vp_rect>& target_rects, 
+                const std::vector<std::vector<float>>& target_embeddings, 
+                std::vector<int>& track_ids) {
+        // fill track_ids according to target_rects & target_embeddings
+        // deep sort logic here ...         
+    }
+}

nodes/track/vp_dsort_track_node.h

@@ -0,0 +1,19 @@
+#pragma once
+#include "vp_track_node.h"
+
+namespace vp_nodes {
+    // track node using deep sort
+    class vp_dsort_track_node: public vp_track_node
+    {
+    private:
+        /* config data for deep sort algo*/
+    protected:
+        // fill track_ids using deep sort algo
+        virtual void track(int channel_index, const std::vector<vp_objects::vp_rect>& target_rects, 
+                        const std::vector<std::vector<float>>& target_embeddings, 
+                        std::vector<int>& track_ids) override;
+    public:
+        vp_dsort_track_node(std::string node_name, vp_track_for track_for = vp_track_for::NORMAL);
+        virtual ~vp_dsort_track_node();
+    };
+}

nodes/track/vp_sort_track_node.cpp

@@ -0,0 +1,189 @@
+#include "vp_sort_track_node.h"
+
+namespace vp_nodes {
+        
+    vp_sort_track_node::vp_sort_track_node(std::string node_name, 
+                                            vp_track_for track_for):
+                                            vp_track_node(node_name, track_for) {
+        this->initialized();
+        KalmanTracker::kf_count = 0;
+    }
+    
+    vp_sort_track_node::~vp_sort_track_node() {
+        deinitialized();
+    }
+
+    void vp_sort_track_node::track(int channel_index, const std::vector<vp_objects::vp_rect>& target_rects, 
+                    const std::vector<std::vector<float>>& target_embeddings, 
+                    std::vector<int>& track_ids) {
+        // fill track_ids according to target_rects (target_embeddings ignored)
+		track_ids.resize(target_rects.size());
+		for (auto&  item : track_ids) {
+			item = -1;
+		}
+
+		// check if trackers are initialized or not for specific channel
+		if (all_trackers.count(channel_index) == 0) {
+			all_trackers[channel_index] = std::vector<KalmanTracker>();
+			VP_INFO(vp_utils::string_format("[%s] initialize kalmantracker the first time for channel %d", node_name.c_str(), channel_index));
+		}
+		// track on specific channel
+		auto& trackers = all_trackers[channel_index];
+
+        if (trackers.empty()) {
+            /* first frame*/
+            for (unsigned int i = 0; i < target_rects.size(); i++) {
+				KalmanTracker trk = KalmanTracker(cv::Rect_<float>(target_rects[i].x, target_rects[i].y, target_rects[i].width, target_rects[i].height));
+				trackers.push_back(trk);
+			}
+            return;
+        }
+        //3.1. get predicted locations from existing trackers.
+        predictedBoxes.clear();
+		for (auto it = trackers.begin(); it != trackers.end();) {
+			Rect_<float> pBox = (*it).predict();
+			if (pBox.x >= 0 && pBox.y >= 0) {
+				predictedBoxes.push_back(pBox);
+				it++;
+			}
+			else {
+				it = trackers.erase(it);
+			}
+		}
+        
+        // 3.2. associate detections to tracked object (both represented as bounding boxes)
+		// dets : detFrameData[fi]
+		auto trkNum = predictedBoxes.size();
+		auto detNum = target_rects.size();
+
+		iouMatrix.clear();
+		iouMatrix.resize(trkNum, vector<double>(detNum, 0));
+        
+		// compute iou matrix as a distance matrix
+        for (unsigned int i = 0; i < trkNum; i++)  {
+			for (unsigned int j = 0; j < detNum; j++) {
+				// use 1-iou because the hungarian algorithm computes a minimum-cost assignment.
+				iouMatrix[i][j] = 1 - GetIOU(predictedBoxes[i], cv::Rect_<float>(target_rects[j].x, target_rects[j].y, target_rects[j].width, target_rects[j].height));
+			}
+		}
+
+        // solve the assignment problem using hungarian algorithm.
+		// the resulting assignment is [track(prediction) : detection], with len=preNum
+		HungarianAlgorithm HungAlgo;
+		assignment.clear();
+		HungAlgo.Solve(iouMatrix, assignment);
+
+		// find matches, unmatched_detections and unmatched_predictions
+		unmatchedTrajectories.clear();
+		unmatchedDetections.clear();
+		allItems.clear();
+		matchedItems.clear();
+
+		// there are unmatched detections
+        if (detNum > trkNum) {
+			for (unsigned int n = 0; n < detNum; n++)
+				allItems.insert(n);
+
+			for (unsigned int i = 0; i < trkNum; ++i)
+				matchedItems.insert(assignment[i]);
+
+			set_difference(allItems.begin(), allItems.end(),
+				matchedItems.begin(), matchedItems.end(),
+				insert_iterator<set<int>>(unmatchedDetections, unmatchedDetections.begin()));
+		}
+		// there are unmatched trajectory/predictions
+		else if (detNum < trkNum) {
+			for (unsigned int i = 0; i < trkNum; ++i)
+				if (assignment[i] == -1) // unassigned label will be set as -1 in the assignment algorithm
+					unmatchedTrajectories.insert(i);
+		}
+		else {
+
+		}
+        
+        // filter out matched with low IOU
+		matchedPairs.clear();
+		for (unsigned int i = 0; i < trkNum; ++i) {
+			if (assignment[i] == -1) // pass over invalid values
+				continue;
+			if (1 - iouMatrix[i][assignment[i]] < iouThreshold) {
+				unmatchedTrajectories.insert(i);
+				unmatchedDetections.insert(assignment[i]);
+			}
+			else {
+				matchedPairs.push_back(cv::Point(i, assignment[i]));
+			}
+		}
+
+
+        // 3.3. updating trackers
+		// update matched trackers with assigned detections.
+		// each prediction is corresponding to a tracker
+		int detIdx, trkIdx;
+		for (unsigned int i = 0; i < matchedPairs.size(); i++) {
+			trkIdx = matchedPairs[i].x;
+			detIdx = matchedPairs[i].y;
+			trackers[trkIdx].update(cv::Rect_<float>(target_rects[detIdx].x, 
+                                                    target_rects[detIdx].y, 
+                                                    target_rects[detIdx].width, 
+                                                    target_rects[detIdx].height));
+		}
+
+		// create and initialise new trackers for unmatched detections
+		for (auto& umd : unmatchedDetections) {
+			KalmanTracker tracker = KalmanTracker(cv::Rect_<float>(target_rects[umd].x, 
+                                                                   target_rects[umd].y,
+                                                                   target_rects[umd].width, 
+                                                                   target_rects[umd].height));
+			trackers.push_back(tracker);
+		}
+
+        // get trackers' output
+		frameTrackingResult.clear();
+		for (auto it = trackers.begin(); it != trackers.end();) {
+			if (((*it).m_time_since_update < 1) &&
+				((*it).m_hit_streak >= min_hits)) {
+				TrackingBox res;
+				res.box = (*it).get_state();
+				res.id = (*it).m_id + 1;
+				//res.frame = meta->frame_index;
+				frameTrackingResult.push_back(res);
+				it++;
+			}
+			else
+				it++;
+
+			// remove dead tracklet
+			if (it != trackers.end() && (*it).m_time_since_update > max_age)
+				it = trackers.erase(it);
+		}
+
+        for (const auto& tb : frameTrackingResult) {
+			// id and box need to correspond
+			for (int i = 0; i < target_rects.size(); ++i) {
+				/* code */
+				if(GetIOU(cv::Rect_<float>(target_rects[i].x, 
+											target_rects[i].y, 
+											target_rects[i].width, 
+											target_rects[i].height),
+						   cv::Rect_<float>(tb.box.x, 
+											tb.box.y, 
+											tb.box.width, 
+											tb.box.height)) > 0.8) {
+				track_ids[i] = tb.id;
+				}
+			}
+        }
+        return;
+    }
+
+    double vp_sort_track_node::GetIOU(cv::Rect_<float> bb_test, cv::Rect_<float> bb_gt){
+         float in = (bb_test & bb_gt).area();
+        float un = bb_test.area() + bb_gt.area() - in;
+
+        if (un < DBL_EPSILON)
+            return 0;
+
+        return (double)(in / un);
+    }
+}

nodes/track/vp_sort_track_node.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <vector>
+#include <set>
+#include <map>
+#include "vp_track_node.h"
+#include "sort/Hungarian.h"
+#include "sort/KalmanTracker.h"
+
+namespace vp_nodes {
+    // track node using sort
+    class vp_sort_track_node: public vp_track_node
+    {
+    private:
+        /* config data for sort algo */
+        /* data */
+        typedef struct TrackingBox
+        {
+            //int frame;
+            int id;
+            Rect_<float> box;
+        }TrackingBox;
+
+        int max_age = 1;
+        int min_hits = 3;
+        double iouThreshold = 0.5;
+        // vector<KalmanTracker> trackers;
+        std::map<int, std::vector<KalmanTracker>> all_trackers;
+        std::vector<cv::Rect_<float>> predictedBoxes;
+        std::vector<vector<double>> iouMatrix;
+        std::vector<int> assignment;
+        std::set<int> unmatchedDetections;
+        std::set<int> unmatchedTrajectories;
+        std::set<int> allItems;
+        std::set<int> matchedItems;
+        std::vector<cv::Point> matchedPairs;
+        std::vector<TrackingBox> frameTrackingResult;
+    private:
+        double GetIOU(cv::Rect_<float> bb_test, cv::Rect_<float> bb_gt);
+    protected:
+        // fill track_ids using sort algo
+        virtual void track(int channel_index, const std::vector<vp_objects::vp_rect>& target_rects, 
+                        const std::vector<std::vector<float>>& target_embeddings, 
+                        std::vector<int>& track_ids) override;
+    public:
+        vp_sort_track_node(std::string node_name, vp_track_for track_for = vp_track_for::NORMAL);
+        virtual ~vp_sort_track_node();
+    };
+
+}
+

nodes/track/vp_track_node.cpp

@@ -0,0 +1,128 @@
+
+#include "vp_track_node.h"
+//#include "../objects/shapes/vp_rect.h"
+
+namespace vp_nodes {
+        
+    vp_track_node::vp_track_node(std::string node_name, 
+                                vp_track_for track_for): 
+                                vp_node(node_name), 
+                                track_for(track_for) {
+    }
+    
+    vp_track_node::~vp_track_node() {
+    }
+
+    std::shared_ptr<vp_objects::vp_meta> vp_track_node::handle_control_meta(std::shared_ptr<vp_objects::vp_control_meta> meta) {
+        return meta;
+    }
+
+    std::shared_ptr<vp_objects::vp_meta> vp_track_node::handle_frame_meta(std::shared_ptr<vp_objects::vp_frame_meta> meta) {
+        // channel_index can be different each call
+        auto channel_index = meta->channel_index;
+
+        // data used for tracking
+        std::vector<vp_objects::vp_rect> rects;      // rects of targets
+        std::vector<std::vector<float>> embeddings;  // embeddings of targets
+        std::vector<int> track_ids;                  // track ids of targets
+
+        // step 1, collect data
+        preprocess(meta, rects, embeddings);
+
+        // step 2, track by channel
+        track(channel_index, rects, embeddings, track_ids);
+
+        // step 3, postprocess
+        postprocess(meta, rects, embeddings, track_ids);
+
+        return meta;
+    }
+
+    void vp_track_node::preprocess(std::shared_ptr<vp_objects::vp_frame_meta> frame_meta, 
+                                std::vector<vp_objects::vp_rect>& target_rects, 
+                                std::vector<std::vector<float>>& target_embeddings) {
+        if (track_for == vp_track_for::NORMAL) {
+            for(auto& i: frame_meta->targets) {
+                target_rects.push_back(i->get_rect());      // rect fo target (via i variable)
+                target_embeddings.push_back(i->embeddings); // embeddings of target (via i variable)
+            }
+        }
+
+        if (track_for == vp_track_for::FACE) {
+            for(auto& i: frame_meta->face_targets) {
+                target_rects.push_back(i->get_rect());       // rect of face target (via i variable)
+                target_embeddings.push_back(i->embeddings);  // embeddings of face target (via i variable)
+            }
+        }
+        /* ... extend for more track for... */
+    }
+
+    // write track_ids back to frame meta
+    // we can also cache history rects for each target, and then push them back to tracks field (such as vp_frame_target::tracks)
+    void vp_track_node::postprocess(std::shared_ptr<vp_objects::vp_frame_meta> frame_meta, 
+                    const std::vector<vp_objects::vp_rect>& target_rects, 
+                    const std::vector<std::vector<float>>& target_embeddings, 
+                    const std::vector<int>& track_ids) {
+
+        if (track_ids.empty()) {
+            return;
+        }
+        // assert for length of vectors since they are generated by step1 & step2 separately
+        // assert(target_rects.size() == target_embeddings.size());
+        assert(target_rects.size() == track_ids.size());
+
+        // support multi channels
+        auto& tracks_by_id = all_tracks_by_id[frame_meta->channel_index];
+        auto& last_tracked_frame_indexes = all_last_tracked_frame_indexes[frame_meta->channel_index];
+
+        if (track_for == vp_track_for::NORMAL) {
+            //assert(target_rects.size() == frame_meta->targets.size());
+            for (int i = 0; i < frame_meta->targets.size(); i++) {
+                auto& target = frame_meta->targets[i];
+                auto& rect = target_rects[i];
+                auto& track_id = track_ids[i];
+
+                // -1 means no track result returned yet
+                if (track_id != -1) {
+                    tracks_by_id[track_id].push_back(rect);                           // cache
+                    last_tracked_frame_indexes[track_id] = frame_meta->frame_index;   // update stamp
+
+                    target->track_id = track_id;               // write track_id back to target
+                    target->tracks = tracks_by_id[track_id];   // write tracks back to target
+                }
+            }
+        }
+
+        if (track_for == vp_track_for::FACE) {
+            // assert(target_rects.size() == frame_meta->face_targets.size());
+            for (int i = 0; i < frame_meta->face_targets.size(); i++) {
+                auto& face = frame_meta->face_targets[i];
+                auto& rect = target_rects[i];
+                auto& track_id = track_ids[i];
+
+                // -1 means no track result returned yet
+                if (track_id != -1) {
+                    tracks_by_id[track_id].push_back(rect);                           // cache
+                    last_tracked_frame_indexes[track_id] = frame_meta->frame_index;   // update stamp
+
+                    face->track_id = track_id;                // write track_id back to face target
+                    face->tracks = tracks_by_id[track_id];    // write tracks back to face target
+                }
+            }
+        }
+        /* ... extend for more track for... */
+
+        // remove cache tracks if has been long time since last updated (maybe it disappeared already).
+        for (auto i = last_tracked_frame_indexes.begin(); i != last_tracked_frame_indexes.end();) {
+            if (frame_meta->frame_index - (i->second) > max_allowed_disappear_frames 
+                || frame_meta->frame_index < i->second) {
+                VP_DEBUG(vp_utils::string_format("[%s] [tracking] long time no update, so erase cache of tracks for track_id:`%d`, size of tracks is:`%d`", node_name.c_str(), i->first, tracks_by_id[i->first].size()));
+                tracks_by_id.erase(i->first);              // erase tracks first
+                i = last_tracked_frame_indexes.erase(i);   // erase stamp then
+            }
+            else {
+                i++;
+            }
+        }
+    }
+}

nodes/track/vp_track_node.h

@@ -0,0 +1,60 @@
+
+#pragma once
+
+#include <map>
+#include <assert.h>
+#include "../vp_node.h"
+
+namespace vp_nodes {
+    // track node applied to which type of target (vp_frame_target, vp_frame_face_target or others)
+    enum class vp_track_for {
+        NORMAL = 1,    // vp_frame_target
+        FACE = 2       // vp_frame_face_target
+                       // others to extend
+    };
+
+    // base class for tracking, can not be initialized directly.
+    // note that a track node can work on different channels at the same time
+    class vp_track_node: public vp_node {
+    private:
+        // track for
+        vp_track_for track_for = vp_track_for::NORMAL;
+        
+        // cache tracks at previous frames
+        // std::map<int, std::vector<vp_objects::vp_rect>> tracks_by_id;
+        std::map<int, std::map<int, std::vector<vp_objects::vp_rect>>> all_tracks_by_id;
+
+        // stamp
+        // std::map<int, int> last_tracked_frame_indexes;
+        std::map<int, std::map<int, int>> all_last_tracked_frame_indexes;
+
+        // remove cache tracks if it has been long time since last tracked.
+        const int max_allowed_disappear_frames = 25;
+    protected:
+        virtual std::shared_ptr<vp_objects::vp_meta> handle_frame_meta(std::shared_ptr<vp_objects::vp_frame_meta> meta) override final;
+        virtual std::shared_ptr<vp_objects::vp_meta> handle_control_meta(std::shared_ptr<vp_objects::vp_control_meta> meta) override final;
+
+        // prepare data according to `track_for`
+        void preprocess(std::shared_ptr<vp_objects::vp_frame_meta> frame_meta, 
+                        std::vector<vp_objects::vp_rect>& target_rects, 
+                        std::vector<std::vector<float>>& target_embeddings);
+        
+        // track api
+        // it is a pure virtual function which should be implemented by derived class.
+        // In:  rects & embeddings whose size() can be zero
+        // Out: track ids
+        virtual void track(int channel_index, const std::vector<vp_objects::vp_rect>& target_rects, 
+                        const std::vector<std::vector<float>>& target_embeddings, 
+                        std::vector<int>& track_ids) = 0;
+
+        // write track_ids back to frame meta
+        // we can also cache history rects for each target, and then push them back to tracks field (like vp_frame_target::tracks)
+        void postprocess(std::shared_ptr<vp_objects::vp_frame_meta> frame_meta, 
+                        const std::vector<vp_objects::vp_rect>& target_rects, 
+                        const std::vector<std::vector<float>>& target_embeddings, 
+                        const std::vector<int>& track_ids);
+    public:
+        vp_track_node(std::string node_name, vp_track_for track_for = vp_track_for::NORMAL);
+        virtual ~vp_track_node();
+    };
+}