Refactor project structure

modules/ANSMOT/ByteTrackEigen/include/EigenBYTETracker.h

@@ -0,0 +1,114 @@
+#pragma once
+#include <chrono>
+#include <stdexcept>
+#include <stdlib.h>
+#include <memory>
+#include <vector>
+#include <sstream>
+#include "EigenKalmanFilter.h"
+#include "EigenKalmanBBoxTrack.h"
+#include "EigenBaseTrack.h"
+#include "EigenBoundingBoxIoUMatching.h"
+#include "EigenLinearAssignment.h"
+#include "EigenBoundingBoxTrackUtils.h"
+
+namespace ByteTrackEigen {
+    /**
+     * @brief BYTETracker class for tracking objects in video frames using Kalman Filters.
+     *
+     * This class implements the BYTETrack algorithm, which combines the use of Kalman Filters
+     * and Hungarian algorithm for object tracking in video streams.
+     *
+     * Key functionalities include processing frame detections, updating track states, and
+     * maintaining lists of active, lost, and removed tracks.
+     */
+    class BYTETracker {
+    public:
+        /**
+         * @brief Constructor for BYTETracker.
+         *
+         * Initializes the tracker with specific thresholds and parameters for tracking.
+         *
+         * @param track_thresh Threshold for track confidence.
+         * @param track_buffer Size of the buffer to store track history.
+         * @param match_thresh Threshold for matching detections to tracks.
+         * @param frame_rate Frame rate of the video being processed.
+         */
+        BYTETracker(float track_thresh = 0.25, int track_buffer = 30, float match_thresh = 0.8, int frame_rate = 30);
+        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;
+        std::vector<KalmanBBoxTrack> update(const Eigen::MatrixXf& output_results, const std::vector<std::string> obj_ids);
+
+    private:
+        // Internal constants and thresholds
+        const float BASE_FRAME_RATE = 30.0;
+        const float MIN_KEEP_THRESH = 0.1f;
+        const float LOWER_CONFIDENCE_MATCHING_THRESHOLD = 0.5;
+        const float ACTIVATION_MATCHING_THRESHOLD = 0.7;
+
+        void estimateFrameRate();
+
+        // Member variables for tracking settings and state
+        float track_thresh; // Threshold for determining track validity.
+        float match_thresh; // Threshold for matching detections to existing tracks.
+        int frame_id;       // Current frame identifier.
+        float det_thresh;   // Detection threshold for filtering weak detections.
+        int buffer_size;    // Size of the history buffer for tracks.
+        int max_time_lost;  // Maximum time a track can be lost before removal.
+        int track_buffer_;  // Stored track buffer for recalculating max_time_lost.
+
+        // 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_;
+
+        // Kalman filter and track lists
+        KalmanFilter kalman_filter; // Kalman filter for state estimation.
+        std::vector<std::shared_ptr<KalmanBBoxTrack>> tracked_tracks; // Active tracks.
+        std::vector<std::shared_ptr<KalmanBBoxTrack>> lost_tracks;    // Tracks that are currently lost.
+        std::vector<std::shared_ptr<KalmanBBoxTrack>> removed_tracks; // Tracks that are removed.
+
+        LinearAssignment linear_assignment; // For solving assignment problems.
+
+        // Internal methods for processing detections and tracks
+        std::vector<KalmanBBoxTrack>extract_kalman_bbox_tracks(const Eigen::MatrixXf dets, const Eigen::VectorXf scores_keep, const Eigen::VectorXf clss_ids, const std::vector<std::string> obj_ids );
+        Eigen::MatrixXf select_matrix_rows_by_indices(const Eigen::MatrixXf matrix, const std::vector<int> indices);
+        std::vector<std::string> select_vector_by_indices(const std::vector<std::string> obj_ids, const std::vector<int> indices);
+
+        std::pair<std::vector<KalmanBBoxTrack>, std::vector<KalmanBBoxTrack>> filter_and_partition_detections(const Eigen::MatrixXf& output_results, const std::vector<std::string> obj_ids);
+        std::pair<std::vector<std::shared_ptr<KalmanBBoxTrack>>, std::vector<std::shared_ptr<KalmanBBoxTrack>>> partition_tracks_by_activation();
+        std::tuple<std::vector<std::pair<int, int>>, std::set<int>, std::set<int>> assign_tracks_to_detections(
+            const std::vector<std::shared_ptr<KalmanBBoxTrack>> tracks,
+            const std::vector<KalmanBBoxTrack> detections,
+            double thresh
+        );
+        void update_tracks_from_detections(
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& tracks,
+            const std::vector<KalmanBBoxTrack> detections,
+            const std::vector<std::pair<int, int>> track_detection_pair_indices,
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& reacquired_tracked_tracks,
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& activated_tracks
+        );
+        std::vector<std::shared_ptr<KalmanBBoxTrack>> extract_active_tracks(
+            const std::vector<std::shared_ptr<KalmanBBoxTrack>>& tracks,
+            std::set<int> unpaired_track_indices
+        );
+        void flag_unpaired_tracks_as_lost(
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& currently_tracked_tracks,
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& lost_tracks,
+            std::set<int> unpaired_track_indices
+        );
+        void prune_and_merge_tracked_tracks(
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& reacquired_tracked_tracks,
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& activated_tracks
+        );
+        void handle_lost_and_removed_tracks(
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& removed_tracks,
+            std::vector<std::shared_ptr<KalmanBBoxTrack>>& lost_tracks
+        );
+    };
+
+}

