Add rectangle to every track

Author: Sergey Nuzhny

Tracker/Ctracker.cpp

@@ -22,32 +22,34 @@ CTracker::CTracker(
 // ---------------------------------------------------------------------------
 //
 // ---------------------------------------------------------------------------
-void CTracker::Update(const std::vector<Point_t>& detections)
+void CTracker::Update(
+	const std::vector<Point_t>& detections,
+	const std::vector<cv::Rect>& rects)
 {
+	assert(detections.size() == rects.size());
+
 	// -----------------------------------
 	// If there is no tracks yet, then every cv::Point begins its own track.
 	// -----------------------------------
 	if (tracks.size() == 0)
 	{
 		// If no tracks yet
-        for (auto d : detections)
+		for (size_t i = 0; i < detections.size(); ++i)
 		{
-            tracks.push_back(std::make_unique<CTrack>(d, dt, Accel_noise_mag, NextTrackID++));
+			tracks.push_back(std::make_unique<CTrack>(detections[i], rects[i], dt, Accel_noise_mag, NextTrackID++));
 		}
 	}
 
-	// -----------------------------------
-	// Здесь треки уже есть в любом случае
-	// -----------------------------------
 	size_t N = tracks.size();		// треки
 	size_t M = detections.size();	// детекты
 
-	// Матрица расстояний от N-ного трека до M-ного детекта.
-	distMatrix_t Cost(N * M);
 	assignments_t assignment; // назначения
 
 	if (!tracks.empty())
 	{
+		// Матрица расстояний от N-ного трека до M-ного детекта.
+		distMatrix_t Cost(N * M);
+
 		// -----------------------------------
 		// Треки уже есть, составим матрицу расстояний
 		// -----------------------------------
@@ -107,7 +109,7 @@ void CTracker::Update(const std::vector<Point_t>& detections)
 	{
         if (find(assignment.begin(), assignment.end(), i) == assignment.end())
 		{
-			tracks.push_back(std::make_unique<CTrack>(detections[i], dt, Accel_noise_mag, NextTrackID++));
+			tracks.push_back(std::make_unique<CTrack>(detections[i], rects[i], dt, Accel_noise_mag, NextTrackID++));
 		}
 	}
 
@@ -120,11 +122,11 @@ void CTracker::Update(const std::vector<Point_t>& detections)
 		if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
 		{
 			tracks[i]->skipped_frames = 0;
-			tracks[i]->Update(detections[assignment[i]], true, max_trace_length);
+			tracks[i]->Update(detections[assignment[i]], rects[assignment[i]], true, max_trace_length);
 		}
 		else				     // if not continue using predictions
 		{
-			tracks[i]->Update(Point_t(0, 0), false, max_trace_length);
+			tracks[i]->Update(Point_t(), cv::Rect(), false, max_trace_length);
 		}
 	}
 

Tracker/Ctracker.h

@@ -6,24 +6,30 @@
 #include <vector>
 #include <memory>
 
-//template<typename TRACK_OBJ>
+// --------------------------------------------------------------------------
 class CTrack
 {
 public:
-	CTrack(Point_t p, track_t dt, track_t Accel_noise_mag, size_t trackID)
+	CTrack(const Point_t& p, const cv::Rect& rect, track_t dt, track_t Accel_noise_mag, size_t trackID)
 		:
 		track_id(trackID),
 		skipped_frames(0),
 		prediction(p),
+		lastRect(rect),
 		KF(p, dt, Accel_noise_mag)
 	{
 	}
 
-	void Update(Point_t p, bool dataCorrect, size_t max_trace_length)
+	void Update(const Point_t& p, const cv::Rect& rect, bool dataCorrect, size_t max_trace_length)
 	{
 		KF.GetPrediction();
 		prediction = KF.Update(p, dataCorrect);
 
+		if (dataCorrect)
+		{
+			lastRect = rect;
+		}
+
 		if (trace.size() > max_trace_length)
 		{
 			trace.erase(trace.begin(), trace.end() - max_trace_length);
@@ -35,21 +41,32 @@ class CTrack
 	std::vector<Point_t> trace;
 	size_t track_id;
 	size_t skipped_frames; 
+	
 	Point_t prediction;
 
+	cv::Rect GetLastRect()
+	{
+		return cv::Rect(
+			static_cast<int>(prediction.x - lastRect.width / 2),
+			static_cast<int>(prediction.y - lastRect.height / 2),
+			lastRect.width,
+			lastRect.height);
+	}
+
 private:
+	cv::Rect lastRect;
 	TKalmanFilter KF;
 };
 
-
+// --------------------------------------------------------------------------
 class CTracker
 {
 public:
 	CTracker(track_t dt_, track_t Accel_noise_mag_, track_t dist_thres_ = 60, size_t maximum_allowed_skipped_frames_ = 10, size_t max_trace_length_ = 10);
 	~CTracker(void);
 
 	std::vector<std::unique_ptr<CTrack>> tracks;
-	void Update(const std::vector<Point_t>& detections);
+	void Update(const std::vector<Point_t>& detections, const std::vector<cv::Rect>& rects);
 
 private:
 	// Шаг времени опроса фильтра

main.cpp

@@ -76,7 +76,7 @@ int main(int ac, char** av)
 		const std::vector<Point_t>& centers = detector.Detect(gray);
 		const std::vector<cv::Rect>& rects = detector.GetDetects();
 
-		tracker.Update(centers);
+		tracker.Update(centers, rects);
 
 		int64 t2 = cv::getTickCount();
 
@@ -86,16 +86,14 @@ int main(int ac, char** av)
 		{
 			cv::circle(frame, p, 3, cv::Scalar(0, 255, 0), 1, CV_AA);
 		}
-		for (auto r : rects)
-		{
-			cv::rectangle(frame, r, cv::Scalar(0, 255, 0), 1, CV_AA);
-		}
 
 		std::cout << tracker.tracks.size() << std::endl;
 
 		for (int i = 0; i < tracker.tracks.size(); i++)
 		{
-			if (tracker.tracks[i]->trace.size()>1)
+			cv::rectangle(frame, tracker.tracks[i]->GetLastRect(), cv::Scalar(0, 255, 0), 1, CV_AA);
+
+			if (tracker.tracks[i]->trace.size() > 1)
 			{
 				for (int j = 0; j < tracker.tracks[i]->trace.size() - 1; j++)
 				{
@@ -145,21 +143,27 @@ int main(int ac, char** av)
 		pts.push_back(Point_t(Xmeasured + 100.0f*sin(alpha / 3.0f), Ymeasured + 100.0f*cos(alpha / 1.0f)));
 		alpha += 0.05f;
 
+		std::vector<cv::Rect> rects;
+		for (auto p : pts)
+		{
+			rects.push_back(cv::Rect(static_cast<int>(p.x - 1), static_cast<int>(p.y - 1), 3, 3));
+		}
+
 
 		for (size_t i = 0; i < pts.size(); i++)
 		{
 			cv::circle(frame, pts[i], 3, cv::Scalar(0, 255, 0), 1, CV_AA);
 		}
 
-		tracker.Update(pts);
+		tracker.Update(pts, rects);
 
 		std::cout << tracker.tracks.size() << std::endl;
 
 		for (size_t i = 0; i < tracker.tracks.size(); i++)
 		{
 			const auto& tracks = tracker.tracks[i];
 
-			if (tracks->trace.size()>1)
+			if (tracks->trace.size() > 1)
 			{
 				for (size_t j = 0; j < tracks->trace.size() - 1; j++)
 				{