Refactor project structure

modules/ANSMOT/ByteTrackNCNN/include/NCNNBYTETracker.h

@@ -0,0 +1,58 @@
+#pragma once
+#include "NCNNSTrack.h"
+#include <NCNNObject.h>
+#include <chrono>
+namespace ByteTrackNCNN {
+	class BYTETracker
+	{
+	public:
+		BYTETracker(int frame_rate = 30, int track_buffer = 30);
+		~BYTETracker();
+
+		std::vector<STrack> update(const  std::vector<ByteTrackNCNN::Object>& objects);
+		//cv::Scalar get_color(int idx);
+		void update_parameters(int frameRate = 30, int trackBuffer = 30, double trackThreshold = 0.5, double highThreshold = 0.6, double matchThresold = 0.8, bool autoFrameRate = false);
+		float getEstimatedFps() const;
+	private:
+		std::vector<STrack*> joint_stracks(std::vector<STrack*>& tlista, std::vector<STrack>& tlistb);
+		std::vector<STrack> joint_stracks(std::vector<STrack>& tlista, std::vector<STrack>& tlistb);
+
+		std::vector<STrack> sub_stracks(std::vector<STrack>& tlista, std::vector<STrack>& tlistb);
+		void remove_duplicate_stracks(std::vector<STrack>& resa, std::vector<STrack>& resb, std::vector<STrack>& stracksa, std::vector<STrack>& stracksb);
+
+		void linear_assignment(std::vector< std::vector<float> >& cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh,
+			std::vector< std::vector<int> >& matches, std::vector<int>& unmatched_a, std::vector<int>& unmatched_b);
+		std::vector< std::vector<float> > iou_distance(std::vector<STrack*>& atracks, std::vector<STrack>& btracks, int& dist_size, int& dist_size_size);
+		std::vector< std::vector<float> > iou_distance(std::vector<STrack>& atracks, std::vector<STrack>& btracks);
+		std::vector< std::vector<float> > ious(std::vector< std::vector<float> >& atlbrs, std::vector< std::vector<float> >& btlbrs);
+
+		double lapjv(const  std::vector< std::vector<float> >& cost, std::vector<int>& rowsol, std::vector<int>& colsol,
+			bool extend_cost = false, float cost_limit = LONG_MAX, bool return_cost = true);
+
+		void estimateFrameRate();
+
+	private:
+
+		float track_thresh;
+		float high_thresh;
+		float match_thresh;
+		int frame_id;
+		int max_time_lost;
+		int track_id_count;
+		int track_buffer_;
+
+		// Frame rate auto-estimation
+		bool auto_frame_rate_;
+		float estimated_fps_;
+		float time_scale_factor_;
+		size_t fps_sample_count_;
+		std::chrono::steady_clock::time_point last_update_time_;
+		bool has_last_update_time_;
+
+		std::vector<STrack> tracked_stracks;
+		std::vector<STrack> lost_stracks;
+		std::vector<STrack> removed_stracks;
+		ByteTrackNCNN::ByteKalmanFilter kalman_filter;
+	};
+
+}

modules/ANSMOT/ByteTrackNCNN/include/NCNNBytekalmanFilter.h

@@ -0,0 +1,28 @@
+#pragma once
+#include "NCNNdataType.h"
+namespace ByteTrackNCNN {
+	class ByteKalmanFilter
+	{
+	public:
+		static const double chi2inv95[10];
+		ByteKalmanFilter();
+		KAL_DATA initiate(const DETECTBOX& measurement);
+		void predict(KAL_MEAN& mean, KAL_COVA& covariance);
+		KAL_HDATA project(const KAL_MEAN& mean, const KAL_COVA& covariance);
+		KAL_DATA update(const KAL_MEAN& mean,
+			const KAL_COVA& covariance,
+			const DETECTBOX& measurement);
+
+		Eigen::Matrix<float, 1, -1> gating_distance(
+			const KAL_MEAN& mean,
+			const KAL_COVA& covariance,
+			const std::vector<DETECTBOX>& measurements,
+			bool only_position = false);
+
+	private:
+		Eigen::Matrix<float, 8, 8, Eigen::RowMajor> _motion_mat;
+		Eigen::Matrix<float, 4, 8, Eigen::RowMajor> _update_mat;
+		float _std_weight_position;
+		float _std_weight_velocity;
+	};
+}

modules/ANSMOT/ByteTrackNCNN/include/NCNNObject.h

@@ -0,0 +1,25 @@
+#pragma once
+#include "NCNNRect.h"
+namespace ByteTrackNCNN
+{
+    struct Object
+    {
+        ByteTrackNCNN::Rect<float> box; //left, top, right, bottom
+        int label;
+        float confidence;
+        float left;
+        float top;
+        float right;
+        float bottom;
+        std::string object_id;
+
+        Object(const ByteTrackNCNN::Rect<float>& _box,
+               const int& _label,
+               const float& _confidence,
+               const float& _left,
+               const float& _top,
+               const float& _right,
+               const float& _bottom,
+               const std::string& _object_id);
+    };
+}

modules/ANSMOT/ByteTrackNCNN/include/NCNNRect.h

@@ -0,0 +1,51 @@
+#pragma once
+#include "Eigen/Dense"
+namespace ByteTrackNCNN
+{
+    template<typename T>
+    using Tlwh = Eigen::Matrix<T, 1, 4, Eigen::RowMajor>;
+
+    template<typename T>
+    using Tlbr = Eigen::Matrix<T, 1, 4, Eigen::RowMajor>;
+
+    template<typename T>
+    using Xyah = Eigen::Matrix<T, 1, 4, Eigen::RowMajor>;
+
+    template<typename T>
+    class Rect
+    {
+        public:
+        Tlwh<T> tlwh;
+
+        Rect() = default;
+        Rect(const T &x, const T &y, const T &width, const T &height);
+
+        ~Rect();
+
+        const T &x() const;
+        const T &y() const;
+        const T &width() const;
+        const T &height() const;
+
+        T &x();
+        T &y();
+        T &width();
+        T &height();
+
+        const T &tl_x() const;
+        const T &tl_y() const;
+        T br_x() const;
+        T br_y() const;
+
+        Tlbr<T> getTlbr() const;
+        Xyah<T> getXyah() const;
+
+        float calcIoU(const Rect<T>& other) const;
+    };
+
+    template<typename T>
+    Rect<T> generate_rect_by_tlbr(const Tlbr<T>& tlbr);
+
+    template<typename T>
+    Rect<T> generate_rect_by_xyah(const Xyah<T>& xyah);
+}

modules/ANSMOT/ByteTrackNCNN/include/NCNNSTrack.h

@@ -0,0 +1,63 @@
+#pragma once
+#include "NCNNBytekalmanFilter.h"
+#include <unordered_map>
+namespace ByteTrackNCNN {
+	enum TrackState { New = 0, Tracked, Lost, Removed };
+	class STrack
+	{
+	public:
+		STrack(std::vector<float> tlwh_, float _score,
+			int _class_id, float _left, float _top,
+			float _right, float _bottom, std::string _object_id);
+		~STrack();
+
+		std::vector<float> static tlbr_to_tlwh(std::vector<float>& tlbr);
+		void static multi_predict(std::vector<STrack*>& stracks, ByteTrackNCNN::ByteKalmanFilter& kalman_filter);
+		void static_tlwh();
+		void static_tlbr();
+		std::vector<float> tlwh_to_xyah(std::vector<float> tlwh_tmp);
+		std::vector<float> to_xyah();
+		void mark_lost();
+		void mark_removed();
+		int next_id();
+		int end_frame();
+		void reset_count();
+
+		void activate(ByteTrackNCNN::ByteKalmanFilter& kalman_filter, int frame_id);
+		void activate(ByteTrackNCNN::ByteKalmanFilter& kalman_filter, int frame_id, int track_id_count);
+		void re_activate(STrack& new_track, int frame_id, bool new_id = false);
+		void update(STrack& new_track, int frame_id);
+
+	public:
+		bool is_activated;
+		int track_id;
+		int state;
+		int count;
+
+		std::vector<float> _tlwh;
+		std::vector<float> tlwh;
+		std::vector<float> tlbr;
+		float left; // left, top, right, bottom (original bounding of detected object) 
+		float top;
+		float right;
+		float bottom;
+		int frame_id;
+		int tracklet_len;
+		int start_frame;
+		std::string object_id;
+
+		KAL_MEAN mean;
+		KAL_COVA covariance;
+		float score;
+		int class_id;
+
+	private:
+		ByteTrackNCNN::ByteKalmanFilter kalman_filter;
+		std::unordered_map<int, float> class_id_scores_;
+		int detection_count_;
+		bool class_id_locked_;
+		static const int CLASS_ID_LOCK_FRAMES = 10;
+		void voteClassId(int new_class_id, float score);
+	};
+}
+

modules/ANSMOT/ByteTrackNCNN/include/NCNNdataType.h