modules/ANSMOT/ByteTrackEigen/include/EigenBaseTrack.h

@@ -0,0 +1,90 @@
+#pragma once
+#include <iostream>
+#include <map>
+#include <vector>
+#include <stdexcept>
+#include <limits>
+
+namespace ByteTrackEigen {
+    // Enum class for defining the state of a track.
+    enum class TrackState {
+        New,      // Indicates a newly created track.
+        Tracked,  // Indicates a track that is currently being tracked.
+        Lost,     // Indicates a track that has been lost.
+        Removed   // Indicates a track that has been removed.
+    };
+
+    /**
+     * @brief The BaseTrack class serves as the foundational class for object tracking.
+     *        It defines the essential attributes and methods for a track, such as ID, state, score, features, and history.
+     *        The class provides mechanisms to handle the state of a track, including creating, updating, and maintaining its lifecycle.
+     *        Designed to be extended, it allows for specific tracking functionalities to be implemented in derived classes.
+     */
+    class BaseTrack {
+    private:
+        static int _count; // Static counter used to assign unique IDs to each track.
+
+    protected:
+        int track_id; // Unique identifier for the track.
+        bool is_activated; // Flag to indicate if the track is active.
+        TrackState state; // Current state of the track.
+        std::map<int, int> history; // History of track updates, typically storing frame ID and associated data.
+        std::vector<int> features; // Features associated with the track.
+        int curr_feature; // Current feature of the track.
+        float score; // Score associated with the track, indicating the confidence of tracking.
+        int start_frame; // Frame at which the track was started.
+        int frame_id; // Current frame ID of the track.
+        int time_since_update; // Time elapsed since the last update.
+        std::pair<double, double> location; // Current location of the track.
+
+    public:
+        // Default constructor.
+        BaseTrack();
+
+        // Constructor with score initialization.
+        BaseTrack(float score);
+
+        // Returns the end frame of the track.
+        int end_frame() const;
+
+        // Generates the next unique track ID.
+        static int next_id();
+
+        // Activates the track. This method is intended to be overridden in derived classes.
+        virtual void activate();
+
+        // Predicts the next state of the track. This method is intended to be overridden in derived classes.
+        virtual void predict();
+
+        // Updates the track with new data. This method is intended to be overridden in derived classes.
+        virtual void update();
+
+        // Marks the track as lost.
+        void mark_lost();
+
+        // Marks the track as removed.
+        void mark_removed();
+
+        // Resets the static track ID counter. Useful for unit testing or reinitializing the tracking system.
+        static void reset_count();
+
+        // Getter for is_activated.
+        bool get_is_activated() const;
+
+        // Getter for state.
+        TrackState get_state() const;
+
+        // Getter for score.
+        float get_score() const;
+
+        // Getter for start_frame.
+        int get_start_frame() const;
+
+        // Getter for frame_id.
+        int get_frame_id() const;
+
+        // Getter for track_id.
+        int get_track_id() const;
+    };
+
+}

