WSeries_Calibration/QtCameraHardWareCopilot/include/Tracker.h

#pragma once

#ifdef ORTTOOLKIT
#define ORTTOOLKIT __declspec(dllexport)
#else
#define ORTTOOLKIT __declspec(dllimport)
#endif

#include <opencv/cv.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>


#include <iostream>
#include <queue>

#include <iostream>
#include <vector>
#include <fstream>
#include <set>

#include <Vision.h>




namespace OrtToolkit {
	namespace Tracker {
		class HungarianAlgorithm
		{
		public:
			HungarianAlgorithm();
			~HungarianAlgorithm();
			double Solve(std::vector<std::vector<double>>& DistMatrix, std::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);
		};

		class KalmanTracker
		{
		public:
			KalmanTracker()
			{
				init_kf(cv::Rect_<float>());
				m_time_since_update = 0;
				m_hits = 0;
				m_hit_streak = 0;
				m_age = 0;
				m_id = OrtToolkit::Util::create_uuid();
			}
			KalmanTracker(cv::Rect_<float> initRect)
			{
				init_kf(initRect);
				m_time_since_update = 0;
				m_hits = 0;
				m_hit_streak = 0;
				m_age = 0;
				m_id = OrtToolkit::Util::create_uuid();
			}

			~KalmanTracker()
			{
				m_history.clear();
			}

			cv::Rect_<float> predict();
			void update(cv::Rect_<float> stateMat);

			cv::Rect_<float> get_state();
			cv::Rect_<float> 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;//跟踪器总活跃次数
			std::string m_id;

		private:
			void init_kf(cv::Rect_<float> stateMat);

			cv::KalmanFilter kf;
			cv::Mat measurement;

			std::vector<cv::Rect_<float>> m_history;
		};
		class ORTTOOLKIT IouTracker {
		public:
			IouTracker(float low = 0.5, float iou = 0.3, int t_min = 3, int disapper_t = 1, int id_c = 10000, std::vector<cv::Point2f>  FaceStd = { {38.2946, 51.6963},{73.5318, 51.6963},
				{56.0252, 71.7366},{41.5493, 92.3655},
				{70.7299, 92.3655} });
			~IouTracker() {};
			void updata(std::vector<std::array<float, 4>> bbox, std::vector<float> score, std::vector<OrtToolkit::Type::track>& tracks_finished, std::vector<OrtToolkit::Type::track>& tracks_new);
			void updata(OrtToolkit::Type::Type_Boxs bboxs, OrtToolkit::Type::Type_Scores scores, std::vector<std::array<OrtToolkit::Type::Base_Type_iPoint, 5>> LandmarksPoints, std::vector<std::array<OrtToolkit::Type::Base_Type_Score, 5>> LandmarksScores, std::vector<OrtToolkit::Type::track>& tracks_finished, std::vector<OrtToolkit::Type::track>& tracks_new);
			void iou_batch(OrtToolkit::Type::Type_Boxs bb_test, OrtToolkit::Type::Type_Boxs bb_get, OrtToolkit::Type::Type_Scores& iou);
			// 全区禁区
			void RestrictedZone(cv::Mat frame, OrtToolkit::Type::Res_PP_YOLOE res, std::queue <cv::Mat>& OriginalQueue, std::queue <cv::Mat>& ThumbnailQueue, int Width, int Height, std::queue<tm>& TimeQueue, std::queue<int>& TypeQueue, tm Nowtime, int type);
			// 目标检测禁区
			void RestrictedZone(cv::Mat frame, OrtToolkit::Type::Res_PP_YOLOE res, std::vector<std::vector<cv::Point>>  Border, std::queue <cv::Mat>& OriginalQueue, std::queue <cv::Mat>& ThumbnailQueue, int Width, int Height, std::queue<tm>& TimeQueue, std::queue<int>& TypeQueue, tm Nowtime, int type);
			// 人脸检测禁区
			void RestrictedZone(cv::Mat frame, OrtToolkit::Type::Res_YOLOV7_FACE res, std::vector<std::vector<cv::Point>>  Border, std::queue <cv::Mat>& OriginalQueue, std::queue <cv::Mat>& ThumbnailQueue, std::queue <std::string>& FaceID, int Width, int Height, std::queue<tm>& TimeQueue, std::queue<int>& TypeQueue, tm Nowtime, int type, std::vector<cv::Mat>& FaceImg);

			float sigma_l;//检测器置信度的最小阈值

		public:
			std::vector<cv::Point2f> standard;//人脸标准模板

		private:
			float sigma_iou;//IOU的最小阈值
			int t_min;//判定跟踪的最小等待时
			int disapper_time;//消失后继续跟踪的最大等待值
			int max_id = 0;//ID起始值
			int id_clear;//ID清理值
			std::vector<OrtToolkit::Type::track> tracks_observe;
		};

		ORTTOOLKIT struct TrackingBox
		{
			std::string id;
			cv::Rect_<float> box;
		};

		class ORTTOOLKIT Sort
		{
		public:

			Sort(int max_age = 24, int min_hits = 3, double iouThreshold = 0.3);
			void update(std::vector<TrackingBox> detData);
			void UpdateOut();
			~Sort();
		public:
			void GetFrameTrackingResult(std::vector<TrackingBox>& frameTrackingResult);
			void GetNewAdd(std::vector<TrackingBox>& NewAdd);
			void GetNewDel(std::vector<TrackingBox>& NewDel);
			void GetAllTrack(std::vector<TrackingBox>& NewDel);

		private:
			int max_age = 24;// 目标丢失后使用预测的方式进行预测的帧数
			int min_hits = 3;// 连续min_hits帧关联,才开始跟踪
			double iouThreshold = 0.3;// Iou阈值
			std::vector<KalmanTracker> trackers;
			std::vector<KalmanTracker> NewDel;
		};


	}
}

namespace Draw {
	void ORTTOOLKIT Border(cv::Mat& Image, const std::vector<cv::Point>& point_set, int gate_width = 10, int thickness = -1);
	void ORTTOOLKIT Border(cv::Mat& Image, const std::vector<std::vector<cv::Point>>& point_set, int gate_width = 10, int thickness = -1);
	void ORTTOOLKIT FillBorder(cv::Mat& Image, const std::vector<cv::Point>& point_set);
	void ORTTOOLKIT FillBorder(cv::Mat& Image, const std::vector<std::vector<cv::Point>>& point_set);
	void ORTTOOLKIT DrawBox(cv::Mat& Image, const std::array<float, 4>& box, int thicknes = 10);
}