@@ -0,0 +1,52 @@
+/*!
+    @Description : https://github.com/shaoshengsong/
+    @Author      : shaoshengsong
+    @Date        : 2022-09-21 05:49:06
+*/
+#pragma once
+#include <cstddef>
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+namespace ByteTrackNCNN {
+    const int k_feature_dim = 512;//feature dim
+
+    const std::string  k_feature_model_path = "feature.onnx";
+    const std::string  k_detect_model_path = "yolov5s.onnx";
+
+
+    typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> DETECTBOX;
+    typedef Eigen::Matrix<float, -1, 4, Eigen::RowMajor> DETECTBOXSS;
+    typedef Eigen::Matrix<float, 1, k_feature_dim, Eigen::RowMajor> FEATURE;
+    typedef Eigen::Matrix<float, Eigen::Dynamic, k_feature_dim, Eigen::RowMajor> FEATURESS;
+    //typedef std::vector<FEATURE> FEATURESS;
+
+    //Kalmanfilter
+    //typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_FILTER;
+    typedef Eigen::Matrix<float, 1, 8, Eigen::RowMajor> KAL_MEAN;
+    typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_COVA;
+    typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> KAL_HMEAN;
+    typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> KAL_HCOVA;
+    using KAL_DATA = std::pair<KAL_MEAN, KAL_COVA>;
+    using KAL_HDATA = std::pair<KAL_HMEAN, KAL_HCOVA>;
+
+    //main
+    using RESULT_DATA = std::pair<int, DETECTBOX>;
+
+    //tracker:
+    using TRACKER_DATA = std::pair<int, FEATURESS>;
+    using MATCH_DATA = std::pair<int, int>;
+    typedef struct t {
+        std::vector<MATCH_DATA> matches;
+        std::vector<int> unmatched_tracks;
+        std::vector<int> unmatched_detections;
+    }TRACHER_MATCHD;
+
+    //linear_assignment:
+    typedef Eigen::Matrix<float, -1, -1, Eigen::RowMajor> DYNAMICM;
+}
+
+
+
+

modules/ANSMOT/ByteTrackNCNN/include/NCNNlapjv.h