modules/ANSMOT/ByteTrackEigen/include/EigenBoundingBoxIoUMatching.h

@@ -0,0 +1,50 @@
+#pragma once
+#include <iostream>
+#include <vector>
+#include <Eigen/Dense>
+#include "EigenKalmanBBoxTrack.h"
+#include "EigenExport.h"
+namespace ByteTrackEigen {
+    /**
+ * Calculates the Intersection over Union (IoU) for pairs of bounding boxes.
+ *
+ * This function computes the IoU for each pair of boxes in two sets. The IoU is a measure of the
+ * overlap between two bounding boxes.
+ *
+ * @param track_boxes Matrix of tracking boxes (N, 4) where N is the number of track boxes.
+ * @param detection_boxes Matrix of detection boxes (M, 4) where M is the number of detection boxes.
+ * @return Matrix of IoU values of size (N, M).
+ */
+    Eigen::MatrixXd box_iou_batch(const Eigen::MatrixXd& track_boxes, const Eigen::MatrixXd& detection_boxes);
+
+    /**
+     * Computes the IoU-based distance matrix for tracking purposes.
+     *
+     * This function converts the tracking and detection data into bounding box format and then
+     * calculates the IoU matrix. The IoU matrix is then transformed into a cost matrix for tracking.
+     *
+     * @param track_list_a Vector of KalmanBBoxTrack, representing the first set of tracks.
+     * @param track_list_b Vector of KalmanBBoxTrack, representing the second set of tracks.
+     * @return A matrix representing the cost of matching tracks in track_list_a to track_list_b.
+     */
+    Eigen::MatrixXd iou_distance(
+        const std::vector<KalmanBBoxTrack>& track_list_a,
+        const std::vector<KalmanBBoxTrack>& track_list_b
+    );
+
+    /**
+     * Matches detections to tracks based on the highest IoU.
+     *
+     * This function calculates the IoU for each detection-track pair and assigns detections to tracks
+     * based on the highest IoU value. It updates the track IDs accordingly.
+     *
+     * @param tlbr_boxes Matrix of bounding boxes for detections (N, 4).
+     * @param tracks Vector of KalmanBBoxTrack representing the current tracks.
+     * @return Vector of updated track IDs after matching.
+     */
+    std::vector<int> match_detections_with_tracks(
+        const Eigen::MatrixXd& tlbr_boxes,
+        const std::vector<KalmanBBoxTrack>& tracks
+    );
+
+}

modules/ANSMOT/ByteTrackEigen/include/EigenBoundingBoxTrackUtils.h

@@ -0,0 +1,54 @@
+#pragma once
+#include <vector>
+#include <map>
+#include <unordered_set>
+#include <Eigen/Dense>
+#include "EigenKalmanBBoxTrack.h"
+#include "EigenBoundingBoxIoUMatching.h"
+
+namespace ByteTrackEigen {
+    /**
+     * @brief Joins two lists of KalmanBBoxTrack pointers, ensuring unique track IDs.
+     *
+     * This function combines track lists A and B. If a track ID is present in both lists,
+     * the track from list B is used in the final list. This ensures that each track ID is
+     * unique in the resulting list.
+     *
+     * @param track_list_a First list of shared pointers to KalmanBBoxTrack objects.
+     * @param track_list_b Second list of shared pointers to KalmanBBoxTrack objects.
+     * @return std::vector<std::shared_ptr<KalmanBBoxTrack>> A list with unique tracks based on track IDs.
+     */
+    std::vector<std::shared_ptr<KalmanBBoxTrack>> join_tracks(
+        const std::vector<std::shared_ptr<KalmanBBoxTrack>> track_list_a,
+        const std::vector<std::shared_ptr<KalmanBBoxTrack>> track_list_b);
+
+    /**
+     * @brief Subtracts tracks in track_list_b from track_list_a based on track IDs.
+     *
+     * This function creates a new list of tracks from track_list_a excluding those
+     * tracks whose IDs are found in track_list_b, effectively performing a set subtraction.
+     *
+     * @param track_list_a First list of shared pointers to KalmanBBoxTrack objects.
+     * @param track_list_b Second list of shared pointers to KalmanBBoxTrack objects.
+     * @return std::vector<std::shared_ptr<KalmanBBoxTrack>> A list of tracks present in track_list_a but not in track_list_b.
+     */
+    std::vector<std::shared_ptr<KalmanBBoxTrack>> sub_tracks(
+        const std::vector<std::shared_ptr<KalmanBBoxTrack>>& track_list_a,
+        const std::vector<std::shared_ptr<KalmanBBoxTrack>>& track_list_b);
+
+    /**
+     * @brief Removes duplicate tracks from two lists based on IOU distance and track age.
+     *
+     * This function identifies and removes duplicate tracks between two lists based on the
+     * Intersection Over Union (IOU) distance. If two tracks have an IOU distance less than
+     * a threshold (0.15), the older track is retained.
+     *
+     * @param track_list_a First list of shared pointers to KalmanBBoxTrack objects.
+     * @param track_list_b Second list of shared pointers to KalmanBBoxTrack objects.
+     * @return Pair of vectors each containing unique tracks after removing duplicates.
+     */
+    std::pair<std::vector<std::shared_ptr<KalmanBBoxTrack>>, std::vector<std::shared_ptr<KalmanBBoxTrack>>> remove_duplicate_tracks(
+        const std::vector<std::shared_ptr<KalmanBBoxTrack>> track_list_a,
+        const std::vector<std::shared_ptr<KalmanBBoxTrack>> track_list_b);
+
+}

