Refactor project structure

modules/ANSMOT/UCMCTracker.h

@@ -0,0 +1,127 @@
+
+#ifndef UCMCTRACK_H
+#define UCMCTRACK_H
+#define UCMCTRACK_API __declspec(dllexport)
+#pragma once
+#include <opencv2/opencv.hpp>
+#include <Eigen/Eigen>
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+#include <chrono>
+#include "UCMClapjv.h"
+#include "UCMCKalman.h"
+namespace UCMC {
+    const int color_list[][3] = {
+        {255, 56, 56},
+        {255, 157, 151},
+        {255, 112, 31},
+        {255, 178, 29},
+        {207, 210, 49},
+        {72, 249, 10},
+        {146, 204, 23},
+        {61, 219, 134},
+        {26, 147, 52},
+        {0, 212, 187},
+        {44, 153, 168},
+        {0, 194, 255},
+        {52, 69, 147},
+        {100, 115, 255},
+        {0, 24, 236},
+        {132, 56, 255},
+        {82, 0, 133},
+        {203, 56, 255},
+        {255, 149, 200},
+        {255, 55, 198}
+    };
+    struct Params {
+        double a1 = 100.;
+        double a2 = 100.;
+        double wx = 5.;
+        double wy = 5.;
+        double vmax = 10.;
+        double max_age = 10.;
+        double high_score = 0.5;
+        double conf_threshold = 0.01;
+        double dt = 0.033;
+        std::string dataset = "MOT";
+        std::vector<double> Ki;
+        std::vector<double> Ko;
+    };
+    struct Object
+    {
+        cv::Rect_<float> rect;
+        int label;
+        float prob;
+        std::string object_id;
+    };
+    struct Obj {
+        int id = 0;
+        int track_idx = 0;
+        Object obj;
+        Eigen::Matrix<double, 2, 1> y;
+        Eigen::Matrix2d R;
+    };
+
+    class UCMCTRACK_API Mapper {
+        Eigen::Matrix3d A, InvA;
+        Eigen::MatrixXd KiKo;
+    public:
+        bool debug = false;
+        Mapper() {};
+        Mapper(std::vector<double>& _Ki, std::vector<double>& _Ko);
+        [[nodiscard]] std::vector<double> uvError(cv::Rect box);
+        void uv2xy(Eigen::MatrixXd uv, Eigen::MatrixXd sigma_uv,
+            Eigen::Matrix<double, 2, 1>& xy, Eigen::Matrix2d& sigma_xy);        
+        void map_to(cv::Rect box, Eigen::Matrix<double, 2, 1>& y, Eigen::Matrix2d& R);
+    };
+    class UCMCTRACK_API Tracker
+    {
+    private:
+        Params params;
+        std::vector<UCMCKalman::Tracker> trackers;
+        int frame_idx = 0;
+        int tracker_count = 0;
+        std::vector<int> confirmed_idx, coasted_idx, tentative_idx, detidx_remain;
+
+        // Frame rate auto-estimation
+        bool auto_frame_rate_ = true;
+        float estimated_fps_ = 30.0f;
+        float time_scale_factor_ = 1.0f;
+        size_t fps_sample_count_ = 0;
+        std::chrono::steady_clock::time_point last_update_time_;
+        bool has_last_update_time_ = false;
+
+        void estimateFrameRate();
+
+    public:
+        Mapper mapper;
+        bool debug = false;
+        Tracker(Params& params);
+        Tracker() {};
+        void update_parameters(double a1, double a2,
+            double wx, double wy,
+            double vmax, double max_age,
+            double high_score, double conf_threshold,
+            double dt, const std::vector<double>& Ki,
+            const std::vector<double>& Ko,
+            bool autoFrameRate = true);
+        [[nodiscard]] float getEstimatedFps() const;
+        [[nodiscard]] std::vector<Obj> update(std::vector<Object>& det_results);
+        void update(std::vector<Obj>& dets, int frame_id);
+        void data_association(std::vector<Obj>& dets, int frame_id);
+        void associate_tentative(std::vector<Obj>& dets);
+        void initial_tentative(std::vector<Obj>& dets);
+        void delete_old_trackers();
+        void update_status(std::vector<Obj>& dets);
+        ~Tracker();
+    };
+
+    static cv::Scalar get_color(int index);
+    static cv::Mat draw_boxes(cv::Mat image,
+        std::vector<UCMC::Obj>& objects,
+        std::vector<std::string>& names,
+        int draw_size = 20,
+        bool draw_label = true);
+}
+#endif