@@ -0,0 +1,65 @@
+#ifndef LAPJV_H
+#define LAPJV_H
+namespace ByteTrackNCNN
+{
+#define LARGE 1000000
+
+#if !defined TRUE
+#define TRUE 1
+#endif
+#if !defined FALSE
+#define FALSE 0
+#endif
+
+#define NEW(x, t, n) if ((x = (t *)malloc(sizeof(t) * (n))) == 0) { return -1; }
+#define FREE(x) if (x != 0) { free(x); x = 0; }
+#define SWAP_INDICES(a, b) { int_t _temp_index = a; a = b; b = _temp_index; }
+
+#if 0
+#include <assert.h>
+#define ASSERT(cond) assert(cond)
+#define PRINTF(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#define PRINT_COST_ARRAY(a, n) \
+    while (1) { \
+        printf(#a" = ["); \
+        if ((n) > 0) { \
+            printf("%f", (a)[0]); \
+            for (uint_t j = 1; j < n; j++) { \
+                printf(", %f", (a)[j]); \
+            } \
+        } \
+        printf("]\n"); \
+        break; \
+    }
+#define PRINT_INDEX_ARRAY(a, n) \
+    while (1) { \
+        printf(#a" = ["); \
+        if ((n) > 0) { \
+            printf("%d", (a)[0]); \
+            for (uint_t j = 1; j < n; j++) { \
+                printf(", %d", (a)[j]); \
+            } \
+        } \
+        printf("]\n"); \
+        break; \
+    }
+#else
+#define ASSERT(cond)
+#define PRINTF(fmt, ...)
+#define PRINT_COST_ARRAY(a, n)
+#define PRINT_INDEX_ARRAY(a, n)
+#endif
+
+
+    typedef signed int int_t;
+    typedef unsigned int uint_t;
+    typedef double cost_t;
+    typedef char boolean;
+    typedef enum fp_t { FP_1 = 1, FP_2 = 2, FP_DYNAMIC = 3 } fp_t;
+
+    extern int_t lapjv_internal(
+        const uint_t n, cost_t* cost[],
+        int_t* x, int_t* y);
+
+}
+#endif // LAPJV_H

modules/ANSMOT/ByteTrackNCNN/src/NCNNBYTETracker.cpp

@@ -0,0 +1,336 @@
+#include <fstream>
+#include <algorithm>
+#include "NCNNBYTETracker.h"
+#include "NCNNObject.h"
+namespace ByteTrackNCNN{
+	BYTETracker::BYTETracker(int frame_rate, int track_buffer)
+	{
+		track_thresh = 0.5;
+		high_thresh = 0.01; // Track everything
+		match_thresh = 0.95;
+		this->frame_id = 0;
+		max_time_lost = std::max(5, int(frame_rate / 30.0 * track_buffer));
+		track_id_count = 0;
+		track_buffer_ = track_buffer;
+		auto_frame_rate_ = false;
+		estimated_fps_ = static_cast<float>(frame_rate);
+		time_scale_factor_ = 1.0f;
+		fps_sample_count_ = 0;
+		has_last_update_time_ = false;
+		tracked_stracks.clear();
+		lost_stracks.clear();
+		removed_stracks.clear();
+	}
+
+	void BYTETracker::update_parameters(int frameRate, int trackBuffer, double trackThreshold, double highThreshold, double matchThresold, bool autoFrameRate)
+	{
+		track_thresh = (float)trackThreshold;
+		high_thresh = (float)highThreshold;
+		match_thresh = (float)matchThresold;
+		track_buffer_ = trackBuffer;
+		auto_frame_rate_ = autoFrameRate;
+		estimated_fps_ = static_cast<float>(frameRate);
+		time_scale_factor_ = 1.0f;
+		fps_sample_count_ = 0;
+		has_last_update_time_ = false;
+		max_time_lost = std::max(5, int(frameRate / 30.0 * trackBuffer));
+		this->frame_id = 0;
+		track_id_count = 0;
+		tracked_stracks.clear();
+		lost_stracks.clear();
+		removed_stracks.clear();
+	}
+
+	float BYTETracker::getEstimatedFps() const {
+		return estimated_fps_;
+	}
+
+	void BYTETracker::estimateFrameRate() {
+		auto now = std::chrono::steady_clock::now();
+
+		if (!has_last_update_time_) {
+			last_update_time_ = now;
+			has_last_update_time_ = true;
+			return;
+		}
+
+		double delta_sec = std::chrono::duration<double>(now - last_update_time_).count();
+		last_update_time_ = now;
+
+		if (delta_sec < 0.001 || delta_sec > 5.0) {
+			return;
+		}
+
+		float current_fps = static_cast<float>(1.0 / delta_sec);
+		current_fps = std::max(1.0f, std::min(current_fps, 120.0f));
+
+		fps_sample_count_++;
+
+		float alpha = (fps_sample_count_ <= 10) ? 0.3f : 0.1f;
+		estimated_fps_ = alpha * current_fps + (1.0f - alpha) * estimated_fps_;
+
+		// Compute time scale factor: ratio of actual interval to expected interval
+		float expected_dt = 1.0f / estimated_fps_;
+		time_scale_factor_ = static_cast<float>(delta_sec) / expected_dt;
+		time_scale_factor_ = std::max(0.5f, std::min(time_scale_factor_, 10.0f));
+
+		if (fps_sample_count_ >= 10) {
+			int new_max_time_lost = std::max(5, int(estimated_fps_ / 30.0 * track_buffer_));
+
+			double ratio = static_cast<double>(new_max_time_lost) / static_cast<double>(max_time_lost);
+			if (ratio > 1.1 || ratio < 0.9) {
+				max_time_lost = new_max_time_lost;
+			}
+		}
+	}
+	BYTETracker::~BYTETracker()
+	{
+	}
+	 std::vector<ByteTrackNCNN::STrack> BYTETracker::update(const  std::vector<ByteTrackNCNN::Object>& objects)
+	{
+		// Auto-estimate frame rate from update() call timing
+		if (auto_frame_rate_) {
+			estimateFrameRate();
+		}
+
+		////////////////// Step 1: Get detections //////////////////
+		this->frame_id++;
+		 std::vector<STrack> activated_stracks;
+		 std::vector<STrack> refind_stracks;
+		 std::vector<STrack> removed_stracks;
+		 std::vector<STrack> lost_stracks;
+		 std::vector<STrack> detections;
+		 std::vector<STrack> detections_low;
+
+		 std::vector<STrack> detections_cp;
+		 std::vector<STrack> tracked_stracks_swap;
+		 std::vector<STrack> resa, resb;
+		 std::vector<STrack> output_stracks;
+
+		 std::vector<STrack*> unconfirmed;
+		 std::vector<STrack*> tracked_stracks;
+		 std::vector<STrack*> strack_pool;
+		 std::vector<STrack*> r_tracked_stracks;
+
+		if (objects.size() > 0)
+		{
+			for (int i = 0; i < objects.size(); i++)
+			{
+				std::vector<float> tlbr_;
+				tlbr_.resize(4);
+				tlbr_[0] = objects[i].box.x();
+				tlbr_[1] = objects[i].box.y();
+				tlbr_[2] = objects[i].box.x() + objects[i].box.width();
+				tlbr_[3] = objects[i].box.y() + objects[i].box.height();
+				float score = objects[i].confidence;
+				int class_id = objects[i].label;
+				float left = objects[i].left;
+				float top = objects[i].top;
+				float right = objects[i].right;
+				float bottom = objects[i].bottom;
+				std::string object_id = objects[i].object_id;
+
+				STrack strack(STrack::tlbr_to_tlwh(tlbr_), score, class_id,left,top,right,bottom, object_id);
+				if (score >= track_thresh)
+				{
+					detections.push_back(strack);
+				}
+				else
+				{
+					detections_low.push_back(strack);
+				}
+			}
+		}
+
+		// Add newly detected tracklets to tracked_stracks
+		for (int i = 0; i < this->tracked_stracks.size(); i++)
+		{
+			if (!this->tracked_stracks[i].is_activated)
+				unconfirmed.push_back(&this->tracked_stracks[i]);
+			else
+				tracked_stracks.push_back(&this->tracked_stracks[i]);
+		}
+
+		////////////////// Step 2: First association, with IoU //////////////////
+		strack_pool = joint_stracks(tracked_stracks, this->lost_stracks);
+
+		// Multi-predict: call multi_predict() multiple times when frames are skipped
+		int num_predicts = 1;
+		if (auto_frame_rate_ && time_scale_factor_ > 1.5f) {
+			num_predicts = std::min(static_cast<int>(std::round(time_scale_factor_)), 10);
+		}
+		for (int p = 0; p < num_predicts; p++) {
+			STrack::multi_predict(strack_pool, this->kalman_filter);
+		}
+
+		// Adaptive matching: relax threshold during frame skips
+		float effective_match_thresh = match_thresh;
+		if (num_predicts > 1) {
+			effective_match_thresh = std::min(match_thresh + 0.005f * (num_predicts - 1), 0.99f);
+		}
+
+		 std::vector< std::vector<float> > dists;
+		int dist_size = 0, dist_size_size = 0;
+		dists = iou_distance(strack_pool, detections, dist_size, dist_size_size);
+
+		 std::vector< std::vector<int> > matches;
+		 std::vector<int> u_track, u_detection;
+		linear_assignment(dists, dist_size, dist_size_size, effective_match_thresh, matches, u_track, u_detection);
+
+		for (int i = 0; i < matches.size(); i++)
+		{
+			STrack *track = strack_pool[matches[i][0]];
+			STrack *det = &detections[matches[i][1]];
+			if (track->state == TrackState::Tracked)
+			{
+				track->update(*det, this->frame_id);
+				activated_stracks.push_back(*track);
+			}
+			else
+			{
+				track->re_activate(*det, this->frame_id, false);
+				refind_stracks.push_back(*track);
+			}
+		}
+
+		////////////////// Step 3: Second association, using low score dets //////////////////
+		for (int i = 0; i < u_detection.size(); i++)
+		{
+			detections_cp.push_back(detections[u_detection[i]]);
+		}
+		detections.clear();
+		detections.assign(detections_low.begin(), detections_low.end());
+	
+		for (int i = 0; i < u_track.size(); i++)
+		{
+			if (strack_pool[u_track[i]]->state == TrackState::Tracked)
+			{
+				r_tracked_stracks.push_back(strack_pool[u_track[i]]);
+			}
+		}
+
+		dists.clear();
+		dists = iou_distance(r_tracked_stracks, detections, dist_size, dist_size_size);
+
+		matches.clear();
+		u_track.clear();
+		u_detection.clear();
+		linear_assignment(dists, dist_size, dist_size_size, 0.5, matches, u_track, u_detection);
+
+		for (int i = 0; i < matches.size(); i++)
+		{
+			STrack *track = r_tracked_stracks[matches[i][0]];
+			STrack *det = &detections[matches[i][1]];
+			if (track->state == TrackState::Tracked)
+			{
+				track->update(*det, this->frame_id);
+				activated_stracks.push_back(*track);
+			}
+			else
+			{
+				track->re_activate(*det, this->frame_id, false);
+				refind_stracks.push_back(*track);
+			}
+		}
+
+		for (int i = 0; i < u_track.size(); i++)
+		{
+			STrack *track = r_tracked_stracks[u_track[i]];
+			if (track->state != TrackState::Lost)
+			{
+				track->mark_lost();
+				lost_stracks.push_back(*track);
+			}
+		}
+
+		// Deal with unconfirmed tracks, usually tracks with only one beginning frame
+		detections.clear();
+		detections.assign(detections_cp.begin(), detections_cp.end());
+
+		dists.clear();
+		dists = iou_distance(unconfirmed, detections, dist_size, dist_size_size);
+
+		matches.clear();
+		std::vector<int> u_unconfirmed;
+		u_detection.clear();
+		linear_assignment(dists, dist_size, dist_size_size, 0.7, matches, u_unconfirmed, u_detection);
+
+		for (int i = 0; i < matches.size(); i++)
+		{
+			unconfirmed[matches[i][0]]->update(detections[matches[i][1]], this->frame_id);
+			activated_stracks.push_back(*unconfirmed[matches[i][0]]);
+		}
+
+		for (int i = 0; i < u_unconfirmed.size(); i++)
+		{
+			STrack *track = unconfirmed[u_unconfirmed[i]];
+			track->mark_removed();
+			removed_stracks.push_back(*track);
+		}
+
+		////////////////// Step 4: Init new stracks //////////////////
+		for (int i = 0; i < u_detection.size(); i++)
+		{
+			STrack *track = &detections[u_detection[i]];
+			if (track->score < this->high_thresh)
+				continue;
+			track_id_count++;
+			track->activate(this->kalman_filter, this->frame_id, track_id_count);
+			activated_stracks.push_back(*track);
+		}
+
+		////////////////// Step 5: Update state //////////////////
+		for (int i = 0; i < this->lost_stracks.size(); i++)
+		{
+			if (this->frame_id - this->lost_stracks[i].end_frame() > this->max_time_lost)
+			{
+				this->lost_stracks[i].mark_removed();
+				removed_stracks.push_back(this->lost_stracks[i]);
+			}
+		}
+	
+		for (int i = 0; i < this->tracked_stracks.size(); i++)
+		{
+			if (this->tracked_stracks[i].state == TrackState::Tracked)
+			{
+				tracked_stracks_swap.push_back(this->tracked_stracks[i]);
+			}
+		}
+		this->tracked_stracks.clear();
+		this->tracked_stracks.assign(tracked_stracks_swap.begin(), tracked_stracks_swap.end());
+
+		this->tracked_stracks = joint_stracks(this->tracked_stracks, activated_stracks);
+		this->tracked_stracks = joint_stracks(this->tracked_stracks, refind_stracks);
+
+		//std::cout << activated_stracks.size() << std::endl;
+
+		this->lost_stracks = sub_stracks(this->lost_stracks, this->tracked_stracks);
+		for (int i = 0; i < lost_stracks.size(); i++)
+		{
+			this->lost_stracks.push_back(lost_stracks[i]);
+		}
+
+		this->lost_stracks = sub_stracks(this->lost_stracks, this->removed_stracks);
+		for (int i = 0; i < removed_stracks.size(); i++)
+		{
+			this->removed_stracks.push_back(removed_stracks[i]);
+		}
+	
+		remove_duplicate_stracks(resa, resb, this->tracked_stracks, this->lost_stracks);
+
+		this->tracked_stracks.clear();
+		this->tracked_stracks.assign(resa.begin(), resa.end());
+		this->lost_stracks.clear();
+		this->lost_stracks.assign(resb.begin(), resb.end());
+	
+		for (int i = 0; i < this->tracked_stracks.size(); i++)
+		{
+			output_stracks.push_back(this->tracked_stracks[i]);
+	/*		if (this->tracked_stracks[i].is_activated)
+			{
+				output_stracks.push_back(this->tracked_stracks[i]);
+			}*/
+		}
+		return output_stracks;
+	}	
+}

modules/ANSMOT/ByteTrackNCNN/src/NCNNBytekalmanFilter.cpp

@@ -0,0 +1,152 @@
+#include "NCNNBytekalmanFilter.h"
+#include <Eigen/Cholesky>
+
+namespace ByteTrackNCNN
+{
+	const double ByteKalmanFilter::chi2inv95[10] = {
+	0,
+	3.8415,
+	5.9915,
+	7.8147,
+	9.4877,
+	11.070,
+	12.592,
+	14.067,
+	15.507,
+	16.919
+	};
+	ByteKalmanFilter::ByteKalmanFilter()
+	{
+		int ndim = 4;
+		double dt = 1.;
+
+		_motion_mat = Eigen::MatrixXf::Identity(8, 8);
+		for (int i = 0; i < ndim; i++) {
+			_motion_mat(i, ndim + i) = dt;
+		}
+		_update_mat = Eigen::MatrixXf::Identity(4, 8);
+
+		this->_std_weight_position = 1. / 20;
+		this->_std_weight_velocity = 1. / 160;
+	}
+
+	KAL_DATA ByteKalmanFilter::initiate(const DETECTBOX &measurement)
+	{
+		DETECTBOX mean_pos = measurement;
+		DETECTBOX mean_vel;
+		for (int i = 0; i < 4; i++) mean_vel(i) = 0;
+
+		KAL_MEAN mean;
+		for (int i = 0; i < 8; i++) {
+			if (i < 4) mean(i) = mean_pos(i);
+			else mean(i) = mean_vel(i - 4);
+		}
+
+		KAL_MEAN std;
+		std(0) = 2 * _std_weight_position * measurement[3];
+		std(1) = 2 * _std_weight_position * measurement[3];
+		std(2) = 1e-2;
+		std(3) = 2 * _std_weight_position * measurement[3];
+		std(4) = 10 * _std_weight_velocity * measurement[3];
+		std(5) = 10 * _std_weight_velocity * measurement[3];
+		std(6) = 1e-5;
+		std(7) = 10 * _std_weight_velocity * measurement[3];
+
+		KAL_MEAN tmp = std.array().square();
+		KAL_COVA var = tmp.asDiagonal();
+		return std::make_pair(mean, var);
+	}
+
+	void ByteKalmanFilter::predict(KAL_MEAN &mean, KAL_COVA &covariance)
+	{
+		//revise the data;
+		DETECTBOX std_pos;
+		std_pos << _std_weight_position * mean(3),
+			_std_weight_position * mean(3),
+			1e-2,
+			_std_weight_position * mean(3);
+		DETECTBOX std_vel;
+		std_vel << _std_weight_velocity * mean(3),
+			_std_weight_velocity * mean(3),
+			1e-5,
+			_std_weight_velocity * mean(3);
+		KAL_MEAN tmp;
+		tmp.block<1, 4>(0, 0) = std_pos;
+		tmp.block<1, 4>(0, 4) = std_vel;
+		tmp = tmp.array().square();
+		KAL_COVA motion_cov = tmp.asDiagonal();
+		KAL_MEAN mean1 = this->_motion_mat * mean.transpose();
+		KAL_COVA covariance1 = this->_motion_mat * covariance *(_motion_mat.transpose());
+		covariance1 += motion_cov;
+
+		mean = mean1;
+		covariance = covariance1;
+	}
+
+	KAL_HDATA ByteKalmanFilter::project(const KAL_MEAN &mean, const KAL_COVA &covariance)
+	{
+		DETECTBOX std;
+		std << _std_weight_position * mean(3), _std_weight_position * mean(3),
+			1e-1, _std_weight_position * mean(3);
+		KAL_HMEAN mean1 = _update_mat * mean.transpose();
+		KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose());
+		Eigen::Matrix<float, 4, 4> diag = std.asDiagonal();
+		diag = diag.array().square().matrix();
+		covariance1 += diag;
+		//    covariance1.diagonal() << diag;
+		return std::make_pair(mean1, covariance1);
+	}
+
+	KAL_DATA
+		ByteKalmanFilter::update(
+			const KAL_MEAN &mean,
+			const KAL_COVA &covariance,
+			const DETECTBOX &measurement)
+	{
+		KAL_HDATA pa = project(mean, covariance);
+		KAL_HMEAN projected_mean = pa.first;
+		KAL_HCOVA projected_cov = pa.second;
+
+		//chol_factor, lower =
+		//scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)
+		//kalmain_gain =
+		//scipy.linalg.cho_solve((cho_factor, lower),
+		//np.dot(covariance, self._upadte_mat.T).T,
+		//check_finite=False).T
+		Eigen::Matrix<float, 4, 8> B = (covariance * (_update_mat.transpose())).transpose();
+		Eigen::Matrix<float, 8, 4> kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4
+		Eigen::Matrix<float, 1, 4> innovation = measurement - projected_mean; //eg.1x4
+		auto tmp = innovation * (kalman_gain.transpose());
+		KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix();
+		KAL_COVA new_covariance = covariance - kalman_gain * projected_cov*(kalman_gain.transpose());
+		return std::make_pair(new_mean, new_covariance);
+	}
+
+	Eigen::Matrix<float, 1, -1>
+		ByteKalmanFilter::gating_distance(
+			const KAL_MEAN &mean,
+			const KAL_COVA &covariance,
+			const std::vector<DETECTBOX> &measurements,
+			bool only_position)
+	{
+		KAL_HDATA pa = this->project(mean, covariance);
+		if (only_position) {
+			printf("not implement!");
+			exit(0);
+		}
+		KAL_HMEAN mean1 = pa.first;
+		KAL_HCOVA covariance1 = pa.second;
+
+		//    Eigen::Matrix<float, -1, 4, Eigen::RowMajor> d(size, 4);
+		DETECTBOXSS d(measurements.size(), 4);
+		int pos = 0;
+		for (DETECTBOX box : measurements) {
+			d.row(pos++) = box - mean1;
+		}
+		Eigen::Matrix<float, -1, -1, Eigen::RowMajor> factor = covariance1.llt().matrixL();
+		Eigen::Matrix<float, -1, -1> z = factor.triangularView<Eigen::Lower>().solve<Eigen::OnTheRight>(d).transpose();
+		auto zz = ((z.array())*(z.array())).matrix();
+		auto square_maha = zz.colwise().sum();
+		return square_maha;
+	}
+}

modules/ANSMOT/ByteTrackNCNN/src/NCNNObject.cpp

@@ -0,0 +1,21 @@
+#include "NCNNObject.h"
+#include "NCNNRect.h"
+ByteTrackNCNN::Object::Object(const ByteTrackNCNN::Rect<float>& _rect,
+								const int& _label,
+								const float& _prob,
+								const float& _left,
+								const float& _top,
+								const float& _right,
+								const float& _bottom,
+								const std::string& _object_id) : 
+								box(_rect),
+								label(_label),
+								confidence(_prob),
+								left(_left),
+								top(_top),
+								right(_right),
+								bottom(_bottom),
+								object_id(_object_id)
+{
+
+}

modules/ANSMOT/ByteTrackNCNN/src/NCNNRect.cpp

@@ -0,0 +1,147 @@
+#include <algorithm>
+#include "NCNNRect.h"
+template <typename T>
+ByteTrackNCNN::Rect<T>::Rect(const T& x, const T& y, const T& width, const T& height) :
+	tlwh({ x, y, width, height })
+{
+}
+
+template <typename T>
+ByteTrackNCNN::Rect<T>::~Rect()
+{
+}
+
+template <typename T>
+const T& ByteTrackNCNN::Rect<T>::x() const
+{
+	return tlwh[0];
+}
+
+template <typename T>
+const T& ByteTrackNCNN::Rect<T>::y() const
+{
+	return tlwh[1];
+}
+
+template <typename T>
+const T& ByteTrackNCNN::Rect<T>::width() const
+{
+	return tlwh[2];
+}
+
+template <typename T>
+const T& ByteTrackNCNN::Rect<T>::height() const
+{
+	return tlwh[3];
+}
+
+template <typename T>
+T& ByteTrackNCNN::Rect<T>::x()
+{
+	return tlwh[0];
+}
+
+template <typename T>
+T& ByteTrackNCNN::Rect<T>::y()
+{
+	return tlwh[1];
+}
+
+template <typename T>
+T& ByteTrackNCNN::Rect<T>::width()
+{
+	return tlwh[2];
+}
+
+template <typename T>
+T& ByteTrackNCNN::Rect<T>::height()
+{
+	return tlwh[3];
+}
+
+template <typename T>
+const T& ByteTrackNCNN::Rect<T>::tl_x() const
+{
+	return tlwh[0];
+}
+
+template <typename T>
+const T& ByteTrackNCNN::Rect<T>::tl_y() const
+{
+	return tlwh[1];
+}
+
+template <typename T>
+T ByteTrackNCNN::Rect<T>::br_x() const
+{
+	return tlwh[0] + tlwh[2];
+}
+
+template <typename T>
+T ByteTrackNCNN::Rect<T>::br_y() const
+{
+	return tlwh[1] + tlwh[3];
+}
+
+template <typename T>
+ByteTrackNCNN::Tlbr<T> ByteTrackNCNN::Rect<T>::getTlbr() const
+{
+	return {
+		tlwh[0],
+		tlwh[1],
+		tlwh[0] + tlwh[2],
+		tlwh[1] + tlwh[3],
+	};
+}
+
+template <typename T>
+ByteTrackNCNN::Xyah<T> ByteTrackNCNN::Rect<T>::getXyah() const
+{
+	return {
+		tlwh[0] + tlwh[2] / 2,
+		tlwh[1] + tlwh[3] / 2,
+		tlwh[2] / tlwh[3],
+		tlwh[3],
+	};
+}
+
+template<typename T>
+float ByteTrackNCNN::Rect<T>::calcIoU(const Rect<T>& other) const
+{
+	const float box_area = (other.tlwh[2] + 1) * (other.tlwh[3] + 1);
+	const float iw = std::min(tlwh[0] + tlwh[2], other.tlwh[0] + other.tlwh[2]) - std::max(tlwh[0], other.tlwh[0]) + 1;
+	float iou = 0;
+	if (iw > 0)
+	{
+		const float ih = std::min(tlwh[1] + tlwh[3], other.tlwh[1] + other.tlwh[3]) - std::max(tlwh[1], other.tlwh[1]) + 1;
+		if (ih > 0)
+		{
+			const float ua = (tlwh[0] + tlwh[2] - tlwh[0] + 1) * (tlwh[1] + tlwh[3] - tlwh[1] + 1) + box_area - iw * ih;
+			iou = iw * ih / ua;
+		}
+	}
+	return iou;
+}
+
+template<typename T>
+ByteTrackNCNN::Rect<T> generate_rect_by_tlbr(const ByteTrackNCNN::Tlbr<T>& tlbr)
+{
+	return ByteTrackNCNN::Rect<T>(tlbr[0], tlbr[1], tlbr[2] - tlbr[0], tlbr[3] - tlbr[1]);
+}
+
+template<typename T>
+ByteTrackNCNN::Rect<T> generate_rect_by_xyah(const ByteTrackNCNN::Xyah<T>& xyah)
+{
+	const auto width = xyah[2] * xyah[3];
+	return ByteTrackNCNN::Rect<T>(xyah[0] - width / 2, xyah[1] - xyah[3] / 2, width, xyah[3]);
+}
+
+// explicit instantiation
+template class ByteTrackNCNN::Rect<int>;
+template class ByteTrackNCNN::Rect<float>;
+
+template ByteTrackNCNN::Rect<int> ByteTrackNCNN::generate_rect_by_tlbr<int>(const ByteTrackNCNN::Tlbr<int>&);
+template ByteTrackNCNN::Rect<float> ByteTrackNCNN::generate_rect_by_tlbr<float>(const ByteTrackNCNN::Tlbr<float>&);
+
+template ByteTrackNCNN::Rect<int> ByteTrackNCNN::generate_rect_by_xyah<int>(const ByteTrackNCNN::Xyah<int>&);
+template ByteTrackNCNN::Rect<float> ByteTrackNCNN::generate_rect_by_xyah<float>(const ByteTrackNCNN::Xyah<float>&);

modules/ANSMOT/ByteTrackNCNN/src/NCNNSTrack.cpp

@@ -0,0 +1,283 @@
+#include "NCNNSTrack.h"
+namespace ByteTrackNCNN {
+	
+	STrack::STrack(std::vector<float> tlwh_, float _score,
+				   int _class_id, float _left, float _top,
+				   float _right, float _bottom, std::string _object_id)
+	{
+		this->_tlwh.resize(4);
+		this->_tlwh.assign(tlwh_.begin(), tlwh_.end());
+
+		this->is_activated = false;
+		this->track_id = 0;
+		this->state = TrackState::New;
+		
+		this->tlwh.resize(4);
+		this->tlbr.resize(4);
+
+		this->static_tlwh();
+		this->static_tlbr();
+		this->frame_id = 0;
+		this->tracklet_len = 0;
+		this->start_frame = 0;
+
+		this->score = _score;
+		this->class_id = _class_id;
+		this->left = _left;
+		this->top = _top;
+		this->right = _right;
+		this->bottom = _bottom;
+		this->object_id = _object_id;
+		this->reset_count();
+		this->detection_count_ = 1;
+		this->class_id_locked_ = false;
+		this->class_id_scores_[_class_id] = _score;
+	}
+
+	STrack::~STrack()
+	{
+	}
+
+	void STrack::voteClassId(int new_class_id, float score)
+	{
+		if (class_id_locked_) return;
+
+		class_id_scores_[new_class_id] += score;
+		detection_count_++;
+
+		int best_id = class_id;
+		float best_score = 0.0f;
+		for (const auto& entry : class_id_scores_)
+		{
+			if (entry.second > best_score)
+			{
+				best_score = entry.second;
+				best_id = entry.first;
+			}
+		}
+		class_id = best_id;
+
+		if (detection_count_ >= CLASS_ID_LOCK_FRAMES)
+		{
+			class_id_locked_ = true;
+		}
+	}
+	void STrack::reset_count() {
+		count = 0;
+	}
+
+	void STrack::activate(ByteTrackNCNN::ByteKalmanFilter &kalman_filter, int frame_id)
+	{
+		this->kalman_filter = kalman_filter;
+		this->track_id = this->next_id();
+
+		 std::vector<float> _tlwh_tmp(4);
+		_tlwh_tmp[0] = this->_tlwh[0];
+		_tlwh_tmp[1] = this->_tlwh[1];
+		_tlwh_tmp[2] = this->_tlwh[2];
+		_tlwh_tmp[3] = this->_tlwh[3];
+		 std::vector<float> xyah = tlwh_to_xyah(_tlwh_tmp);
+		DETECTBOX xyah_box;
+		xyah_box[0] = xyah[0];
+		xyah_box[1] = xyah[1];
+		xyah_box[2] = xyah[2];
+		xyah_box[3] = xyah[3];
+		auto mc = this->kalman_filter.initiate(xyah_box);
+		this->mean = mc.first;
+		this->covariance = mc.second;
+
+		static_tlwh();
+		static_tlbr();
+
+		this->tracklet_len = 0;
+		this->state = TrackState::Tracked;
+		if (frame_id == 1)
+		{
+			this->is_activated = true;
+		}
+		//this->is_activated = true;
+		this->frame_id = frame_id;
+		this->start_frame = frame_id;
+	}
+	void STrack::activate(ByteTrackNCNN::ByteKalmanFilter& kalman_filter, int frame_id, int track_id_count)
+	{
+		this->kalman_filter = kalman_filter;
+		this->track_id = track_id_count;
+
+		std::vector<float> _tlwh_tmp(4);
+		_tlwh_tmp[0] = this->_tlwh[0];
+		_tlwh_tmp[1] = this->_tlwh[1];
+		_tlwh_tmp[2] = this->_tlwh[2];
+		_tlwh_tmp[3] = this->_tlwh[3];
+		std::vector<float> xyah = tlwh_to_xyah(_tlwh_tmp);
+		DETECTBOX xyah_box;
+		xyah_box[0] = xyah[0];
+		xyah_box[1] = xyah[1];
+		xyah_box[2] = xyah[2];
+		xyah_box[3] = xyah[3];
+		auto mc = this->kalman_filter.initiate(xyah_box);
+		this->mean = mc.first;
+		this->covariance = mc.second;
+
+		static_tlwh();
+		static_tlbr();
+
+		this->tracklet_len = 0;
+		this->state = TrackState::Tracked;
+		if (frame_id == 1)
+		{
+			this->is_activated = true;
+		}
+		//this->is_activated = true;
+		this->frame_id = frame_id;
+		this->start_frame = frame_id;
+	}
+
+	void STrack::re_activate(STrack &new_track, int frame_id, bool new_id)
+	{
+		 std::vector<float> xyah = tlwh_to_xyah(new_track.tlwh);
+		DETECTBOX xyah_box;
+		xyah_box[0] = xyah[0];
+		xyah_box[1] = xyah[1];
+		xyah_box[2] = xyah[2];
+		xyah_box[3] = xyah[3];
+		auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box);
+		this->mean = mc.first;
+		this->covariance = mc.second;
+
+		static_tlwh();
+		static_tlbr();
+
+		this->tracklet_len = 0;
+		this->state = TrackState::Tracked;
+		this->is_activated = true;
+		this->frame_id = frame_id;
+		this->score = new_track.score;
+		voteClassId(new_track.class_id, new_track.score);
+		this->left = new_track.left;
+		this->top = new_track.top;
+		this->right = new_track.right;
+		this->bottom = new_track.bottom;
+		this->object_id = new_track.object_id;
+		if (new_id)
+			this->track_id = next_id();
+	}
+
+	void STrack::update(STrack &new_track, int frame_id)
+	{
+		this->frame_id = frame_id;
+		this->tracklet_len++;
+
+		 std::vector<float> xyah = tlwh_to_xyah(new_track.tlwh);
+		DETECTBOX xyah_box;
+		xyah_box[0] = xyah[0];
+		xyah_box[1] = xyah[1];
+		xyah_box[2] = xyah[2];
+		xyah_box[3] = xyah[3];
+
+		auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box);
+		this->mean = mc.first;
+		this->covariance = mc.second;
+
+		static_tlwh();
+		static_tlbr();
+
+		this->state = TrackState::Tracked;
+		this->is_activated = true;
+		this->score = new_track.score;
+		voteClassId(new_track.class_id, new_track.score);
+		this->left = new_track.left;
+		this->top = new_track.top;
+		this->right = new_track.right;
+		this->bottom = new_track.bottom;
+		this->object_id = new_track.object_id;
+	}
+
+	void STrack::static_tlwh()
+	{
+		if (this->state == TrackState::New)
+		{
+			tlwh[0] = _tlwh[0];
+			tlwh[1] = _tlwh[1];
+			tlwh[2] = _tlwh[2];
+			tlwh[3] = _tlwh[3];
+			return;
+		}
+
+		tlwh[0] = mean[0];
+		tlwh[1] = mean[1];
+		tlwh[2] = mean[2];
+		tlwh[3] = mean[3];
+
+		tlwh[2] *= tlwh[3];
+		tlwh[0] -= tlwh[2] / 2;
+		tlwh[1] -= tlwh[3] / 2;
+	}
+
+	void STrack::static_tlbr()
+	{
+		tlbr.clear();
+		tlbr.assign(tlwh.begin(), tlwh.end());
+		tlbr[2] += tlbr[0];
+		tlbr[3] += tlbr[1];
+	}
+
+	 std::vector<float> STrack::tlwh_to_xyah( std::vector<float> tlwh_tmp)
+	{
+		 std::vector<float> tlwh_output = tlwh_tmp;
+		tlwh_output[0] += tlwh_output[2] / 2;
+		tlwh_output[1] += tlwh_output[3] / 2;
+		if (tlwh_output[3] > 0)
+			tlwh_output[2] /= tlwh_output[3];
+		else
+			tlwh_output[2] = 0;
+		return tlwh_output;
+	}
+
+	 std::vector<float> STrack::to_xyah()
+	{
+		return tlwh_to_xyah(tlwh);
+	}
+
+	 std::vector<float> STrack::tlbr_to_tlwh( std::vector<float> &tlbr)
+	{
+		tlbr[2] -= tlbr[0];
+		tlbr[3] -= tlbr[1];
+		return tlbr;
+	}
+
+	void STrack::mark_lost()
+	{
+		state = TrackState::Lost;
+	}
+
+	void STrack::mark_removed()
+	{
+		state = TrackState::Removed;
+	}
+
+	int STrack::next_id()
+	{
+		count++;
+		return count;
+	}
+
+	int STrack::end_frame()
+	{
+		return this->frame_id;
+	}
+
+	void STrack::multi_predict( std::vector<STrack*> &stracks, ByteTrackNCNN::ByteKalmanFilter &kalman_filter)
+	{
+		for (int i = 0; i < stracks.size(); i++)
+		{
+			if (stracks[i]->state != TrackState::Tracked)
+			{
+				stracks[i]->mean[7] = 0;
+			}
+			kalman_filter.predict(stracks[i]->mean, stracks[i]->covariance);
+			stracks[i]->static_tlwh();
+			stracks[i]->static_tlbr();
+		}
+	}
+}

