Refactor project structure

modules/ANSMOT/OCSort/include/Association.h

@@ -0,0 +1,18 @@
+#ifndef ASSOCIATION_H
+#define ASSOCIATION_H
+
+#include <Eigen/Dense>
+#include <algorithm>
+#include <vector>
+#include "OCSortlapjv.h"
+#define m_pi 3.1415926
+
+namespace ANSOCSort {
+    std::tuple<Eigen::MatrixXf, Eigen::MatrixXf> speed_direction_batch(const Eigen::MatrixXf& dets,
+        const Eigen::MatrixXf& tracks);
+    Eigen::MatrixXf iou_batch(const Eigen::MatrixXf& bboxes1, const Eigen::MatrixXf& bboxes2);
+    Eigen::MatrixXf giou_batch(const Eigen::MatrixXf& bboxes1, const Eigen::MatrixXf& bboxes2);
+    std::tuple<std::vector<Eigen::Matrix<int, 1, 2>>, std::vector<int>, std::vector<int>> associate(Eigen::MatrixXf detections, Eigen::MatrixXf trackers, float iou_threshold, Eigen::MatrixXf velocities, Eigen::MatrixXf previous_obs_, float vdc_weight);
+}
+
+#endif

modules/ANSMOT/OCSort/include/KalmanBoxTracker.h

@@ -0,0 +1,54 @@
+#ifndef KALMANBOXTRACKER_H
+#define KALMANBOXTRACKER_H
+#include "OCSortKalmanFilter.h"
+#include "OCSortUtilities.h"
+#include "iostream"
+/*
+This class represents the internal state of individual
+tracked objects observed as bbox.
+*/
+namespace ANSOCSort {
+    class KalmanBoxTracker {
+    public:
+        /*method*/
+        KalmanBoxTracker() : kf(nullptr) {};
+        KalmanBoxTracker(Eigen::VectorXf bbox_,int delta_t_ = 3);
+        ~KalmanBoxTracker();
+
+        // Deep copy (raw pointer ownership)
+        KalmanBoxTracker(const KalmanBoxTracker& other);
+        KalmanBoxTracker& operator=(const KalmanBoxTracker& other);
+
+        // Move semantics
+        KalmanBoxTracker(KalmanBoxTracker&& other) noexcept;
+        KalmanBoxTracker& operator=(KalmanBoxTracker&& other) noexcept;
+
+        void update(Eigen::Matrix<float, 9, 1>* bbox_);
+        Eigen::RowVectorXf predict();
+        Eigen::VectorXf get_state();
+
+    public:
+        /*variable*/
+        static int count;
+        Eigen::VectorXf bbox;// [9,1]
+        KalmanFilterNew* kf;
+        int time_since_update;
+        int id;
+        std::vector<Eigen::VectorXf> history;
+        int hits;
+        int hit_streak;
+        int age = 0;        
+        Eigen::RowVectorXf last_observation = Eigen::RowVectorXf::Zero(9);
+        std::unordered_map<int, Eigen::VectorXf> observations;
+        std::vector<Eigen::VectorXf> history_observations;
+        Eigen::RowVectorXf velocity = Eigen::RowVectorXf::Zero(2);// [2,1]
+        int delta_t;
+        double GetScore() { return bbox[4]; }
+        int GetClassId() { return (int)bbox[5]; }
+        int GetDGId() { return (int)bbox[6]; }
+        int GetCamId() { return (int)bbox[7]; }
+        int GetModelId() { return (int)bbox[8]; }
+    };
+}
+
+#endif

modules/ANSMOT/OCSort/include/OCSort.h

