Refactor project structure

modules/ANSMOT/UCMCKalman.h

@@ -0,0 +1,60 @@
+#ifndef UCMCKALMAN_H
+#define UCMCKALMAN_H
+#pragma once
+#include <Eigen/Eigen>
+#include <Eigen/Dense>
+#include <unordered_map>
+namespace UCMCKalman {
+    enum struct TrackStatus { Tentative, Confirmed, Coasted, Deleted };
+    
+    void lapjv(const Eigen::MatrixXd& costMatrix, double costLimit,
+        std::vector<int>& x, std::vector<int>& y);
+    void linearAssignment(const Eigen::MatrixXd& costMatrix, double thresh,
+        std::vector<std::vector<int>>& matches,
+        std::vector<int>& unmatched_a,
+        std::vector<int>& unmatched_b);
+
+    class Filter {
+    public:
+        Filter(int dim_x, int dim_z, int dim_u = 0);
+        void predict();
+        void update(const Eigen::VectorXd& z,const Eigen::MatrixXd& R);
+        [[nodiscard]] Eigen::VectorXd getState() const;
+        [[nodiscard]] Eigen::MatrixXd getCovariance() const;
+        
+        // private:
+        int dim_x, dim_z, dim_u;
+        Eigen::VectorXd x, x_prior, x_post;
+        Eigen::MatrixXd F, P, Q, H, R, M, z, K, y, S, SI, _I, P_prior, P_post;
+        double _alpha_sq;
+    };
+    class Tracker {
+    public:
+        Tracker(Eigen::Vector2d y, Eigen::Matrix2d R, double wx, double wy, double vmax,
+            double w, double h, double dt = 1.0 / 30.0, int tracker_count = 0);
+        void update(const Eigen::Vector2d& y, Eigen::Matrix2d& R);
+        [[nodiscard]] Eigen::Vector2d predict();
+        [[nodiscard]] Eigen::Vector4d getState() const;
+        [[nodiscard]] double distance(const Eigen::Vector2d& y, Eigen::Matrix2d& R, bool show) const;
+
+        int id;
+        int age;
+        int death_count;
+        int birth_count;
+        int detidx;
+        double w;
+        double h;
+        TrackStatus status;
+        int class_id_;
+        void voteClassId(int new_class_id, float score);
+
+    private:
+        Filter kf;
+        std::unordered_map<int, float> class_id_scores_;
+        int detection_count_;
+        bool class_id_locked_;
+        static const int CLASS_ID_LOCK_FRAMES = 10;
+    };
+}
+
+#endif