modules/ANSMOT/ByteTrackNCNN/src/NCNNlapjv.cpp

@@ -0,0 +1,346 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "NCNNlapjv.h"
+namespace ByteTrackNCNN
+{
+	/** Column-reduction and reduction transfer for a dense cost matrix.
+	 */
+	int_t _ccrrt_dense(const uint_t n, cost_t* cost[],
+		int_t* free_rows, int_t* x, int_t* y, cost_t* v)
+	{
+		int_t n_free_rows;
+		boolean* unique;
+
+		for (uint_t i = 0; i < n; i++) {
+			x[i] = -1;
+			v[i] = LARGE;
+			y[i] = 0;
+		}
+		for (uint_t i = 0; i < n; i++) {
+			for (uint_t j = 0; j < n; j++) {
+				const cost_t c = cost[i][j];
+				if (c < v[j]) {
+					v[j] = c;
+					y[j] = i;
+				}
+				PRINTF("i=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, j, c, v[j], y[j]);
+			}
+		}
+		PRINT_COST_ARRAY(v, n);
+		PRINT_INDEX_ARRAY(y, n);
+		NEW(unique, boolean, n);
+		memset(unique, TRUE, n);
+		{
+			int_t j = n;
+			do {
+				j--;
+				const int_t i = y[j];
+				if (x[i] < 0) {
+					x[i] = j;
+				}
+				else {
+					unique[i] = FALSE;
+					y[j] = -1;
+				}
+			} while (j > 0);
+		}
+		n_free_rows = 0;
+		for (uint_t i = 0; i < n; i++) {
+			if (x[i] < 0) {
+				free_rows[n_free_rows++] = i;
+			}
+			else if (unique[i]) {
+				const int_t j = x[i];
+				cost_t min = LARGE;
+				for (uint_t j2 = 0; j2 < n; j2++) {
+					if (j2 == (uint_t)j) {
+						continue;
+					}
+					const cost_t c = cost[i][j2] - v[j2];
+					if (c < min) {
+						min = c;
+					}
+				}
+				PRINTF("v[%d] = %f - %f\n", j, v[j], min);
+				v[j] -= min;
+			}
+		}
+		FREE(unique);
+		return n_free_rows;
+	}
+
+
+	/** Augmenting row reduction for a dense cost matrix.
+	 */
+	int_t _carr_dense(
+		const uint_t n, cost_t* cost[],
+		const uint_t n_free_rows,
+		int_t* free_rows, int_t* x, int_t* y, cost_t* v)
+	{
+		uint_t current = 0;
+		int_t new_free_rows = 0;
+		uint_t rr_cnt = 0;
+		PRINT_INDEX_ARRAY(x, n);
+		PRINT_INDEX_ARRAY(y, n);
+		PRINT_COST_ARRAY(v, n);
+		PRINT_INDEX_ARRAY(free_rows, n_free_rows);
+		while (current < n_free_rows) {
+			int_t i0;
+			int_t j1, j2;
+			cost_t v1, v2, v1_new;
+			boolean v1_lowers;
+
+			rr_cnt++;
+			PRINTF("current = %d rr_cnt = %d\n", current, rr_cnt);
+			const int_t free_i = free_rows[current++];
+			j1 = 0;
+			v1 = cost[free_i][0] - v[0];
+			j2 = -1;
+			v2 = LARGE;
+			for (uint_t j = 1; j < n; j++) {
+				PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2);
+				const cost_t c = cost[free_i][j] - v[j];
+				if (c < v2) {
+					if (c >= v1) {
+						v2 = c;
+						j2 = j;
+					}
+					else {
+						v2 = v1;
+						v1 = c;
+						j2 = j1;
+						j1 = j;
+					}
+				}
+			}
+			i0 = y[j1];
+			v1_new = v[j1] - (v2 - v1);
+			v1_lowers = v1_new < v[j1];
+			PRINTF("%d %d 1=%d,%f 2=%d,%f v1'=%f(%d,%g) \n", free_i, i0, j1, v1, j2, v2, v1_new, v1_lowers, v[j1] - v1_new);
+			if (rr_cnt < current * n) {
+				if (v1_lowers) {
+					v[j1] = v1_new;
+				}
+				else if (i0 >= 0 && j2 >= 0) {
+					j1 = j2;
+					i0 = y[j2];
+				}
+				if (i0 >= 0) {
+					if (v1_lowers) {
+						free_rows[--current] = i0;
+					}
+					else {
+						free_rows[new_free_rows++] = i0;
+					}
+				}
+			}
+			else {
+				PRINTF("rr_cnt=%d >= %d (current=%d * n=%d)\n", rr_cnt, current * n, current, n);
+				if (i0 >= 0) {
+					free_rows[new_free_rows++] = i0;
+				}
+			}
+			x[free_i] = j1;
+			y[j1] = free_i;
+		}
+		return new_free_rows;
+	}
+
+
+	/** Find columns with minimum d[j] and put them on the SCAN list.
+	 */
+	uint_t _find_dense(const uint_t n, uint_t lo, cost_t* d, int_t* cols, int_t* y)
+	{
+		uint_t hi = lo + 1;
+		cost_t mind = d[cols[lo]];
+		for (uint_t k = hi; k < n; k++) {
+			int_t j = cols[k];
+			if (d[j] <= mind) {
+				if (d[j] < mind) {
+					hi = lo;
+					mind = d[j];
+				}
+				cols[k] = cols[hi];
+				cols[hi++] = j;
+			}
+		}
+		return hi;
+	}
+
+
+	// Scan all columns in TODO starting from arbitrary column in SCAN
+	// and try to decrease d of the TODO columns using the SCAN column.
+	int_t _scan_dense(const uint_t n, cost_t* cost[],
+		uint_t* plo, uint_t* phi,
+		cost_t* d, int_t* cols, int_t* pred,
+		int_t* y, cost_t* v)
+	{
+		uint_t lo = *plo;
+		uint_t hi = *phi;
+		cost_t h, cred_ij;
+
+		while (lo != hi) {
+			int_t j = cols[lo++];
+			const int_t i = y[j];
+			const cost_t mind = d[j];
+			h = cost[i][j] - v[j] - mind;
+			PRINTF("i=%d j=%d h=%f\n", i, j, h);
+			// For all columns in TODO
+			for (uint_t k = hi; k < n; k++) {
+				j = cols[k];
+				cred_ij = cost[i][j] - v[j] - h;
+				if (cred_ij < d[j]) {
+					d[j] = cred_ij;
+					pred[j] = i;
+					if (cred_ij == mind) {
+						if (y[j] < 0) {
+							return j;
+						}
+						cols[k] = cols[hi];
+						cols[hi++] = j;
+					}
+				}
+			}
+		}
+		*plo = lo;
+		*phi = hi;
+		return -1;
+	}
+
+
+	/** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper.
+	 *
+	 * This is a dense matrix version.
+	 *
+	 * \return The closest free column index.
+	 */
+	int_t find_path_dense(
+		const uint_t n, cost_t* cost[],
+		const int_t start_i,
+		int_t* y, cost_t* v,
+		int_t* pred)
+	{
+		uint_t lo = 0, hi = 0;
+		int_t final_j = -1;
+		uint_t n_ready = 0;
+		int_t* cols;
+		cost_t* d;
+
+		NEW(cols, int_t, n);
+		NEW(d, cost_t, n);
+
+		for (uint_t i = 0; i < n; i++) {
+			cols[i] = i;
+			pred[i] = start_i;
+			d[i] = cost[start_i][i] - v[i];
+		}
+		PRINT_COST_ARRAY(d, n);
+		while (final_j == -1) {
+			// No columns left on the SCAN list.
+			if (lo == hi) {
+				PRINTF("%d..%d -> find\n", lo, hi);
+				n_ready = lo;
+				hi = _find_dense(n, lo, d, cols, y);
+				PRINTF("check %d..%d\n", lo, hi);
+				PRINT_INDEX_ARRAY(cols, n);
+				for (uint_t k = lo; k < hi; k++) {
+					const int_t j = cols[k];
+					if (y[j] < 0) {
+						final_j = j;
+					}
+				}
+			}
+			if (final_j == -1) {
+				PRINTF("%d..%d -> scan\n", lo, hi);
+				final_j = _scan_dense(
+					n, cost, &lo, &hi, d, cols, pred, y, v);
+				PRINT_COST_ARRAY(d, n);
+				PRINT_INDEX_ARRAY(cols, n);
+				PRINT_INDEX_ARRAY(pred, n);
+			}
+		}
+
+		PRINTF("found final_j=%d\n", final_j);
+		PRINT_INDEX_ARRAY(cols, n);
+		{
+			const cost_t mind = d[cols[lo]];
+			for (uint_t k = 0; k < n_ready; k++) {
+				const int_t j = cols[k];
+				v[j] += d[j] - mind;
+			}
+		}
+
+		FREE(cols);
+		FREE(d);
+
+		return final_j;
+	}
+
+
+	/** Augment for a dense cost matrix.
+	 */
+	int_t _ca_dense(
+		const uint_t n, cost_t* cost[],
+		const uint_t n_free_rows,
+		int_t* free_rows, int_t* x, int_t* y, cost_t* v)
+	{
+		int_t* pred;
+
+		NEW(pred, int_t, n);
+
+		for (int_t* pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) {
+			int_t i = -1, j;
+			uint_t k = 0;
+
+			PRINTF("looking at free_i=%d\n", *pfree_i);
+			j = find_path_dense(n, cost, *pfree_i, y, v, pred);
+			ASSERT(j >= 0);
+			ASSERT(j < n);
+			while (i != *pfree_i) {
+				PRINTF("augment %d\n", j);
+				PRINT_INDEX_ARRAY(pred, n);
+				i = pred[j];
+				PRINTF("y[%d]=%d -> %d\n", j, y[j], i);
+				y[j] = i;
+				PRINT_INDEX_ARRAY(x, n);
+				SWAP_INDICES(j, x[i]);
+				k++;
+				if (k >= n) {
+					ASSERT(FALSE);
+				}
+			}
+		}
+		FREE(pred);
+		return 0;
+	}
+
+
+	/** Solve dense sparse LAP.
+	 */
+	int lapjv_internal(
+		const uint_t n, cost_t* cost[],
+		int_t* x, int_t* y)
+	{
+		int ret;
+		int_t* free_rows;
+		cost_t* v;
+
+		NEW(free_rows, int_t, n);
+		NEW(v, cost_t, n);
+		ret = _ccrrt_dense(n, cost, free_rows, x, y, v);
+		int i = 0;
+		while (ret > 0 && i < 2) {
+			ret = _carr_dense(n, cost, ret, free_rows, x, y, v);
+			i++;
+		}
+		if (ret > 0) {
+			ret = _ca_dense(n, cost, ret, free_rows, x, y, v);
+		}
+		FREE(v);
+		FREE(free_rows);
+		return ret;
+	}
+
+
+}