modules/ANSMOT/ByteTrackEigen/include/EigenExport.h

@@ -0,0 +1,13 @@
+#ifdef BUILDING_BYTE_TRACK_EIGEN
+#ifdef _WIN32
+#define BYTE_TRACK_EIGEN_API __declspec(dllexport)
+#else
+#define BYTE_TRACK_EIGEN_API __attribute__((visibility("default")))
+#endif
+#else
+#ifdef _WIN32
+#define BYTE_TRACK_EIGEN_API __declspec(dllimport)
+#else
+#define BYTE_TRACK_EIGEN_API
+#endif
+#endif

modules/ANSMOT/ByteTrackEigen/include/EigenHungarianAlgorithm.h

@@ -0,0 +1,80 @@
+/**
+ * @class HungarianAlgorithmEigen
+ *
+ * @brief Implements the Hungarian Algorithm using the Eigen library for solving the assignment problem.
+ *
+ * This class is designed to solve the assignment problem, which is to find the minimum cost way of assigning tasks to agents.
+ * It makes use of the Eigen library for matrix operations, providing an efficient implementation suitable for large scale problems.
+ *
+ * Usage:
+ * Eigen::MatrixXd cost_matrix = ...; // Define the cost matrix
+ * Eigen::VectorXi assignment;
+ * HungarianAlgorithmEigen solver;
+ * double cost = solver.SolveAssignmentProblem(cost_matrix, assignment);
+ *
+ * Note:
+ * The algorithm assumes that the input cost matrix contains only non-negative values. It is not thread-safe and is designed for single-threaded environments.
+ */
+
+#pragma once
+#include <iostream>
+#include <Eigen/Dense>
+#include <cfloat>
+
+namespace ByteTrackEigen {
+    // The HungarianAlgorithmEigen class encapsulates the Hungarian Algorithm
+// for solving the assignment problem, which finds the minimum cost matching
+// between elements of two sets. It uses the Eigen library to handle matrix operations.
+    class HungarianAlgorithmEigen
+    {
+    public:
+        // Constructor and destructor
+        HungarianAlgorithmEigen();
+        ~HungarianAlgorithmEigen();
+
+        // Solves the assignment problem given a distance matrix and returns the total cost.
+        // The assignment of rows to columns is returned in the 'Assignment' vector.
+        double solve_assignment_problem(Eigen::MatrixXd& dist_matrix, Eigen::VectorXi& assignment);
+
+    private:
+        // Constants used as special values indicating not found or default assignments.
+        const int NOT_FOUND_VALUE = -1;
+        const int DEFAULT_ASSIGNMENT_VALUE = -1;
+
+        // The distance matrix representing the cost of assignment between rows and columns.
+        Eigen::MatrixXd dist_matrix;
+
+        // Matrices and vectors used in the Hungarian algorithm
+        Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> star_matrix, new_star_matrix, prime_matrix;
+        Eigen::Array<bool, Eigen::Dynamic, 1> covered_columns, covered_rows;
+
+        // Finds a starred zero in a column, if any.
+        Eigen::Index find_star_in_column(int col);
+
+        // Finds a primed zero in a row, if any.
+        Eigen::Index find_prime_in_row(int row);
+
+        // Updates the star and prime matrices given a row and column.
+        void update_star_and_prime_matrices(int row, int col);
+
+        // Reduces the matrix by subtracting the minimum value from each uncovered row
+        // and adding it to each covered column, thus creating at least one new zero.
+        void reduce_matrix_by_minimum_value();
+
+        // Covers columns without a starred zero and potentially modifies star/prime matrices.
+        void cover_columns_lacking_stars();
+
+        // Executes the steps of the Hungarian algorithm.
+        void execute_hungarian_algorithm();
+
+        // Initializes helper arrays used by the algorithm.
+        void init_helper_arrays(int num_rows, int num_columns);
+
+        // Constructs the assignment vector from the star matrix.
+        void construct_assignment_vector(Eigen::VectorXi& assignment);
+
+        // Calculates the total cost of the assignment.
+        double calculate_total_cost(Eigen::VectorXi& assignment);
+    };
+
+}