@@ -0,0 +1,47 @@
+#ifndef OCSORT_H
+#define OCSORT_H
+#define OCSORT_API __declspec(dllexport)
+
+#include "KalmanBoxTracker.h"
+#include "Association.h"
+#include "OCSortlapjv.h"
+#include <functional>
+#include <unordered_map>
+#include <chrono>
+#include "OCSortObject.h"
+
+namespace ANSOCSort {
+    class OCSORT_API OCSort {
+    public:
+        OCSort();
+        void update_parameters(float det_thresh_, int max_age_ = 30, int min_hits_ = 3, float iou_threshold_ = 0.3,
+            int delta_t_ = 3, std::string asso_func_ = "iou", float inertia_ = 0.2, bool use_byte_ = false,
+            bool autoFrameRate = true);
+        float getEstimatedFps() const;
+        std::vector<Eigen::RowVectorXf> update(Eigen::MatrixXf dets);
+    public:
+        float det_thresh;
+        int max_age;
+        int min_hits;
+        float iou_threshold;
+        int delta_t;
+        std::function<Eigen::MatrixXf(const Eigen::MatrixXf&, const Eigen::MatrixXf&)> asso_func;
+        float inertia;
+        bool use_byte;
+        std::vector<KalmanBoxTracker> trackers;
+        int frame_count;
+
+    private:
+        // 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_;
+
+        void estimateFrameRate();
+    };
+
+}
+#endif

modules/ANSMOT/OCSort/include/OCSortKalmanFilter.h

@@ -0,0 +1,99 @@
+#ifndef OCSORTKALMANFILTER_H
+#define OCSORTKALMANFILTER_H
+#include <Eigen/Dense>
+#include <any>
+#include <map>
+#include <vector>
+namespace ANSOCSort {
+    class KalmanFilterNew {
+    public:
+        KalmanFilterNew();
+        KalmanFilterNew(int dim_x_, int dim_z_);
+        void predict();
+        void update(Eigen::VectorXf* z_);
+        void freeze();
+        void unfreeze();
+        KalmanFilterNew& operator=(const KalmanFilterNew&) = default;
+
+    public:
+        int dim_z = 4;
+        int dim_x = 7;
+        int dim_u = 0;
+        /// state: This is the Kalman state variable [7,1].
+        Eigen::VectorXf x;
+        // P: Covariance matrix. Initially declared as an identity matrix. Data type is float. [7,7].
+        Eigen::MatrixXf P;
+        // Q: Process noise covariance matrix. [7,7].
+        Eigen::MatrixXf Q;
+        // B: Control matrix. Not used in target tracking. [n,n].
+        Eigen::MatrixXf B;
+        // F: Prediction matrix / state transition matrix. [7,7].
+        Eigen::Matrix<float, 7, 7> F;
+        // H: Observation model / matrix. [4,7].
+        Eigen::Matrix<float, 4, 7> H;
+        // R: Observation noise covariance matrix. [4,4].
+        Eigen::Matrix<float, 4, 4> R;
+        // _alpha_sq: Fading memory control, controlling the update weight. Float.
+        float _alpha_sq = 1.0;
+        // M: Measurement matrix, converting state vector x to measurement vector z. [7,4]. It has the opposite effect of matrix H.
+        Eigen::MatrixXf M;
+        // z: Measurement vector. [4,1].
+        Eigen::VectorXf z;
+        /* The following variables are intermediate variables used in calculations */
+        // K: Kalman gain. [7,4].
+        Eigen::MatrixXf K;
+        // y: Measurement residual. [4,1].
+        Eigen::MatrixXf y;
+        // S: Measurement residual covariance.
+        Eigen::MatrixXf S;
+        // SI: Transpose of measurement residual covariance (simplified for subsequent calculations).
+        Eigen::MatrixXf SI;
+        // Identity matrix of size [dim_x,dim_x], used for convenient calculations. This cannot be changed.
+        const Eigen::MatrixXf I = Eigen::MatrixXf::Identity(dim_x, dim_x);
+        // There will always be a copy of x, P after predict() is called.
+        // If there is a need to assign values between two Eigen matrices, the precondition is that they should be initialized properly, as this ensures that the number of columns and rows are compatible.
+        Eigen::VectorXf x_prior;
+        Eigen::MatrixXf P_prior;
+        // there will always be a copy of x,P after update() is called
+        Eigen::VectorXf x_post;
+        Eigen::MatrixXf P_post;
+        // keeps all observations. When there is a 'z', it is directly pushed back.
+        std::vector<Eigen::VectorXf*> history_obs;
+        // The following is newly added by ocsort.
+        // Used to mark the tracking state (whether there is still a target matching this trajectory), default value is false.
+        bool observed = false;
+        std::vector<Eigen::VectorXf*> new_history; // Used to create a virtual trajectory.
+
+        /* todo: Let's change the way we store variables, as C++ does not have Python's self.dict.
+        Using map<string,any> incurs high memory overhead, and there are errors when assigning values to Eigen data.
+        Someone in the group suggested using metadata to achieve this, but I don't know how to do it.
+        Therefore, here we will use a structure to save variables.
+        */
+        struct Data {
+            Eigen::VectorXf x;
+            Eigen::MatrixXf P;
+            Eigen::MatrixXf Q;
+            Eigen::MatrixXf B;
+            Eigen::MatrixXf F;
+            Eigen::MatrixXf H;
+            Eigen::MatrixXf R;
+            float _alpha_sq = 1.;
+            Eigen::MatrixXf M;
+            Eigen::VectorXf z;
+            Eigen::MatrixXf K;
+            Eigen::MatrixXf y;
+            Eigen::MatrixXf S;
+            Eigen::MatrixXf SI;
+            Eigen::VectorXf x_prior;
+            Eigen::MatrixXf P_prior;
+            Eigen::VectorXf x_post;
+            Eigen::MatrixXf P_post;
+            std::vector<Eigen::VectorXf*> history_obs;
+            bool observed = false;
+            // The following is to determine whether the data has been saved due to freezing.
+            bool IsInitialized = false;
+        };
+        struct Data attr_saved;
+    };
+}
+#endif