modules/ANSMOT/ByteTrackNCNN/src/NCNNutils.cpp

@@ -0,0 +1,437 @@
+#include <map>
+#include "NCNNBYTETracker.h"
+#include "NCNNlapjv.h"
+
+namespace ByteTrackNCNN {
+	std::vector<STrack*> BYTETracker::joint_stracks( std::vector<STrack*> &tlista,  std::vector<STrack> &tlistb)
+{
+    std::map<int, int> exists;
+    std::vector<STrack*> res;
+    for (int i = 0; i < tlista.size(); i++)
+    {
+        exists.insert(std::pair<int, int>(tlista[i]->track_id, 1));
+        res.push_back(tlista[i]);
+    }
+    for (int i = 0; i < tlistb.size(); i++)
+    {
+        int tid = tlistb[i].track_id;
+        if (!exists[tid] || exists.count(tid) == 0)
+        {
+            exists[tid] = 1;
+            res.push_back(&tlistb[i]);
+        }
+    }
+    return res;
+}
+
+std::vector<STrack> BYTETracker::joint_stracks( std::vector<STrack> &tlista,  std::vector<STrack> &tlistb)
+{
+    std::map<int, int> exists;
+    std::vector<STrack> res;
+    for (int i = 0; i < tlista.size(); i++)
+    {
+        exists.insert(std::pair<int, int>(tlista[i].track_id, 1));
+        res.push_back(tlista[i]);
+    }
+    for (int i = 0; i < tlistb.size(); i++)
+    {
+        int tid = tlistb[i].track_id;
+        if (!exists[tid] || exists.count(tid) == 0)
+        {
+            exists[tid] = 1;
+            res.push_back(tlistb[i]);
+        }
+    }
+    return res;
+}
+
+std::vector<STrack> BYTETracker::sub_stracks( std::vector<STrack> &tlista,  std::vector<STrack> &tlistb)
+{
+    std::map<int, STrack> stracks;
+    for (int i = 0; i < tlista.size(); i++)
+    {
+        stracks.insert(std::pair<int, STrack>(tlista[i].track_id, tlista[i]));
+    }
+    for (int i = 0; i < tlistb.size(); i++)
+    {
+        int tid = tlistb[i].track_id;
+        if (stracks.count(tid) != 0)
+        {
+            stracks.erase(tid);
+        }
+    }
+
+    std::vector<STrack> res;
+    std::map<int, STrack>::iterator  it;
+    for (it = stracks.begin(); it != stracks.end(); ++it)
+    {
+        res.push_back(it->second);
+    }
+
+    return res;
+}
+
+void BYTETracker::remove_duplicate_stracks( std::vector<STrack> &resa,  std::vector<STrack> &resb,  std::vector<STrack> &stracksa,  std::vector<STrack> &stracksb)
+{
+    std::vector< std::vector<float> > pdist = iou_distance(stracksa, stracksb);
+    std::vector<std::pair<int, int> > pairs;
+    for (int i = 0; i < pdist.size(); i++)
+    {
+        for (int j = 0; j < pdist[i].size(); j++)
+        {
+            if (pdist[i][j] < 0.15)
+            {
+                pairs.push_back(std::pair<int, int>(i, j));
+            }
+        }
+    }
+
+    std::vector<int> dupa, dupb;
+    for (int i = 0; i < pairs.size(); i++)
+    {
+        int timep = stracksa[pairs[i].first].frame_id - stracksa[pairs[i].first].start_frame;
+        int timeq = stracksb[pairs[i].second].frame_id - stracksb[pairs[i].second].start_frame;
+        if (timep > timeq)
+            dupb.push_back(pairs[i].second);
+        else
+            dupa.push_back(pairs[i].first);
+    }
+
+    for (int i = 0; i < stracksa.size(); i++)
+    {
+        std::vector<int>::iterator iter = find(dupa.begin(), dupa.end(), i);
+        if (iter == dupa.end())
+        {
+            resa.push_back(stracksa[i]);
+        }
+    }
+
+    for (int i = 0; i < stracksb.size(); i++)
+    {
+        std::vector<int>::iterator iter = find(dupb.begin(), dupb.end(), i);
+        if (iter == dupb.end())
+        {
+            resb.push_back(stracksb[i]);
+        }
+    }
+}
+
+void BYTETracker::linear_assignment( std::vector< std::vector<float> > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh,
+                                     std::vector< std::vector<int> > &matches,  std::vector<int> &unmatched_a,  std::vector<int> &unmatched_b)
+{
+    if (cost_matrix.size() == 0)
+    {
+        for (int i = 0; i < cost_matrix_size; i++)
+        {
+            unmatched_a.push_back(i);
+        }
+        for (int i = 0; i < cost_matrix_size_size; i++)
+        {
+            unmatched_b.push_back(i);
+        }
+        return;
+    }
+
+    std::vector<int> rowsol;  std::vector<int> colsol;
+    float c = lapjv(cost_matrix, rowsol, colsol, true, thresh);
+    for (int i = 0; i < rowsol.size(); i++)
+    {
+        if (rowsol[i] >= 0)
+        {
+            std::vector<int> match;
+            match.push_back(i);
+            match.push_back(rowsol[i]);
+            matches.push_back(match);
+        }
+        else
+        {
+            unmatched_a.push_back(i);
+        }
+    }
+
+    for (int i = 0; i < colsol.size(); i++)
+    {
+        if (colsol[i] < 0)
+        {
+            unmatched_b.push_back(i);
+        }
+    }
+}
+
+std::vector< std::vector<float> > BYTETracker::ious( std::vector< std::vector<float> > &atlbrs,  std::vector< std::vector<float> > &btlbrs)
+{
+    std::vector< std::vector<float> > ious;
+    if (atlbrs.size()*btlbrs.size() == 0)
+        return ious;
+
+    ious.resize(atlbrs.size());
+    for (int i = 0; i < ious.size(); i++)
+    {
+        ious[i].resize(btlbrs.size());
+    }
+
+    //bbox_ious
+    for (int k = 0; k < btlbrs.size(); k++)
+    {
+        std::vector<float> ious_tmp;
+        float box_area = (btlbrs[k][2] - btlbrs[k][0] + 1)*(btlbrs[k][3] - btlbrs[k][1] + 1);
+        for (int n = 0; n < atlbrs.size(); n++)
+        {
+            float iw = std::min(atlbrs[n][2], btlbrs[k][2]) - std::max(atlbrs[n][0], btlbrs[k][0]) + 1;
+            if (iw > 0)
+            {
+                float ih = std::min(atlbrs[n][3], btlbrs[k][3]) - std::max(atlbrs[n][1], btlbrs[k][1]) + 1;
+                if(ih > 0)
+                {
+                    float ua = (atlbrs[n][2] - atlbrs[n][0] + 1)*(atlbrs[n][3] - atlbrs[n][1] + 1) + box_area - iw * ih;
+                    ious[n][k] = iw * ih / ua;
+                }
+                else
+                {
+                    ious[n][k] = 0.0;
+                }
+            }
+            else
+            {
+                ious[n][k] = 0.0;
+            }
+        }
+    }
+
+    return ious;
+}
+
+std::vector< std::vector<float> > BYTETracker::iou_distance( std::vector<STrack*> &atracks,  std::vector<STrack> &btracks, int &dist_size, int &dist_size_size)
+{
+    std::vector< std::vector<float> > cost_matrix;
+    if (atracks.size() * btracks.size() == 0)
+    {
+        dist_size = atracks.size();
+        dist_size_size = btracks.size();
+        return cost_matrix;
+    }
+    std::vector< std::vector<float> > atlbrs, btlbrs;
+    for (int i = 0; i < atracks.size(); i++)
+    {
+        atlbrs.push_back(atracks[i]->tlbr);
+    }
+    for (int i = 0; i < btracks.size(); i++)
+    {
+        btlbrs.push_back(btracks[i].tlbr);
+    }
+
+    dist_size = atracks.size();
+    dist_size_size = btracks.size();
+
+    std::vector< std::vector<float> > _ious = ious(atlbrs, btlbrs);
+
+    for (int i = 0; i < _ious.size();i++)
+    {
+        std::vector<float> _iou;
+        for (int j = 0; j < _ious[i].size(); j++)
+        {
+            _iou.push_back(1 - _ious[i][j]);
+        }
+        cost_matrix.push_back(_iou);
+    }
+
+    return cost_matrix;
+}
+
+std::vector< std::vector<float> > BYTETracker::iou_distance( std::vector<STrack> &atracks,  std::vector<STrack> &btracks)
+{
+    std::vector< std::vector<float> > atlbrs, btlbrs;
+    for (int i = 0; i < atracks.size(); i++)
+    {
+        atlbrs.push_back(atracks[i].tlbr);
+    }
+    for (int i = 0; i < btracks.size(); i++)
+    {
+        btlbrs.push_back(btracks[i].tlbr);
+    }
+
+    std::vector< std::vector<float> > _ious = ious(atlbrs, btlbrs);
+    std::vector< std::vector<float> > cost_matrix;
+    for (int i = 0; i < _ious.size(); i++)
+    {
+        std::vector<float> _iou;
+        for (int j = 0; j < _ious[i].size(); j++)
+        {
+            _iou.push_back(1 - _ious[i][j]);
+        }
+        cost_matrix.push_back(_iou);
+    }
+
+    return cost_matrix;
+}
+
+double BYTETracker::lapjv(const  std::vector< std::vector<float> > &cost,  std::vector<int> &rowsol,  std::vector<int> &colsol,
+                          bool extend_cost, float cost_limit, bool return_cost)
+{
+    std::vector< std::vector<float> > cost_c;
+    cost_c.assign(cost.begin(), cost.end());
+
+    std::vector< std::vector<float> > cost_c_extended;
+
+    int n_rows = cost.size();
+    int n_cols = cost[0].size();
+    rowsol.resize(n_rows);
+    colsol.resize(n_cols);
+
+    int n = 0;
+    if (n_rows == n_cols)
+    {
+        n = n_rows;
+    }
+    else
+    {
+        if (!extend_cost)
+        {
+            //std::cout << "set extend_cost=True" << std::endl;
+            system("pause");
+            exit(0);
+        }
+    }
+
+    if (extend_cost || cost_limit < LONG_MAX)
+    {
+        n = n_rows + n_cols;
+        cost_c_extended.resize(n);
+        for (int i = 0; i < cost_c_extended.size(); i++)
+            cost_c_extended[i].resize(n);
+
+        if (cost_limit < LONG_MAX)
+        {
+            for (int i = 0; i < cost_c_extended.size(); i++)
+            {
+                for (int j = 0; j < cost_c_extended[i].size(); j++)
+                {
+                    cost_c_extended[i][j] = cost_limit / 2.0;
+                }
+            }
+        }
+        else
+        {
+            float cost_max = -1;
+            for (int i = 0; i < cost_c.size(); i++)
+            {
+                for (int j = 0; j < cost_c[i].size(); j++)
+                {
+                    if (cost_c[i][j] > cost_max)
+                        cost_max = cost_c[i][j];
+                }
+            }
+            for (int i = 0; i < cost_c_extended.size(); i++)
+            {
+                for (int j = 0; j < cost_c_extended[i].size(); j++)
+                {
+                    cost_c_extended[i][j] = cost_max + 1;
+                }
+            }
+        }
+
+        for (int i = n_rows; i < cost_c_extended.size(); i++)
+        {
+            for (int j = n_cols; j < cost_c_extended[i].size(); j++)
+            {
+                cost_c_extended[i][j] = 0;
+            }
+        }
+        for (int i = 0; i < n_rows; i++)
+        {
+            for (int j = 0; j < n_cols; j++)
+            {
+                cost_c_extended[i][j] = cost_c[i][j];
+            }
+        }
+
+        cost_c.clear();
+        cost_c.assign(cost_c_extended.begin(), cost_c_extended.end());
+    }
+
+    double **cost_ptr;
+    cost_ptr = new double *[n];
+    for (int i = 0; i < n; i++)
+        cost_ptr[i] = new double[n];
+
+    for (int i = 0; i < n; i++)
+    {
+        for (int j = 0; j < n; j++)
+        {
+            cost_ptr[i][j] = cost_c[i][j];
+        }
+    }
+
+    int* x_c = new int[n];
+    int *y_c = new int[n];
+
+    int ret = lapjv_internal(n, cost_ptr, x_c, y_c);
+    if (ret != 0)
+    {
+        for (int i = 0; i < n; i++)
+            delete[] cost_ptr[i];
+        delete[] cost_ptr;
+        delete[] x_c;
+        delete[] y_c;
+        return -1;
+    }
+
+    double opt = 0.0;
+
+    if (n != n_rows)
+    {
+        for (int i = 0; i < n; i++)
+        {
+            if (x_c[i] >= n_cols)
+                x_c[i] = -1;
+            if (y_c[i] >= n_rows)
+                y_c[i] = -1;
+        }
+        for (int i = 0; i < n_rows; i++)
+        {
+            rowsol[i] = x_c[i];
+        }
+        for (int i = 0; i < n_cols; i++)
+        {
+            colsol[i] = y_c[i];
+        }
+
+        if (return_cost)
+        {
+            for (int i = 0; i < rowsol.size(); i++)
+            {
+                if (rowsol[i] != -1)
+                {
+                    //cout << i << "\t" << rowsol[i] << "\t" << cost_ptr[i][rowsol[i]] << endl;
+                    opt += cost_ptr[i][rowsol[i]];
+                }
+            }
+        }
+    }
+    else if (return_cost)
+    {
+        for (int i = 0; i < rowsol.size(); i++)
+        {
+            opt += cost_ptr[i][rowsol[i]];
+        }
+    }
+
+    for (int i = 0; i < n; i++)
+    {
+        delete[]cost_ptr[i];
+    }
+    delete[]cost_ptr;
+    delete[]x_c;
+    delete[]y_c;
+
+    return opt;
+}
+
+//cv::Scalar BYTETracker::get_color(int idx)
+//{
+//    idx += 3;
+//    return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255);
+//}
+
+	
+}