modules/ANSMOT/ByteTrackEigen/include/EigenKalmanBBoxTrack.h

@@ -0,0 +1,132 @@
+#pragma once
+
+#include <iostream>
+#include <vector>
+#include <Eigen/Dense>
+#include <memory>
+#include <unordered_map>
+
+#include "EigenBaseTrack.h"
+#include "EigenKalmanFilter.h"
+/**
+ * @brief The KalmanBBoxTrack class extends the BaseTrack class and incorporates a Kalman filter for object tracking.
+ *        It specializes in tracking objects represented by bounding boxes in the format of top-left corner coordinates,
+ *        width, and height (tlwh format).
+ */
+namespace ByteTrackEigen {
+    class KalmanBBoxTrack : public BaseTrack {
+    private:
+        static KalmanFilter shared_kalman;  // A shared Kalman filter instance for multi-object tracking purposes.
+
+    public:
+        Eigen::Vector4d _tlwh;              // Bounding box of the tracked object in top-left width-height (tlwh) format.
+        KalmanFilter kalman_filter;         // An individual Kalman filter instance for this specific track.
+        Eigen::VectorXd mean;               // The mean state vector of the Kalman filter, representing the tracked object's state.
+        Eigen::MatrixXd covariance;         // The covariance matrix of the Kalman filter state, representing the uncertainty in the tracked state.
+        int tracklet_len;                   // The length of the tracklet, indicating the number of consecutive frames where the object has been tracked.
+        std::string object_id;
+        int class_id;
+        /**
+         * @brief Default constructor for KalmanBBoxTrack.
+         */
+        KalmanBBoxTrack();
+
+        /**
+         * @brief Constructor for KalmanBBoxTrack with initial bounding box and detection score.
+         *
+         * @param tlwh Initial bounding box in top-left width-height format.
+         * @param score Detection score associated with the bounding box.
+         */
+        KalmanBBoxTrack(const std::vector<float> tlwh, float score, int class_id_,std::string object_id_);
+
+        /**
+         * @brief Static method to perform the prediction step on multiple KalmanBBoxTrack objects.
+         *        Updates the mean and covariance of each track based on the shared Kalman filter.
+         *
+         * @param tracks Vector of shared pointers to KalmanBBoxTrack instances.
+         */
+        static void multi_predict(std::vector<std::shared_ptr<KalmanBBoxTrack>>& tracks);
+
+        /**
+         * @brief Converts bounding box from tlwh format to xyah format (center x, center y, aspect ratio, height).
+         *
+         * @param tlwh Bounding box in tlwh format.
+         * @return Eigen::VectorXd Bounding box in xyah format.
+         */
+        Eigen::VectorXd tlwh_to_xyah(Eigen::VectorXd tlwh);
+
+        /**
+         * @brief Activates the track with a given Kalman filter and frame ID.
+         *        Initializes the track ID and sets up the Kalman filter with the bounding box information.
+         *
+         * @param kalman_filter Kalman filter to be used for this track.
+         * @param frame_id Frame ID at which this track is activated.
+         */
+        void activate(KalmanFilter& kalman_filter, int frame_id);
+
+        /**
+         * @brief Updates the track with a new detection, potentially assigning a new ID.
+         *
+         * @param new_track The new detection represented as a KalmanBBoxTrack object.
+         * @param frame_id Frame ID of the new detection.
+         * @param new_id Flag indicating whether to assign a new ID to this track.
+         */
+        void update_track(const KalmanBBoxTrack& new_track, int frame_id, bool new_id = false);
+
+        /**
+         * @brief Re-activates the track with a new detection, possibly assigning a new ID.
+         *        Essentially a wrapper for the update_track method.
+         *
+         * @param new_track The new detection to reactivate the track with.
+         * @param frame_id Frame ID of the reactivation.
+         * @param new_id Flag indicating whether to assign a new ID.
+         */
+        void re_activate(const KalmanBBoxTrack& new_track, int frame_id, bool new_id = false);
+
+        /**
+         * @brief Updates the track with a new detection without changing its ID.
+         *
+         * @param new_track The new detection to update the track with.
+         * @param frame_id Frame ID of the update.
+         */
+        void update(const KalmanBBoxTrack& new_track, int frame_id);
+
+        /**
+         * @brief Converts tlwh bounding box to top-left bottom-right (tlbr) format.
+         *
+         * @param tlwh Bounding box in tlwh format.
+         * @return Eigen::Vector4d Bounding box in tlbr format.
+         */
+        static Eigen::Vector4d tlwh_to_tlbr(const Eigen::Vector4d tlwh);
+
+        /**
+         * @brief Converts tlbr bounding box to tlwh format.
+         *
+         * @param tlbr Bounding box in tlbr format.
+         * @return Eigen::Vector4d Bounding box in tlwh format.
+         */
+        static Eigen::Vector4d tlbr_to_tlwh(const Eigen::Vector4d tlbr);
+
+        /**
+         * @brief Returns the bounding box in tlwh format.
+         *
+         * @return Eigen::Vector4d Bounding box in tlwh format.
+         */
+        Eigen::Vector4d tlwh() const;
+
+        /**
+         * @brief Returns the bounding box in tlbr format.
+         *
+         * @return Eigen::Vector4d Bounding box in tlbr format.
+         */
+        Eigen::Vector4d tlbr() const;
+
+    private:
+        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/ByteTrackEigen/include/EigenKalmanFilter.h

@@ -0,0 +1,77 @@
+#pragma once
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+#include <iostream>
+#include <stdexcept>
+
+/// <summary>
+/// KalmanFilter is a class designed for tracking bounding boxes in image space.
+/// It uses a state estimation technique that employs a series of measurements observed over time,
+/// containing statistical noise and other inaccuracies, and produces estimates of unknown variables
+/// that tend to be more precise than those based on a single measurement alone.
+/// </summary>
+/// 
+
+namespace ByteTrackEigen {
+    class KalmanFilter
+    {
+    public:
+        /// Constructor for KalmanFilter.
+        KalmanFilter();
+
+        /// Destructor for KalmanFilter.
+        ~KalmanFilter();
+
+        /// Compute standard deviations based on the mean.
+        /// This is used for initializing the uncertainty in the filter.
+        /// @param mean The mean vector from which standard deviations are computed.
+        /// @return Vector of standard deviations.
+        Eigen::VectorXd create_std(const Eigen::VectorXd& mean);
+
+        /// Initialize a new track from an unassociated measurement.
+        /// This method is typically called to create a new track with an initial measurement.
+        /// @param measurement The initial measurement vector for the track.
+        /// @return A pair of mean and covariance matrix representing the initial state estimate.
+        std::pair<Eigen::VectorXd, Eigen::MatrixXd> initiate(const Eigen::VectorXd& measurement);
+
+        /// Project the state distribution to the measurement space.
+        /// This method is used to predict the next state of the object based on the current state.
+        /// @param mean The current state mean vector.
+        /// @param covariance The current state covariance matrix.
+        /// @return A pair of mean and covariance matrix representing the predicted state.
+        std::pair<Eigen::VectorXd, Eigen::MatrixXd> project(
+            const Eigen::VectorXd& mean,
+            const Eigen::MatrixXd& covariance
+        );
+
+        /// Run the Kalman filter prediction step for multiple measurements.
+        /// This method is used when multiple measurements are available simultaneously.
+        /// @param mean The mean matrix of all current states.
+        /// @param covariance The covariance matrix of all current states.
+        /// @return A pair of mean and covariance matrices representing the predicted states.
+        std::pair<Eigen::MatrixXd, Eigen::MatrixXd> multi_predict(
+            const Eigen::MatrixXd& mean,
+            const Eigen::MatrixXd& covariance
+        );
+
+        /// Run the Kalman filter correction step.
+        /// This method updates the state of the object based on the received measurement.
+        /// @param mean The predicted state mean vector.
+        /// @param covariance The predicted state covariance matrix.
+        /// @param measurement The new measurement vector.
+        /// @return A pair of mean and covariance matrix representing the updated state.
+        std::pair<Eigen::VectorXd, Eigen::MatrixXd> update(
+            const Eigen::VectorXd& mean,
+            const Eigen::MatrixXd& covariance,
+            const Eigen::VectorXd& measurement
+        );
+
+    private:
+        int ndim; // The dimension of the state space.
+        Eigen::MatrixXd motion_mat; // The motion model matrix, used to predict the next state.
+        Eigen::MatrixXd update_mat; // The update matrix used for projecting state distribution to measurement space.
+        double std_weight_position; // Standard deviation weight for the position, used in uncertainty modeling.
+        double std_weight_velocity; // Standard deviation weight for the velocity, used in uncertainty modeling.
+    };
+
+}

modules/ANSMOT/ByteTrackEigen/include/EigenLinearAssignment.h

@@ -0,0 +1,65 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <vector>
+#include <set>
+#include <iostream>
+#include <algorithm>
+#include <tuple>
+#include "EigenHungarianAlgorithm.h"
+
+/**
+ * @brief The LinearAssignment class encapsulates algorithms for solving the linear assignment problem.
+ *
+ * This class utilizes the Hungarian Algorithm for optimal assignment in a cost matrix. It's designed
+ * to be used in contexts where assignments between different sets (like tasks to workers, or
+ * observations to tracks) need to be made based on a cost matrix that quantifies the cost of each
+ * possible assignment.
+ *
+ * Usage of the Eigen library enhances efficiency and allows for easy manipulation of matrices,
+ * which are fundamental in the operations of this class.
+ */
+
+namespace ByteTrackEigen {
+    class LinearAssignment
+    {
+    private:
+        HungarianAlgorithmEigen hungarian; // An instance of HungarianAlgorithmEigen to perform the actual assignment computations.
+
+    public:
+        /**
+         * @brief Generates matches based on a cost matrix and a set of indices, considering a threshold.
+         *
+         * @param cost_matrix The matrix representing the cost of assigning each pair of elements.
+         * @param indices The matrix of indices representing potential matches.
+         * @param thresh A threshold value to determine acceptable assignments. Assignments with a cost above this threshold won't be considered.
+         * @return A tuple containing:
+         *         1. A vector of pairs representing the selected assignments.
+         *         2. A set of unassigned row indices.
+         *         3. A set of unassigned column indices.
+         */
+        std::tuple<std::vector<std::pair<int, int>>, std::set<int>, std::set<int>> indices_to_matches(
+            const Eigen::MatrixXd& cost_matrix,
+            const Eigen::MatrixXi& indices,
+            double thresh);
+
+        /**
+         * @brief Solves the linear assignment problem for a given cost matrix and threshold.
+         *
+         * This method is a straightforward approach to solve the linear assignment problem when all
+         * elements of the cost matrix are considered for assignment.
+         *
+         * @param cost_matrix The matrix representing the cost of assigning each pair of elements.
+         * @param thresh A threshold value to determine acceptable assignments. Assignments with a cost above this threshold won't be considered.
+         * @return A tuple containing:
+         *         1. A vector of pairs representing the selected assignments.
+         *         2. A set of unassigned row indices.
+         *         3. A set of unassigned column indices.
+         */
+        std::tuple<std::vector<std::pair<int, int>>, std::set<int>, std::set<int>> linear_assignment(
+            const Eigen::MatrixXd& cost_matrix,
+            double thresh
+        );
+    };
+
+}