modules/ANSMOT/OCSort/include/OCSortObject.h

@@ -0,0 +1,24 @@
+#pragma once
+#include "OCSortRect.h"
+namespace ANSOCSort
+{
+    struct Object
+    {
+        Rect<float> rect;
+        int label;
+        float prob;
+        float left;
+        float top;
+        float right;
+        float bottom;
+        std::string object_id;
+
+        Object(const 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);
+    };
+}

modules/ANSMOT/OCSort/include/OCSortRect.h

@@ -0,0 +1,51 @@
+#pragma once
+#include "Eigen/Dense"
+namespace ANSOCSort
+{
+    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/OCSort/include/OCSortUtilities.h

@@ -0,0 +1,18 @@
+#ifndef OCSORTUTILITIES_H
+#define OCSORTUTILITIES_H
+#include "Eigen/Dense"
+#include <unordered_map>
+namespace ANSOCSort {
+    /**
+     * Takes a bounding box in the form [x1,y1,x2,y2] and returns z in the form
+    [x,y,s,r] where x,y is the centre of the box and s is the scale/area and r is
+    the aspect ratio
+     * @param bbox
+     * @return z
+     */
+    Eigen::VectorXf convert_bbox_to_z(Eigen::VectorXf bbox);
+    Eigen::VectorXf speed_direction(Eigen::VectorXf bbox1, Eigen::VectorXf bbox2);
+    Eigen::VectorXf convert_x_to_bbox(Eigen::VectorXf x);
+    Eigen::VectorXf k_previous_obs(std::unordered_map<int, Eigen::VectorXf> observations_, int cur_age, int k);
+}
+#endif

modules/ANSMOT/OCSort/include/OCSortlapjv.h

@@ -0,0 +1,82 @@
+#ifndef OCSORTLAPJV_H
+#define OCSORTLAPJV_H
+#include "vector"
+#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
+
+namespace ANSOCSort {
+    typedef signed int int_t;
+    typedef unsigned int uint_t;
+    typedef float 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);
+
+    extern int_t lapmod_internal(
+        const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        int_t* x, int_t* y, fp_t fp_version);
+    float execLapjv(const std::vector<std::vector<float>>& cost, std::vector<int>& rowsol,
+        std::vector<int>& colsol, bool extend_cost, float cost_limit, bool return_cost);
+}
+
+#endif