Refactor project structure

modules/ANSMOT/ANSByteTrack.cpp

@@ -0,0 +1,196 @@
+#include "ANSByteTrack.h"
+#include "boost/property_tree/ptree.hpp"
+#include "boost/property_tree/json_parser.hpp"
+#include "boost/foreach.hpp"
+#include "boost/optional.hpp"
+#include <map>
+namespace ANSCENTER {
+
+    template <typename T>
+    T get_data(const boost::property_tree::ptree& pt, const std::string& key)
+    {
+        T ret;
+        if (boost::optional<T> data = pt.get_optional<T>(key))
+        {
+            ret = data.get();
+        }
+        return ret;
+    }
+    std::vector<ByteTrack::Object> GetByteTrackInputs(const boost::property_tree::ptree& pt)
+    {
+        std::vector<ByteTrack::Object> inputs_ref;
+        inputs_ref.clear();
+        BOOST_FOREACH(const boost::property_tree::ptree::value_type & child, pt.get_child("results"))
+        {
+            const boost::property_tree::ptree& result = child.second;
+            const auto class_id = get_data<int>(result, "class_id");
+            const auto prob = get_data<float>(result, "prob");
+            const auto x = get_data<float>(result, "x");
+            const auto y = get_data<float>(result, "y");
+            const auto width = get_data<float>(result, "width");
+            const auto height = get_data<float>(result, "height");
+            const auto left = get_data<float>(result, "left");
+            const auto top = get_data<float>(result, "top");
+            const auto right = get_data<float>(result, "right");
+            const auto bottom = get_data<float>(result, "bottom");
+            const auto object_id = get_data<std::string>(result, "object_id");
+            ByteTrack::Object temp{ ByteTrack::Rect(x, y, width, height),
+                                        class_id,prob,left,top,right,bottom,object_id };
+            inputs_ref.push_back(temp);
+        }
+        return inputs_ref;
+    }
+    ANSByteTrack::ANSByteTrack() {
+        _licenseValid = false;
+        CheckLicense();
+        tracker.update_parameters(30, 30, 0.5, 0.6, 0.8);
+    }
+
+    ANSByteTrack::~ANSByteTrack() {
+
+    }
+    bool ANSByteTrack::Destroy() {
+		return true;
+    }
+
+    bool ANSByteTrack::UpdateParameters(const std::string& trackerParameters) {
+        // Use JSON Boost to parse paramter from trackerParameters
+        try {
+            int frameRate, trackBuffer;
+            double trackThreshold, highThreshold, matchThresold;
+            bool autoFrameRate;
+            frameRate = 15;
+            trackBuffer = 300;
+            trackThreshold = 0.5;
+            highThreshold = 0.1;
+            matchThresold = 0.95;
+            autoFrameRate = true;
+            if (!trackerParameters.empty()) {
+                std::stringstream ss;
+                ss << trackerParameters;
+                boost::property_tree::ptree pt;
+                boost::property_tree::read_json(ss, pt);
+                auto rootNode = pt.get_child("parameters");
+
+                auto childNode = rootNode.get_child("frame_rate");
+                frameRate = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("track_buffer");
+                trackBuffer = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("track_threshold");
+                trackThreshold = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("high_threshold");
+                highThreshold = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("match_thresold");
+                matchThresold = childNode.get_value<float>();
+
+                // Optional: auto frame rate estimation
+                if (auto optNode = rootNode.get_child_optional("auto_frame_rate")) {
+                    autoFrameRate = optNode->get_value<int>() != 0;
+                }
+            }
+            tracker.update_parameters(frameRate, trackBuffer, trackThreshold, highThreshold, matchThresold, autoFrameRate);
+            return true;
+        }
+
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrack::UpdateParameters", e.what(), __FILE__, __LINE__);
+            return false;
+        }
+    }
+
+    std::string ANSByteTrack::Update(int modelId, const std::string& detectionData) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSByteTrack::Update", "Invalid license", __FILE__, __LINE__);
+            return "";
+        }
+        try {
+            boost::property_tree::ptree root;
+            boost::property_tree::ptree trackingObjects;
+            boost::property_tree::ptree pt;
+            //1. Get input
+            std::stringstream ss;
+            ss << detectionData;
+            boost::property_tree::read_json(ss, pt);
+            std::vector<ByteTrack::Object> objects = GetByteTrackInputs(pt);
+            //2. Do tracking
+            const auto outputs = tracker.update(objects);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    boost::property_tree::ptree trackingNode;
+                    trackingNode.put("model_id", modelId);
+                    trackingNode.put("track_id", outputs[i]->getTrackId());
+                    trackingNode.put("class_id", outputs[i]->class_id);
+                    trackingNode.put("prob", outputs[i]->getScore());
+                    trackingNode.put("x", outputs[i]->getRect().tlwh[0]);
+                    trackingNode.put("y", outputs[i]->getRect().tlwh[1]);
+                    trackingNode.put("width", outputs[i]->getRect().tlwh[2]);
+                    trackingNode.put("height", outputs[i]->getRect().tlwh[3]);
+                    trackingNode.put("left", outputs[i]->left);
+                    trackingNode.put("top", outputs[i]->top);
+                    trackingNode.put("right", outputs[i]->right);
+                    trackingNode.put("bottom", outputs[i]->bottom);
+                    trackingNode.put("valid", outputs[i]->isActivated());
+                    trackingNode.put("object_id", outputs[i]->object_id);
+                    // Add this node to the list.
+                    trackingObjects.push_back(std::make_pair("", trackingNode));
+                }
+            }  
+            //3. Convert result
+            root.add_child("results", trackingObjects);
+            std::ostringstream stream;
+            boost::property_tree::write_json(stream, root,false);
+            std::string st = stream.str();
+            return st;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrack::Update", e.what(), __FILE__, __LINE__);
+            return "";
+        }
+    }
+  
+    std::vector<TrackerObject> ANSByteTrack::UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSByteTrack::UpdateTracker", "Invalid license", __FILE__, __LINE__);
+			return std::vector<TrackerObject>();
+        }
+        try {
+            std::vector<TrackerObject> trackingResults;
+            std::vector<ByteTrack::Object> objects;
+            for (const auto& detObj : detectionObjects) {
+                ByteTrack::Object temp{ ByteTrack::Rect(detObj.x, detObj.y, detObj.width, detObj.height),
+                                        detObj.class_id,detObj.prob,detObj.left,detObj.top,detObj.right,detObj.bottom,detObj.object_id };
+                objects.push_back(temp);
+			}
+            //2. Do tracking
+            const auto outputs = tracker.update(objects);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+					TrackerObject trackObj;
+					trackObj.track_id = outputs[i]->getTrackId();
+					trackObj.class_id = outputs[i]->class_id;
+					trackObj.prob = outputs[i]->getScore();
+					trackObj.x = outputs[i]->getRect().tlwh[0];
+					trackObj.y = outputs[i]->getRect().tlwh[1];
+					trackObj.width = outputs[i]->getRect().tlwh[2];
+					trackObj.height = outputs[i]->getRect().tlwh[3];
+					trackObj.left = outputs[i]->left;
+					trackObj.top = outputs[i]->top;
+					trackObj.right = outputs[i]->right;
+					trackObj.bottom = outputs[i]->bottom;
+					trackObj.object_id = outputs[i]->object_id;
+					trackingResults.push_back(trackObj);
+                }
+            }
+			return trackingResults;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrack::UpdateTracker", e.what(), __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+    
+    }
+}

modules/ANSMOT/ANSByteTrack.h

@@ -0,0 +1,19 @@
+#ifndef ANSBYTETRACK_H
+#define ANSBYTETRACK_H
+#include "ANSMOT.h"
+#include "BYTETracker.h"
+namespace ANSCENTER {
+    class ANSMOT_API ANSByteTrack :public ANSMOT
+    {
+    private:
+        ByteTrack::BYTETracker tracker;
+    public:
+        ~ANSByteTrack();
+        ANSByteTrack();
+        [[nodiscard]] bool UpdateParameters(const std::string& trackerParameters) override;
+        [[nodiscard]] std::string Update(int modelId, const std::string& detectionData) override;
+        [[nodiscard]] std::vector<TrackerObject> UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) override;
+        [[nodiscard]] bool Destroy() override;
+    };
+}
+#endif 

modules/ANSMOT/ANSByteTrackEigen.cpp

@@ -0,0 +1,238 @@
+#include "ANSByteTrackEigen.h"
+#include "boost/property_tree/ptree.hpp"
+#include "boost/property_tree/json_parser.hpp"
+#include "boost/foreach.hpp"
+#include "boost/optional.hpp"
+#include <map>
+namespace ANSCENTER {
+
+    Eigen::MatrixXf GetByteTrackEigenInputs(const char* jsonString, std::vector<std::string>&object_ids) {
+        Eigen::MatrixXf ret(0, 7); // 0 row and 7 columns
+        try {
+            boost::property_tree::ptree pt;
+            std::stringstream ss;
+            ss << jsonString;
+            object_ids.clear();
+            boost::property_tree::read_json(ss, pt);
+            boost::property_tree::ptree detections = pt.get_child("results");
+            if (detections.size() > 0) {
+                for (const auto& detection : detections) {
+                    float xMin = detection.second.get<float>("x");
+                    float yMin = detection.second.get<float>("y");
+                    float width = detection.second.get<float>("width");
+                    float height = detection.second.get<float>("height");
+                    float conf = detection.second.get<float>("prob");
+                    int classId = detection.second.get<int>("class_id");
+                    std::string object_id = detection.second.get<std::string>("object_id");
+                    double xMax = xMin + width;
+                    double yMax = yMin + height;
+                    Eigen::MatrixXf newRow(1, ret.cols());
+                    newRow << xMin, yMin, xMax, yMax, conf, classId;
+                    ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
+                    ret.row(ret.rows() - 1) = newRow;
+                    object_ids.push_back(object_id);
+                }
+            }
+            return ret;
+        }
+        catch (const std::exception& e) {
+            Eigen::MatrixXf newRow(1, ret.cols());
+            newRow << 0, 0, 0, 0, 0, 0, 0;
+            ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
+            ret.row(ret.rows() - 1) = newRow;
+            return ret;
+        }
+    }
+
+
+    ANSByteTrackEigen::ANSByteTrackEigen()
+    {
+        _licenseValid = false;
+        CheckLicense();
+
+        int frame_rate = 30;
+        int track_buffer = 30;
+        float track_thresh = 0.25;
+        float track_highthres = 0.25;
+        float match_thresh = 0.8;
+
+        tracker.update_parameters(frame_rate, track_buffer, track_thresh, track_highthres, match_thresh);
+    }
+
+    ANSByteTrackEigen::~ANSByteTrackEigen() {
+
+    }
+    bool ANSByteTrackEigen::Destroy() {
+        return true;
+    }
+
+    bool ANSByteTrackEigen::UpdateParameters(const std::string& trackerParameters) {
+        //// Use JSON Boost to parse paramter from trackerParameters
+        try {
+            int frameRate, trackBuffer;
+            double trackThreshold, highThreshold, matchThresold;
+            bool autoFrameRate;
+            frameRate = 15;
+            trackBuffer = 300;
+            trackThreshold = 0.5;
+            highThreshold = 0.1;
+            matchThresold = 0.95;
+            autoFrameRate = true;
+            if (!trackerParameters.empty()) {
+                std::stringstream ss;
+                ss << trackerParameters;
+                boost::property_tree::ptree pt;
+                boost::property_tree::read_json(ss, pt);
+                auto rootNode = pt.get_child("parameters");
+
+                auto childNode = rootNode.get_child("frame_rate");
+                frameRate = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("track_buffer");
+                trackBuffer = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("track_threshold");
+                trackThreshold = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("high_threshold");
+                highThreshold = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("match_thresold");
+                matchThresold = childNode.get_value<float>();
+
+                // Optional: auto frame rate estimation
+                if (auto optNode = rootNode.get_child_optional("auto_frame_rate")) {
+                    autoFrameRate = optNode->get_value<int>() != 0;
+                }
+            }
+            tracker.update_parameters(frameRate, trackBuffer, trackThreshold, highThreshold, matchThresold, autoFrameRate);
+            return true;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrackEigen::UpdateParameters", e.what(), __FILE__, __LINE__);
+            return false;
+        }
+    }
+
+    std::string ANSByteTrackEigen::Update(int modelId, const std::string& detectionData) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSByteTrackEigen::Update", "Invalid license", __FILE__, __LINE__);
+            return "";
+        }
+        try {
+            boost::property_tree::ptree root;
+            boost::property_tree::ptree trackingObjects;
+            boost::property_tree::ptree pt;
+            std::vector<std::string>obj_ids;
+            Eigen::MatrixXf objects = GetByteTrackEigenInputs(detectionData.c_str(), obj_ids);
+            ////2. Do tracking
+            const auto outputs = tracker.update(objects, obj_ids);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    double x = outputs[i]._tlwh[0]; //top left x
+                    double y = outputs[i]._tlwh[1]; //top left y
+                    double w = outputs[i]._tlwh[2]; //width
+                    double h = outputs[i]._tlwh[3]; //height
+                    double left = x;
+                    double top = y;
+                    double right = x + w;
+                    double bottom = y + h;
+                    int object_id = outputs[i].get_frame_id(); // Should be object id
+                    bool is_valid = outputs[i].get_is_activated();
+                    boost::property_tree::ptree trackingNode;
+                    trackingNode.put("model_id", modelId);
+                    trackingNode.put("track_id", outputs[i].get_track_id());
+                    trackingNode.put("class_id", outputs[i].get_track_id());//It should be class id
+                    trackingNode.put("prob", outputs[i].get_score());
+                    trackingNode.put("x", outputs[i]._tlwh[0]);
+                    trackingNode.put("y", outputs[i]._tlwh[1]);
+                    trackingNode.put("width", outputs[i]._tlwh[2]);
+                    trackingNode.put("height", outputs[i]._tlwh[3]);
+                    trackingNode.put("left", left);
+                    trackingNode.put("top", top);
+                    trackingNode.put("right", right);
+                    trackingNode.put("bottom", bottom);
+                    trackingNode.put("valid", is_valid);
+                    trackingNode.put("object_id", object_id);
+                    trackingObjects.push_back(std::make_pair("", trackingNode));
+                }
+            }  
+            //3. Convert result
+            root.add_child("results", trackingObjects);
+            std::ostringstream stream;
+            boost::property_tree::write_json(stream, root,false);
+            std::string st = stream.str();
+            return st;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrackEigen::Update", e.what(), __FILE__, __LINE__);
+            return "";
+        }
+    }
+    std::vector<TrackerObject> ANSByteTrackEigen::UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSByteTrackNCNN::UpdateTracker", "Invalid license", __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+        try {
+            std::vector<TrackerObject> trackingResults;
+            Eigen::MatrixXf ret(0, 7); // 0 row and 7 columns
+            std::vector<std::string> object_ids;
+            for (const auto& detection : detectionObjects) {
+                float xMin = detection.x;
+                float yMin = detection.y;
+                float width = detection.width;
+                float height = detection.height;
+                float conf = detection.prob;
+                int classId = detection.class_id;
+                std::string object_id = detection.object_id;
+                double xMax = xMin + width;
+                double yMax = yMin + height;
+                Eigen::MatrixXf newRow(1, ret.cols());
+                newRow << xMin, yMin, xMax, yMax, conf, classId;
+                ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
+                ret.row(ret.rows() - 1) = newRow;
+                object_ids.push_back(object_id);
+            }
+  
+            //2. Do tracking
+            const auto outputs = tracker.update(ret, object_ids);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    TrackerObject trackObj;
+                    double x = outputs[i]._tlwh[0]; //top left x
+                    double y = outputs[i]._tlwh[1]; //top left y
+                    double w = outputs[i]._tlwh[2]; //width
+                    double h = outputs[i]._tlwh[3]; //height
+                    double left = x;
+                    double top = y;
+                    double right = x + w;
+                    double bottom = y + h;
+                    std::string object_id = outputs[i].object_id; // Should be object id
+                    bool is_valid = outputs[i].get_is_activated();
+
+                    trackObj.track_id = outputs[i].get_track_id();
+                    trackObj.class_id = outputs[i].class_id;
+                    trackObj.prob = outputs[i].get_score();
+                    trackObj.x = x;
+                    trackObj.y = y;
+                    trackObj.width = w;
+                    trackObj.height = h;
+                    trackObj.left =x;
+                    trackObj.top = y;
+                    trackObj.right = right;
+                    trackObj.bottom = bottom;
+                    trackObj.object_id = object_id;
+                    trackingResults.push_back(trackObj);
+                }
+            }
+            return trackingResults;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrackNCNN::UpdateTracker", e.what(), __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+
+    }
+
+}

modules/ANSMOT/ANSByteTrackEigen.h

@@ -0,0 +1,19 @@
+#ifndef ANSBYTETRACKEIGEN_H
+#define ANSBYTETRACKEIGEN_H
+#include "ANSMOT.h"
+#include "EigenBYTETracker.h"
+namespace ANSCENTER {
+    class ANSMOT_API ANSByteTrackEigen :public ANSMOT
+    {
+    private:
+        ByteTrackEigen::BYTETracker tracker;
+    public:
+        ~ANSByteTrackEigen();
+        ANSByteTrackEigen();
+        [[nodiscard]] bool UpdateParameters(const std::string& trackerParameters) override;
+        [[nodiscard]] std::string Update(int modelId, const std::string& detectionData) override;
+        [[nodiscard]] std::vector<TrackerObject> UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) override;
+        [[nodiscard]] bool Destroy() override;
+    };
+}
+#endif // ANSMOT

modules/ANSMOT/ANSByteTrackNCNN.cpp

@@ -0,0 +1,197 @@
+#include "ANSByteTrackNCNN.h"
+#include "boost/property_tree/ptree.hpp"
+#include "boost/property_tree/json_parser.hpp"
+#include "boost/foreach.hpp"
+#include "boost/optional.hpp"
+#include <map>
+namespace ANSCENTER{
+
+    template <typename T>
+    T get_data(const boost::property_tree::ptree& pt, const std::string& key)
+    {
+        T ret;
+        if (boost::optional<T> data = pt.get_optional<T>(key))
+        {
+            ret = data.get();
+        }
+        return ret;
+    }
+    std::vector<ByteTrackNCNN::Object> GetByteTrackNCNNInputs(const boost::property_tree::ptree& pt)
+    {
+        std::vector<ByteTrackNCNN::Object> inputs_ref;
+        inputs_ref.clear();
+        BOOST_FOREACH(const boost::property_tree::ptree::value_type & child, pt.get_child("results"))
+        {
+            const boost::property_tree::ptree& result = child.second;
+            const auto class_id = get_data<int>(result, "class_id");
+            const auto prob = get_data<float>(result, "prob");
+            const auto x = get_data<float>(result, "x");
+            const auto y = get_data<float>(result, "y");
+            const auto width = get_data<float>(result, "width");
+            const auto height = get_data<float>(result, "height");
+            const auto left = get_data<float>(result, "left");
+            const auto top = get_data<float>(result, "top");
+            const auto right = get_data<float>(result, "right");
+            const auto bottom = get_data<float>(result, "bottom");
+            const auto object_id = get_data<std::string>(result, "object_id");
+             
+            ByteTrackNCNN::Object temp{ ByteTrackNCNN::Rect(x, y, width, height),
+                                        class_id,prob,left,top,right,bottom,object_id };
+            inputs_ref.push_back(temp);
+        }
+        return inputs_ref;
+    }
+	ANSByteTrackNCNN::ANSByteTrackNCNN() {
+        _licenseValid = false;
+        CheckLicense();
+		tracker.update_parameters(30, 30, 0.5, 0.6, 0.8);
+	}
+
+	ANSByteTrackNCNN::~ANSByteTrackNCNN() {
+
+	}
+    bool ANSByteTrackNCNN::Destroy() {
+        return true;
+    }
+
+	bool ANSByteTrackNCNN::UpdateParameters(const std::string& trackerParameters) {
+        try {
+            // Use JSON Boost to parse paramter from trackerParameters
+            int frameRate, trackBuffer;
+            double trackThreshold, highThreshold, matchThresold;
+            bool autoFrameRate;
+            frameRate = 15;
+            trackBuffer = 300;
+            trackThreshold = 0.5;
+            highThreshold = 0.1;
+            matchThresold = 0.95;
+            autoFrameRate = true;
+            if (!trackerParameters.empty()) {
+                std::stringstream ss;
+                ss << trackerParameters;
+                boost::property_tree::ptree pt;
+                boost::property_tree::read_json(ss, pt);
+                auto rootNode = pt.get_child("parameters");
+
+                auto childNode = rootNode.get_child("frame_rate");
+                frameRate = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("track_buffer");
+                trackBuffer = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("track_threshold");
+                trackThreshold = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("high_threshold");
+                highThreshold = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("match_thresold");
+                matchThresold = childNode.get_value<float>();
+
+                // Optional: auto frame rate estimation
+                if (auto optNode = rootNode.get_child_optional("auto_frame_rate")) {
+                    autoFrameRate = optNode->get_value<int>() != 0;
+                }
+            }
+
+            tracker.update_parameters(frameRate, trackBuffer, trackThreshold, highThreshold, matchThresold, autoFrameRate);
+            return true;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrackNCNN::UpdateParameters", e.what(), __FILE__, __LINE__);
+            return false;
+        }
+	}
+
+	std::string ANSByteTrackNCNN::Update(int modelId, const std::string& detectionData) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSByteTrackNCNN::Update", "Invalid license", __FILE__, __LINE__);
+            return "";
+        }
+        try {
+            boost::property_tree::ptree root;
+            boost::property_tree::ptree trackingObjects;
+            boost::property_tree::ptree pt;
+            //1. Get input
+            std::stringstream ss;
+            ss << detectionData;
+            boost::property_tree::read_json(ss, pt);
+            std::vector<ByteTrackNCNN::Object> objects = GetByteTrackNCNNInputs(pt);
+            //2. Do tracking
+            const auto outputs = tracker.update(objects);
+            if (outputs.size()) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    boost::property_tree::ptree trackingNode;
+                    trackingNode.put("model_id", modelId);
+                    trackingNode.put("track_id", outputs[i].track_id);
+                    trackingNode.put("class_id", outputs[i].class_id);
+                    trackingNode.put("prob", outputs[i].score);
+                    trackingNode.put("x", outputs[i].tlwh[0]);
+                    trackingNode.put("y", outputs[i].tlwh[1]);
+                    trackingNode.put("width", outputs[i].tlwh[2]);
+                    trackingNode.put("height", outputs[i].tlwh[3]);
+                    trackingNode.put("left", outputs[i].left);
+                    trackingNode.put("top", outputs[i].top);
+                    trackingNode.put("right", outputs[i].right);
+                    trackingNode.put("bottom", outputs[i].bottom);
+                    trackingNode.put("valid", outputs[i].is_activated);
+                    trackingNode.put("object_id", outputs[i].object_id);
+                    // Add this node to the list.
+                    trackingObjects.push_back(std::make_pair("", trackingNode));
+                }
+            } 
+            //3. Convert result
+            root.add_child("results", trackingObjects);
+            std::ostringstream stream;
+            boost::property_tree::write_json(stream, root,false);
+            std::string st = stream.str();
+            return st;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrackNCNN::Update", e.what(), __FILE__, __LINE__);
+            return "";
+        }
+	}
+    std::vector<TrackerObject> ANSByteTrackNCNN::UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSByteTrackNCNN::UpdateTracker", "Invalid license", __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+        try {
+            std::vector<TrackerObject> trackingResults;
+            std::vector<ByteTrackNCNN::Object> objects;
+            for (const auto& detObj : detectionObjects) {
+                ByteTrackNCNN::Object temp{ ByteTrackNCNN::Rect(detObj.x, detObj.y, detObj.width, detObj.height),
+                                        detObj.class_id,detObj.prob,detObj.left,detObj.top,detObj.right,detObj.bottom,detObj.object_id };
+                objects.push_back(temp);
+            }
+            //2. Do tracking
+            const auto outputs = tracker.update(objects);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    TrackerObject trackObj;
+                    trackObj.track_id = outputs[i].track_id;
+                    trackObj.class_id = outputs[i].class_id;
+                    trackObj.prob = outputs[i].score;
+                    trackObj.x = outputs[i].tlwh[0];
+                    trackObj.y = outputs[i].tlwh[1];
+                    trackObj.width = outputs[i].tlwh[2];
+                    trackObj.height = outputs[i].tlwh[3];
+                    trackObj.left = outputs[i].left;
+                    trackObj.top = outputs[i].top;
+                    trackObj.right = outputs[i].right;
+                    trackObj.bottom = outputs[i].bottom;
+                    trackObj.object_id = outputs[i].object_id;
+                    trackingResults.push_back(trackObj);
+                }
+            }
+            return trackingResults;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSByteTrackNCNN::UpdateTracker", e.what(), __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+
+    }
+
+}

modules/ANSMOT/ANSByteTrackNCNN.h

@@ -0,0 +1,19 @@
+#ifndef ANSBYTETRACKNCNN_H
+#define ANSBYTETRACKNCNN_H
+#include "ANSMOT.h"
+#include "NCNNBYTETracker.h"
+namespace ANSCENTER {
+    class ANSMOT_API ANSByteTrackNCNN :public ANSMOT
+    {
+    private:
+        ByteTrackNCNN::BYTETracker tracker;
+    public:
+        ~ANSByteTrackNCNN();
+        ANSByteTrackNCNN();
+        [[nodiscard]] bool UpdateParameters(const std::string& trackerParameters) override;
+        [[nodiscard]] std::string Update(int modelId, const std::string& detectionData) override;
+        [[nodiscard]] std::vector<TrackerObject> UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) override;
+        [[nodiscard]] bool Destroy() override;
+    };
+}
+#endif // ANSMOT

modules/ANSMOT/ANSMOT.cpp

@@ -0,0 +1,36 @@
+#include "ANSMOT.h"
+static bool ansmotLicenceValid = false;
+// Global once_flag to protect license checking
+static std::once_flag ansmotLicenseOnceFlag;
+namespace ANSCENTER {
+	static void VerifyGlobalANSMOTLicense(const std::string& licenseKey) {
+		try {
+			ansmotLicenceValid = ANSCENTER::ANSLicenseHelper::LicenseVerification(licenseKey, 1002, "ODHUB-LV");
+			if (!ansmotLicenceValid) {
+				ansmotLicenceValid = ANSCENTER::ANSLicenseHelper::LicenseVerification(licenseKey, 1007, "ANSCV");
+			}
+		}
+		catch (std::exception& e) {
+			ansmotLicenceValid = false;
+		}
+	}
+	void ANSMOT::CheckLicense() {
+		try {
+			// Check once globally
+			std::call_once(ansmotLicenseOnceFlag, [this]() {
+				VerifyGlobalANSMOTLicense(_licenseKey);
+				});
+
+			// Update this instance's local license flag
+			_licenseValid = ansmotLicenceValid;
+		}
+		catch (const std::exception& e) {
+			this->_logger.LogFatal("ANSMOT::CheckLicense. Error:", e.what(), __FILE__, __LINE__);
+		}
+	}
+    bool ANSMOT::UpdateParameters(const std::string& trackerParameters) { return true; }
+    std::string ANSMOT::Update(int modelId, const std::string& detectionData) { return "";}
+	std::vector<TrackerObject> ANSMOT::UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) { return std::vector<TrackerObject>(); }
+
+	ANSMOT::~ANSMOT() { }
+}

modules/ANSMOT/ANSMOT.h

@@ -0,0 +1,62 @@
+#ifndef ANSANSMOT_H
+#define ANSANSMOT_H
+#define ANSMOT_API __declspec(dllexport)
+
+#include <vector>
+#include <random>
+#include <cstdlib>
+#include <cstdio>
+#include <string>
+#include <exception>
+#include <fstream>
+#include <algorithm>
+#include "ANSLicense.h"
+#include "LabVIEWHeader/extcode.h"
+
+namespace ANSCENTER {
+    struct TrackerObject
+    {
+        std::string object_id;                // Use to check if this object belongs to any camera
+        int class_id;
+		int track_id;
+        float prob;
+        float x;
+        float y;
+        float width;
+        float height;
+        float left;
+        float top;
+        float right;
+        float bottom;
+    };
+    //enum MOTType {
+    //    BYTETRACKNCNN = 0,
+    //    BYTETRACK = 1,
+    //    BYTETRACKEIGEN=2,
+    //    UCMC = 3,
+    //    OCSORT=4
+    //};
+
+    class ANSMOT_API ANSMOT
+    {
+    protected:
+        bool            _licenseValid{ false };
+        std::string     _licenseKey;
+        SPDLogger&      _logger = SPDLogger::GetInstance("ANSMOT", false);
+        void            CheckLicense();
+    public:
+        [[nodiscard]] virtual bool UpdateParameters(const std::string& trackerParameters);
+        [[nodiscard]] virtual std::string Update(int modelId, const std::string& detectionData);
+		[[nodiscard]] virtual std::vector<TrackerObject> UpdateTracker(int modelId, const std::vector<TrackerObject> &detectionObjects);
+        virtual ~ANSMOT();
+        [[nodiscard]] virtual bool Destroy() = 0;
+    };
+}
+
+extern "C" __declspec(dllexport) int            __cdecl CreateANSMOTHandle(ANSCENTER::ANSMOT **Handle, int motType);
+extern "C" __declspec(dllexport) int            __cdecl ReleaseANSMOTHandle(ANSCENTER::ANSMOT **Handle);
+extern "C" __declspec(dllexport) int            __cdecl UpdateANSMOTParameters(ANSCENTER::ANSMOT **Handle, const char* );
+extern "C" __declspec(dllexport) int            __cdecl UpdateANSMOT(ANSCENTER::ANSMOT **Handle, int modelId, const char* detectionData, std::string &result);
+extern "C" __declspec(dllexport) int            __cdecl UpdateANSMOT_LV(ANSCENTER::ANSMOT * *Handle, int modelId, const char* detectionData, LStrHandle trackerResult);
+extern "C" __declspec(dllexport) int            __cdecl UpdateANSMOTTracker(ANSCENTER::ANSMOT** Handle, int modelId, const std::vector<ANSCENTER::TrackerObject>& detectionObjects, std::vector<ANSCENTER::TrackerObject>& trackedObjects);
+#endif // ANSMOT

modules/ANSMOT/ANSOCSortTrack.cpp

@@ -0,0 +1,297 @@
+#include "ANSOCSortTrack.h"
+#include "OCSortObject.h"
+#include "boost/property_tree/ptree.hpp"
+#include "boost/property_tree/json_parser.hpp"
+#include "boost/foreach.hpp"
+#include "boost/optional.hpp"
+#include <map>
+namespace ANSCENTER {
+  
+    Eigen::MatrixXf GetOCSortInputs(const char* jsonString) {
+        Eigen::MatrixXf ret(0, 9); // 0 row and 9 columns
+        try {
+            boost::property_tree::ptree pt;
+            std::stringstream ss;
+            ss << jsonString;
+            boost::property_tree::read_json(ss, pt);
+            boost::property_tree::ptree detections = pt.get_child("results");
+            if (detections.size() > 0) {
+                for (const auto& detection : detections) {
+                    float xMin = detection.second.get<float>("x");
+                    float yMin = detection.second.get<float>("y");
+                    float width = detection.second.get<float>("width");
+                    float height = detection.second.get<float>("height");
+                    float conf = detection.second.get<float>("prob");
+                    int classId = detection.second.get<int>("class_id");
+                    std::string object_id = detection.second.get<std::string>("object_id");
+                    std::stringstream ss(object_id);
+                    std::vector<std::string> result;
+                    int i = 0;
+                    while (ss.good())
+                    {
+                        std::string substr;
+                        getline(ss, substr, ';');
+                        result.push_back(substr);
+                        i++;
+                        if (i > 5)break;
+                    }
+                    // We now need to parse object_id into DGId,CameraId,and ModelId
+                    double xMax = xMin + width;
+                    double yMax = yMin + height;
+                    int DGId = 0;
+                    int CameraId = 0;
+                    int ModelId = 0;
+                    if (result.size() >= 4) {
+                        DGId = stoi(result[1]);
+                        CameraId = stoi(result[2]);
+                        ModelId = stoi(result[3]);
+                    }
+                    Eigen::MatrixXf newRow(1, ret.cols());
+                    newRow << xMin, yMin, xMax, yMax, conf, classId, DGId, CameraId, ModelId;
+                    ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
+                    ret.row(ret.rows() - 1) = newRow;
+                }
+            } 
+            return ret;
+        }
+        catch (const std::exception& e) {
+            return ret;
+        }
+    }
+
+    ANSOCSortTrack::ANSOCSortTrack()
+    {
+        _licenseValid = false;
+        CheckLicense();
+
+        float det_thresh = 0;
+        int max_age = 50;
+        int min_hits = 1;
+        float iou_threshold = 0.22136877277096445;
+        int delta_t = 1;
+        std::string asso_func = "iou";//giou//"iou"
+        float inertia = 0.3941737016672115;
+        bool use_byte = true;
+
+
+        tracker.update_parameters(det_thresh, max_age, min_hits, iou_threshold, delta_t, asso_func, inertia, use_byte);
+    }
+
+    ANSOCSortTrack::~ANSOCSortTrack() {
+
+    }
+
+    bool ANSOCSortTrack::Destroy() {
+        return true;
+    }
+    bool ANSOCSortTrack::UpdateParameters(const std::string& trackerParameters) {
+        //// Use JSON Boost to parse paramter from trackerParameters
+        try {
+            float det_thresh = 0;
+            int max_age = 50;
+            int min_hits = 1;
+            float iou_threshold = 0.22136877277096445;
+            int delta_t = 1;
+            int asso_func_in = 0;
+            std::string asso_func = "giou";
+            float inertia = 0.3941737016672115;
+            bool use_byte = true;
+            int use_byte_in = 0;
+            if (!trackerParameters.empty()) {
+                std::stringstream ss;
+                ss << trackerParameters;
+                boost::property_tree::ptree pt;
+                boost::property_tree::read_json(ss, pt);
+                auto rootNode = pt.get_child("parameters");
+
+                auto childNode = rootNode.get_child("det_thresh");
+                det_thresh = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("max_age");
+                max_age = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("min_hits");
+                min_hits = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("iou_threshold");
+                iou_threshold = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("delta_t");
+                delta_t = childNode.get_value<int>();
+
+                childNode = rootNode.get_child("asso_func");
+                asso_func_in = childNode.get_value<int>();
+
+                if (asso_func_in == 1)asso_func = "giou";
+                else asso_func = "iou";
+
+                childNode = rootNode.get_child("inertia");
+                inertia = childNode.get_value<float>();
+
+                childNode = rootNode.get_child("use_byte");
+                use_byte_in = childNode.get_value<int>();
+
+                if (use_byte_in == 1) use_byte = true;
+                else use_byte = false;
+            }  
+            tracker.update_parameters(det_thresh, max_age, min_hits, iou_threshold, delta_t, asso_func, inertia, use_byte);
+            return true;
+        }
+      
+        catch (const std::exception& e) {  
+            this->_logger.LogFatal("ANSOCSortTrack::UpdateParameters", e.what(), __FILE__, __LINE__);
+            return false;
+        }
+    }
+
+    std::string ANSOCSortTrack::Update(int modelId, const std::string& detectionData) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSOCSortTrack::Update", "Invalid license", __FILE__, __LINE__);
+            return "";
+        }
+        try {
+            boost::property_tree::ptree root;
+            boost::property_tree::ptree trackingObjects;
+            boost::property_tree::ptree pt;
+            std::vector<std::string> obj_ids;
+            Eigen::MatrixXf objects = GetOCSortInputs(detectionData.c_str());
+            //2. Do tracking
+            const auto outputs = tracker.update(objects);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    double x_min = outputs[i][0];
+                    double y_min = outputs[i][1];
+                    double x_max = outputs[i][2];
+                    double y_max = outputs[i][3];
+                    int trackId = static_cast<int>(outputs[i][4]);
+                    int classId = static_cast<int>(outputs[i][5]);
+                    double prob = static_cast<double>(outputs[i][6]);
+                    int DGId = static_cast<int>(outputs[i][7]);
+                    int CameraId = static_cast<int>(outputs[i][8]);
+                    int MId = static_cast<int>(outputs[i][9]);
+                    std::string name = std::to_string(MId) + ":" + std::to_string(classId);
+                    std::string object_id = name + ";" + std::to_string(DGId) + ";" + std::to_string(CameraId) + ";" + std::to_string(MId);
+                    bool is_valid = true;
+
+                    double width = x_max - x_min;
+                    double heigh = y_max - y_min;
+                    double left = x_min;
+                    double top = y_min;
+                    double right = x_max;
+                    double bottom = y_max;
+
+                    boost::property_tree::ptree trackingNode;
+                    trackingNode.put("model_id", modelId);
+                    trackingNode.put("track_id", trackId);
+                    trackingNode.put("class_id", classId);
+                    trackingNode.put("prob", prob);
+                    trackingNode.put("x", x_min);
+                    trackingNode.put("y", y_min);
+                    trackingNode.put("width", width);
+                    trackingNode.put("height", heigh);
+                    trackingNode.put("left", left);
+                    trackingNode.put("top", top);
+                    trackingNode.put("right", right);
+                    trackingNode.put("bottom", bottom);
+                    trackingNode.put("valid", is_valid);
+                    trackingNode.put("object_id", object_id);
+                    trackingObjects.push_back(std::make_pair("", trackingNode));
+                }
+            }    
+            //3. Convert result
+            root.add_child("results", trackingObjects);
+            std::ostringstream stream;
+            boost::property_tree::write_json(stream, root,false);
+            std::string st = stream.str();
+            return st;
+
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSOCSortTrack::Update", e.what(), __FILE__, __LINE__);
+            return "";
+        }
+    }
+
+    std::vector<TrackerObject> ANSOCSortTrack::UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSOCSortTrack::UpdateTracker", "Invalid license", __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+        try {
+            std::vector<TrackerObject> trackingResults;
+            Eigen::MatrixXf ret(0, 9); // 0 row and 9 columns
+            if (detectionObjects.size() > 0) {
+                for (const auto& detection : detectionObjects) {
+                    float xMin = detection.x;
+                    float yMin = detection.y;
+                    float width = detection.width;
+                    float height = detection.height;
+                    float conf = detection.prob;
+                    int classId = detection.class_id;
+                    std::string object_id = detection.object_id;
+                   
+                    // We now need to parse object_id into DGId,CameraId,and ModelId
+                    double xMax = xMin + width;
+                    double yMax = yMin + height;
+                    int DGId = 0;
+                    int CameraId = 0;
+                    int ModelId = 0;
+                    Eigen::MatrixXf newRow(1, ret.cols());
+                    newRow << xMin, yMin, xMax, yMax, conf, classId, DGId, CameraId, ModelId;
+                    ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
+                    ret.row(ret.rows() - 1) = newRow;
+                }
+            }
+            //2. Do tracking
+            const auto outputs = tracker.update(ret);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    TrackerObject trackObj;
+
+                    double x_min = outputs[i][0];
+                    double y_min = outputs[i][1];
+                    double x_max = outputs[i][2];
+                    double y_max = outputs[i][3];
+                    int trackId = static_cast<int>(outputs[i][4]);
+                    int classId = static_cast<int>(outputs[i][5]);
+                    double prob = static_cast<double>(outputs[i][6]);
+                    int DGId = static_cast<int>(outputs[i][7]);
+                    int CameraId = static_cast<int>(outputs[i][8]);
+                    int MId = static_cast<int>(outputs[i][9]);
+                    std::string name = std::to_string(MId) + ":" + std::to_string(classId);
+                    std::string object_id = name + ";" + std::to_string(DGId) + ";" + std::to_string(CameraId) + ";" + std::to_string(MId);
+                    bool is_valid = true;
+
+                    double width = x_max - x_min;
+                    double heigh = y_max - y_min;
+                    double left = x_min;
+                    double top = y_min;
+                    double right = x_max;
+                    double bottom = y_max;
+
+                    trackObj.track_id = trackId;
+                    trackObj.class_id = classId;
+                    trackObj.prob = prob;
+                    trackObj.x = x_min;
+                    trackObj.y = y_min;
+                    trackObj.width = width;
+                    trackObj.height = heigh;
+                    trackObj.left = left;
+                    trackObj.top = top;
+                    trackObj.right = right;
+                    trackObj.bottom = bottom;
+                    trackObj.object_id = object_id;
+                    trackingResults.push_back(trackObj);
+                }
+            }
+            return trackingResults;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSOCSortTrack::UpdateTracker", e.what(), __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+
+    }
+
+
+}

modules/ANSMOT/ANSOCSortTrack.h

@@ -0,0 +1,20 @@
+#ifndef ANSOCSORTTRACK_H
+#define ANSOCSORTTRACK_H
+
+#include "ANSMOT.h"
+#include "OCSort.h"
+namespace ANSCENTER {
+    class ANSMOT_API ANSOCSortTrack :public ANSMOT
+    {
+    private:
+        ANSOCSort::OCSort tracker;
+    public:
+        ANSOCSortTrack();
+        ~ANSOCSortTrack();
+        [[nodiscard]] bool UpdateParameters(const std::string& trackerParameters) override;
+        [[nodiscard]] std::string Update(int modelId, const std::string& detectionData) override;
+        [[nodiscard]] std::vector<TrackerObject> UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) override;
+        [[nodiscard]] bool Destroy() override;
+    };
+}
+#endif 

modules/ANSMOT/ANSUCMC.cpp

@@ -0,0 +1,216 @@
+#include "ANSUCMC.h"
+#include "boost/property_tree/ptree.hpp"
+#include "boost/property_tree/json_parser.hpp"
+#include "boost/foreach.hpp"
+#include "boost/optional.hpp"
+#include <map>
+namespace ANSCENTER {
+
+    template <typename T>
+    T get_data(const boost::property_tree::ptree& pt, const std::string& key)
+    {
+        T ret;
+        if (boost::optional<T> data = pt.get_optional<T>(key))
+        {
+            ret = data.get();
+        }
+        return ret;
+    }
+    std::vector<UCMC::Object> GetUCMCTrackInputs(const boost::property_tree::ptree& pt)
+    {
+        std::vector<UCMC::Object> inputs_ref;
+        inputs_ref.clear();
+        BOOST_FOREACH(const boost::property_tree::ptree::value_type & child, pt.get_child("results"))
+        {
+            const boost::property_tree::ptree& result = child.second;
+            const auto class_id = get_data<int>(result, "class_id");
+            const auto prob = get_data<float>(result, "prob");
+            const auto x = get_data<float>(result, "x");
+            const auto y = get_data<float>(result, "y");
+            const auto width = get_data<float>(result, "width");
+            const auto height = get_data<float>(result, "height");
+            const auto left = get_data<float>(result, "left");
+            const auto top = get_data<float>(result, "top");
+            const auto right = get_data<float>(result, "right");
+            const auto bottom = get_data<float>(result, "bottom");
+            const auto object_id = get_data<std::string>(result, "object_id");
+
+            UCMC::Object temp;
+            temp.rect.x = x;
+            temp.rect.y = y;
+            temp.rect.width = width;
+            temp.rect.height = height;
+            temp.label = class_id;
+            temp.prob = prob;
+            temp.object_id = object_id;
+            inputs_ref.push_back(temp);
+        }
+        return inputs_ref;
+    } 
+    ANSUCMCTrack::ANSUCMCTrack()
+    {
+        _licenseValid = false;
+        CheckLicense();
+
+        double a1 = 100.0;
+        double a2 = 100.0;
+        double wx = 5.0;
+        double wy = 5.0;
+        double vmax = 10.0;
+        double max_age = 10.0;
+        double high_score = 0.5;
+        double conf_threshold = 0.01;
+
+        std::vector<double> Ki = { 1040, 0, 680, 0, 
+                                   0, 1040, 382, 0,
+                                   0, 0, 1, 0};
+
+        std::vector<double> Ko = { -0.33962705646204017, -0.9403932802759871, -0.01771837833151917, 0,
+                                   -0.49984364998094355, 0.1964143950108213, -0.843550657048088,1,
+                                    0.7967495140209739, -0.2776362077328922, -0.5367572524549995, 33.64,
+                                    0, 0, 0, 1 };
+
+      tracker.update_parameters(a1, a2,wx,wy,vmax,max_age,high_score,conf_threshold,0.1,Ki,Ko);
+    }
+    ANSUCMCTrack::~ANSUCMCTrack() 
+    {
+
+    }
+    bool ANSUCMCTrack::Destroy() {
+        return true;
+    }
+    bool ANSUCMCTrack::UpdateParameters(const std::string& trackerParameters) {
+        // Use JSON Boost to parse paramter from trackerParameters
+        try {
+            double a1 = 100.0;
+            double a2 = 100.0;
+            double wx = 5.0;
+            double wy = 5.0;
+            double vmax = 10.0;
+            double max_age = 10.0;
+            double high_score = 0.5;
+            double conf_threshold = 0.01;
+
+            std::vector<double> Ki = { 1040., 0., 680., 0.,0., 1040., 382., 0.,0., 0., 1., 0. };
+            std::vector<double> Ko = { -0.33962705646204017, -0.9403932802759871, -0.01771837833151917, 0,
+                -0.49984364998094355, 0.1964143950108213, -0.843550657048088, 1,
+                0.7967495140209739, -0.2776362077328922, -0.5367572524549995, 33.64,
+                0, 0, 0, 1 };
+            tracker.update_parameters(a1, a2, wx, wy, vmax, max_age, high_score, conf_threshold, 0.1, Ki, Ko);
+            return true;
+        }
+
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSUCMCTrack::UpdateParameters", e.what(), __FILE__, __LINE__);
+            return false;
+        }
+    }
+    std::string ANSUCMCTrack::Update(int modelId, const std::string& detectionData) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSUCMCTrack::Update", "Invalid license", __FILE__, __LINE__);
+            return "";
+        }
+        try {
+            boost::property_tree::ptree root;
+            boost::property_tree::ptree trackingObjects;
+            boost::property_tree::ptree pt;
+            //1. Get input
+            std::stringstream ss;
+            ss << detectionData;
+            boost::property_tree::read_json(ss, pt);
+            std::vector<UCMC::Object> objects = GetUCMCTrackInputs(pt);
+            //2. Do tracking
+            std::vector<UCMC::Obj> outputs = tracker.update(objects);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    boost::property_tree::ptree trackingNode;
+                    trackingNode.put("model_id", modelId);
+                    trackingNode.put("track_id", outputs[i].track_idx);
+                    trackingNode.put("class_id", outputs[i].obj.label);
+                    trackingNode.put("prob", outputs[i].obj.prob);
+                    trackingNode.put("x", outputs[i].obj.rect.x);
+                    trackingNode.put("y", outputs[i].obj.rect.y);
+                    trackingNode.put("width", outputs[i].obj.rect.width);
+                    trackingNode.put("height", outputs[i].obj.rect.height);
+                    trackingNode.put("left", outputs[i].obj.rect.x);
+                    trackingNode.put("top", outputs[i].obj.rect.y);
+                    trackingNode.put("right", outputs[i].obj.rect.x+ outputs[i].obj.rect.width);
+                    trackingNode.put("bottom", outputs[i].obj.rect.y + outputs[i].obj.rect.height);
+                    trackingNode.put("valid", true);
+                    trackingNode.put("object_id", outputs[i].obj.object_id);
+                    // Add this node to the list.
+                    trackingObjects.push_back(std::make_pair("", trackingNode));
+                }
+            }
+            //3. Convert result
+            root.add_child("results", trackingObjects);
+            std::ostringstream stream;
+            boost::property_tree::write_json(stream, root,false);
+            std::string st = stream.str();
+            return st;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSUCMCTrack::Update", e.what(), __FILE__, __LINE__);
+            return "";
+        }
+    }
+    std::vector<TrackerObject> ANSUCMCTrack::UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) {
+        if (!_licenseValid) {
+            this->_logger.LogFatal("ANSByteTrack::UpdateTracker", "Invalid license", __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+        try {
+            std::vector<TrackerObject> trackingResults;
+            std::vector<UCMC::Object> objects;
+            for (const auto& detObj : detectionObjects) {
+                const auto class_id = detObj.class_id;
+                const auto prob = detObj.prob;
+                const auto x = detObj.x;
+                const auto y = detObj.y;
+                const auto width = detObj.width;
+                const auto height = detObj.height;
+                const auto left = detObj.left;
+                const auto top = detObj.top;
+                const auto right = detObj.right;
+                const auto bottom = detObj.bottom;
+                const auto object_id = detObj.object_id;
+
+                UCMC::Object temp;
+                temp.rect.x = x;
+                temp.rect.y = y;
+                temp.rect.width = width;
+                temp.rect.height = height;
+                temp.label = class_id;
+                temp.prob = prob;
+                temp.object_id = object_id;
+                objects.push_back(temp);
+            }
+            //2. Do tracking
+            const auto outputs = tracker.update(objects);
+            if (outputs.size() > 0) {
+                for (int i = 0; i < outputs.size(); i++) {
+                    TrackerObject trackObj;
+                    trackObj.track_id = outputs[i].track_idx;
+                    trackObj.class_id = outputs[i].obj.label;
+                    trackObj.prob = outputs[i].obj.prob;
+                    trackObj.x = outputs[i].obj.rect.x;
+                    trackObj.y = outputs[i].obj.rect.y;
+                    trackObj.width = outputs[i].obj.rect.width;
+                    trackObj.height = outputs[i].obj.rect.height;
+                    trackObj.left = outputs[i].obj.rect.x;
+                    trackObj.top = outputs[i].obj.rect.y;
+                    trackObj.right = outputs[i].obj.rect.x + outputs[i].obj.rect.width;
+                    trackObj.bottom = outputs[i].obj.rect.y + outputs[i].obj.rect.height;
+                    trackObj.object_id = outputs[i].obj.object_id;
+                    trackingResults.push_back(trackObj);
+                }
+            }
+            return trackingResults;
+        }
+        catch (const std::exception& e) {
+            this->_logger.LogFatal("ANSUCMCTrack::UpdateTracker", e.what(), __FILE__, __LINE__);
+            return std::vector<TrackerObject>();
+        }
+
+    }
+}

modules/ANSMOT/ANSUCMC.h

@@ -0,0 +1,20 @@
+#ifndef ANSUCMC_H
+#define ANSUCMC_H
+#pragma once
+#include "ANSMOT.h"
+#include "UCMCTracker.h"
+namespace ANSCENTER {
+    class ANSMOT_API ANSUCMCTrack :public ANSMOT
+    {
+    private:
+        UCMC::Tracker tracker;
+    public:
+        ANSUCMCTrack();
+        ~ANSUCMCTrack();
+        [[nodiscard]] bool UpdateParameters(const std::string& trackerParameters) override;
+        [[nodiscard]] std::string Update(int modelId, const std::string& detectionData) override;
+        [[nodiscard]] std::vector<TrackerObject> UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) override;
+        [[nodiscard]] bool Destroy() override;
+    };
+}
+#endif 

modules/ANSMOT/ByteTrack/include/BYTETracker.h

@@ -0,0 +1,86 @@
+#pragma once
+#include "STrack.h"
+#include "lapjv.h"
+#include "Object.h"
+#include <chrono>
+#include <cstddef>
+#include <limits>
+#include <map>
+#include <memory>
+#include <vector>
+
+namespace ByteTrack
+{
+    class BYTETracker
+    {
+    public:
+        using STrackPtr = std::shared_ptr<STrack>;
+
+        BYTETracker(const int& frame_rate = 30,
+                    const int& track_buffer = 30,
+                    const float& track_thresh = 0.5,
+                    const float& high_thresh = 0.6,
+                    const float& match_thresh = 0.8);
+        ~BYTETracker();
+        std::vector<STrackPtr> update(const std::vector<Object>& objects);
+        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<STrackPtr> jointStracks(const std::vector<STrackPtr> &a_tlist,
+                                            const std::vector<STrackPtr> &b_tlist) const;
+
+        std::vector<STrackPtr> subStracks(const std::vector<STrackPtr> &a_tlist,
+                                          const std::vector<STrackPtr> &b_tlist) const;
+
+        void removeDuplicateStracks(const std::vector<STrackPtr> &a_stracks,
+                                    const std::vector<STrackPtr> &b_stracks,
+                                    std::vector<STrackPtr> &a_res,
+                                    std::vector<STrackPtr> &b_res) const;
+
+        void linearAssignment(const std::vector<std::vector<float>> &cost_matrix,
+                              const int &cost_matrix_size,
+                              const int &cost_matrix_size_size,
+                              const float &thresh,
+                              std::vector<std::vector<int>> &matches,
+                              std::vector<int> &b_unmatched,
+                              std::vector<int> &a_unmatched) const;
+
+        std::vector<std::vector<float>> calcIouDistance(const std::vector<STrackPtr> &a_tracks,
+                                                        const std::vector<STrackPtr> &b_tracks) const;
+
+        std::vector<std::vector<float>> calcIous(const std::vector<Rect<float>> &a_rect,
+                                                 const std::vector<Rect<float>> &b_rect) const;
+
+        double execLapjv(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) const;
+
+        void estimateFrameRate();
+
+    private:
+        float track_thresh_;
+        float high_thresh_;
+        float match_thresh_;
+        size_t max_time_lost_;
+        int track_buffer_;
+
+        size_t frame_id_;
+        size_t track_id_count_;
+
+        // 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<STrackPtr> tracked_stracks_;
+        std::vector<STrackPtr> lost_stracks_;
+        std::vector<STrackPtr> removed_stracks_;
+    };
+}

modules/ANSMOT/ByteTrack/include/KalmanFilter.h

@@ -0,0 +1,36 @@
+#pragma once
+#include "Eigen/Dense"
+#include "Rect.h"
+namespace ByteTrack
+{
+    class KalmanFilter
+    {
+    public:
+        using DetectBox = Xyah<float>;
+
+        using StateMean = Eigen::Matrix<float, 1, 8, Eigen::RowMajor>;
+        using StateCov = Eigen::Matrix<float, 8, 8, Eigen::RowMajor>;
+
+        using StateHMean = Eigen::Matrix<float, 1, 4, Eigen::RowMajor>;
+        using StateHCov = Eigen::Matrix<float, 4, 4, Eigen::RowMajor>;
+
+        KalmanFilter(const float& std_weight_position = 1. / 20,
+                     const float& std_weight_velocity = 1. / 160);
+
+        void initiate(StateMean& mean, StateCov& covariance, const DetectBox& measurement);
+
+        void predict(StateMean& mean, StateCov& covariance);
+
+        void update(StateMean& mean, StateCov& covariance, const DetectBox& measurement);
+
+    private:
+        float std_weight_position_;
+        float std_weight_velocity_;
+
+        Eigen::Matrix<float, 8, 8, Eigen::RowMajor> motion_mat_;
+        Eigen::Matrix<float, 4, 8, Eigen::RowMajor> update_mat_;
+
+        void project(StateHMean &projected_mean, StateHCov &projected_covariance,
+                     const StateMean& mean, const StateCov& covariance);
+    };
+}

modules/ANSMOT/ByteTrack/include/Object.h

@@ -0,0 +1,24 @@
+#pragma once
+#include "Rect.h"
+namespace ByteTrack
+{
+    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/ByteTrack/include/Rect.h

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

@@ -0,0 +1,72 @@
+#pragma once
+
+#include "Rect.h"
+#include "KalmanFilter.h"
+#include <cstddef>
+#include <unordered_map>
+namespace ByteTrack
+{
+    enum class STrackState {
+        New = 0,
+        Tracked = 1,
+        Lost = 2,
+        Removed = 3,
+    };
+
+    class STrack
+    {
+    public:
+        STrack(const Rect<float>& rect, const float& score,int _class_id, float _left, float _top,
+               float _right, float _bottom, std::string _object_id);
+        ~STrack();
+
+        const Rect<float>& getRect() const;
+        const STrackState& getSTrackState() const;
+
+        const bool& isActivated() const;
+        const float& getScore() const;
+        const size_t& getTrackId() const;
+        const size_t& getFrameId() const;
+        const size_t& getStartFrameId() const;
+        const size_t& getTrackletLength() const;
+
+        void activate(const size_t& frame_id, const size_t& track_id);
+        void reActivate(const STrack &new_track, const size_t &frame_id, const int &new_track_id = -1);
+
+        void predict();
+        void update(const STrack &new_track, const size_t &frame_id);
+
+        void markAsLost();
+        void markAsRemoved();
+
+    private:
+        KalmanFilter kalman_filter_;
+        KalmanFilter::StateMean mean_;
+        KalmanFilter::StateCov covariance_;
+        bool is_activated_;
+        float score_;
+
+        Rect<float> rect_;
+        STrackState state_;
+
+        size_t track_id_;
+        size_t frame_id_;
+        size_t start_frame_id_;
+        size_t tracklet_len_;
+
+        void updateRect();
+    public:
+        float left; // left, top, right, bottom (original bounding of detected object)
+        float top;
+        float right;
+        float bottom;
+        std::string object_id;
+        int class_id;
+    private:
+        std::unordered_map<int, float> class_id_scores_; // class_id -> accumulated detection score
+        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/ByteTrack/include/lapjv.h

@@ -0,0 +1,6 @@
+#pragma once
+#include <cstddef>
+namespace ByteTrack
+{
+int lapjv_internal(const size_t n, double *cost[], int *x, int *y);
+}

modules/ANSMOT/ByteTrack/src/BYTETracker.cpp

@@ -0,0 +1,701 @@
+#include "BYTETracker.h"
+#include <algorithm>
+#include <cstddef>
+#include <limits>
+#include <map>
+#include <memory>
+#include <stdexcept>
+#include <utility>
+#include <vector>
+
+ByteTrack::BYTETracker::BYTETracker(const int& frame_rate,
+                                     const int& track_buffer,
+                                     const float& track_thresh,
+                                     const float& high_thresh,
+                                     const float& match_thresh) :
+    track_thresh_(track_thresh),
+    high_thresh_(high_thresh),
+    match_thresh_(match_thresh),
+    max_time_lost_(std::max(static_cast<size_t>(5), static_cast<size_t>(frame_rate / 30.0 * track_buffer))),
+    track_buffer_(track_buffer),
+    frame_id_(0),
+    track_id_count_(0),
+    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();
+}
+
+ByteTrack::BYTETracker::~BYTETracker()
+{
+}
+
+void ByteTrack::BYTETracker::update_parameters(int frameRate,
+                                               int trackBuffer,
+                                               double trackThreshold,
+                                               double highThreshold,
+                                               double matchThresold,
+                                               bool autoFrameRate) {
+    track_thresh_ = trackThreshold;
+    high_thresh_ = highThreshold;
+    match_thresh_ = 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(static_cast<size_t>(5), static_cast<size_t>(frameRate / 30.0 * trackBuffer));
+    frame_id_ = 0;
+    track_id_count_ = 0;
+    tracked_stracks_.clear();
+    lost_stracks_.clear();
+    removed_stracks_.clear();
+}
+
+float ByteTrack::BYTETracker::getEstimatedFps() const {
+    return estimated_fps_;
+}
+
+void ByteTrack::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;
+
+    // Ignore unreasonable gaps (likely pauses, not real frame intervals)
+    if (delta_sec < 0.001 || delta_sec > 5.0) {
+        return;
+    }
+
+    float current_fps = static_cast<float>(1.0 / delta_sec);
+
+    // Clamp to reasonable range
+    current_fps = std::max(1.0f, std::min(current_fps, 120.0f));
+
+    fps_sample_count_++;
+
+    // EMA smoothing: use higher alpha during warmup for faster convergence
+    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));
+
+    // Only adjust max_time_lost_ after warmup and when change is significant
+    if (fps_sample_count_ >= 10) {
+        size_t new_max_time_lost = std::max(
+            static_cast<size_t>(5),
+            static_cast<size_t>(estimated_fps_ / 30.0 * track_buffer_));
+
+        // Only update if there's a meaningful change (>10%)
+        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;
+        }
+    }
+}
+
+std::vector<ByteTrack::BYTETracker::STrackPtr> ByteTrack::BYTETracker::update(const std::vector<Object>& objects)
+{
+    // Auto-estimate frame rate from update() call timing
+    if (auto_frame_rate_) {
+        estimateFrameRate();
+    }
+
+    ////////////////// Step 1: Get detections //////////////////
+    frame_id_++;
+
+    // Create new STracks using the result of object detection
+    std::vector<STrackPtr> det_stracks;
+    std::vector<STrackPtr> det_low_stracks;
+
+    for (const auto &object : objects)
+    {
+        const auto strack = std::make_shared<STrack>(object.rect, 
+                                                     object.prob,
+                                                     object.label,
+                                                     object.left,
+                                                     object.top,
+                                                     object.right,
+                                                     object.bottom,
+                                                     object.object_id);
+        if (object.prob >= track_thresh_)
+        {
+            det_stracks.push_back(strack);
+        }
+        else
+        {
+            det_low_stracks.push_back(strack);
+        }
+    }
+
+    // Create lists of existing STrack
+    std::vector<STrackPtr> active_stracks;
+    std::vector<STrackPtr> non_active_stracks;
+    std::vector<STrackPtr> strack_pool;
+
+    for (const auto& tracked_strack : tracked_stracks_)
+    {
+        if (!tracked_strack->isActivated())
+        {
+            non_active_stracks.push_back(tracked_strack);
+        }
+        else
+        {
+            active_stracks.push_back(tracked_strack);
+        }
+    }
+
+    strack_pool = jointStracks(active_stracks, lost_stracks_);
+
+    // Multi-predict: call 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++)
+    {
+        for (auto &strack : strack_pool)
+        {
+            strack->predict();
+        }
+    }
+
+    ////////////////// Step 2: First association, with IoU //////////////////
+    // 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<STrackPtr> current_tracked_stracks;
+    std::vector<STrackPtr> remain_tracked_stracks;
+    std::vector<STrackPtr> remain_det_stracks;
+    std::vector<STrackPtr> refind_stracks;
+
+    {
+        std::vector<std::vector<int>> matches_idx;
+        std::vector<int> unmatch_detection_idx, unmatch_track_idx;
+
+        const auto dists = calcIouDistance(strack_pool, det_stracks);
+        linearAssignment(dists, strack_pool.size(), det_stracks.size(), effective_match_thresh,
+                         matches_idx, unmatch_track_idx, unmatch_detection_idx);
+
+        for (const auto &match_idx : matches_idx)
+        {
+            const auto track = strack_pool[match_idx[0]];
+            const auto det = det_stracks[match_idx[1]];
+            if (track->getSTrackState() == STrackState::Tracked)
+            {
+                track->update(*det, frame_id_);
+                current_tracked_stracks.push_back(track);
+            }
+            else
+            {
+                track->reActivate(*det, frame_id_);
+                refind_stracks.push_back(track);
+            }
+        }
+
+        for (const auto &unmatch_idx : unmatch_detection_idx)
+        {
+            remain_det_stracks.push_back(det_stracks[unmatch_idx]);
+        }
+
+        for (const auto &unmatch_idx : unmatch_track_idx)
+        {
+            if (strack_pool[unmatch_idx]->getSTrackState() == STrackState::Tracked)
+            {
+                remain_tracked_stracks.push_back(strack_pool[unmatch_idx]);
+            }
+        }
+    }
+
+    ////////////////// Step 3: Second association, using low score dets //////////////////
+    std::vector<STrackPtr> current_lost_stracks;
+
+    {
+        std::vector<std::vector<int>> matches_idx;
+        std::vector<int> unmatch_track_idx, unmatch_detection_idx;
+
+        const auto dists = calcIouDistance(remain_tracked_stracks, det_low_stracks);
+        linearAssignment(dists, remain_tracked_stracks.size(), det_low_stracks.size(), 0.5,
+                         matches_idx, unmatch_track_idx, unmatch_detection_idx);
+
+        for (const auto &match_idx : matches_idx)
+        {
+            const auto track = remain_tracked_stracks[match_idx[0]];
+            const auto det = det_low_stracks[match_idx[1]];
+            if (track->getSTrackState() == STrackState::Tracked)
+            {
+                track->update(*det, frame_id_);
+                current_tracked_stracks.push_back(track);
+            }
+            else
+            {
+                track->reActivate(*det, frame_id_);
+                refind_stracks.push_back(track);
+            }
+        }
+
+        for (const auto &unmatch_track : unmatch_track_idx)
+        {
+            const auto track = remain_tracked_stracks[unmatch_track];
+            if (track->getSTrackState() != STrackState::Lost)
+            {
+                track->markAsLost();
+                current_lost_stracks.push_back(track);
+            }
+        }
+    }
+
+    ////////////////// Step 4: Init new stracks //////////////////
+    std::vector<STrackPtr> current_removed_stracks;
+
+    {
+        std::vector<int> unmatch_detection_idx;
+        std::vector<int> unmatch_unconfirmed_idx;
+        std::vector<std::vector<int>> matches_idx;
+
+        // Deal with unconfirmed tracks, usually tracks with only one beginning frame
+        const auto dists = calcIouDistance(non_active_stracks, remain_det_stracks);
+        linearAssignment(dists, non_active_stracks.size(), remain_det_stracks.size(), 0.7,
+                         matches_idx, unmatch_unconfirmed_idx, unmatch_detection_idx);
+
+        for (const auto &match_idx : matches_idx)
+        {
+            non_active_stracks[match_idx[0]]->update(*remain_det_stracks[match_idx[1]], frame_id_);
+            current_tracked_stracks.push_back(non_active_stracks[match_idx[0]]);
+        }
+
+        for (const auto &unmatch_idx : unmatch_unconfirmed_idx)
+        {
+            const auto track = non_active_stracks[unmatch_idx];
+            track->markAsRemoved();
+            current_removed_stracks.push_back(track);
+        }
+
+        // Add new stracks
+        for (const auto &unmatch_idx : unmatch_detection_idx)
+        {
+            const auto track = remain_det_stracks[unmatch_idx];
+            if (track->getScore() < high_thresh_)
+            {
+                continue;
+            }
+            track_id_count_++;
+            track->activate(frame_id_, track_id_count_);
+            current_tracked_stracks.push_back(track);
+        }
+    }
+
+    ////////////////// Step 5: Update state //////////////////
+    for (const auto &lost_strack : lost_stracks_)
+    {
+        if (frame_id_ - lost_strack->getFrameId() > max_time_lost_)
+        {
+            lost_strack->markAsRemoved();
+            current_removed_stracks.push_back(lost_strack);
+        }
+    }
+
+    tracked_stracks_ = jointStracks(current_tracked_stracks, refind_stracks);
+    lost_stracks_ = subStracks(jointStracks(subStracks(lost_stracks_, tracked_stracks_), current_lost_stracks), removed_stracks_);
+    removed_stracks_ = jointStracks(removed_stracks_, current_removed_stracks);
+
+    std::vector<STrackPtr> tracked_stracks_out, lost_stracks_out;
+    removeDuplicateStracks(tracked_stracks_, lost_stracks_, tracked_stracks_out, lost_stracks_out);
+    tracked_stracks_ = tracked_stracks_out;
+    lost_stracks_ = lost_stracks_out;
+
+    std::vector<STrackPtr> output_stracks;
+    for (const auto &track : tracked_stracks_)
+    {
+        output_stracks.push_back(track);  // Pushback all trackers
+       /* if (track->isActivated())
+        {
+            output_stracks.push_back(track);
+        }*/
+    }
+
+    return output_stracks;
+}
+std::vector<ByteTrack::BYTETracker::STrackPtr> ByteTrack::BYTETracker::jointStracks(const std::vector<STrackPtr> &a_tlist,
+                                                                                      const std::vector<STrackPtr> &b_tlist) const
+{
+    std::map<int, int> exists;
+    std::vector<STrackPtr> res;
+    for (size_t i = 0; i < a_tlist.size(); i++)
+    {
+        exists.emplace(a_tlist[i]->getTrackId(), 1);
+        res.push_back(a_tlist[i]);
+    }
+    for (size_t i = 0; i < b_tlist.size(); i++)
+    {
+        const int &tid = b_tlist[i]->getTrackId();
+        if (exists.count(tid) == 0)
+        {
+            exists[tid] = 1;
+            res.push_back(b_tlist[i]);
+        }
+    }
+    return res;
+}
+
+std::vector<ByteTrack::BYTETracker::STrackPtr> ByteTrack::BYTETracker::subStracks(const std::vector<STrackPtr> &a_tlist,
+                                                                                    const std::vector<STrackPtr> &b_tlist) const
+{
+    std::map<int, STrackPtr> stracks;
+    for (size_t i = 0; i < a_tlist.size(); i++)
+    {
+        stracks.emplace(a_tlist[i]->getTrackId(), a_tlist[i]);
+    }
+
+    for (size_t i = 0; i < b_tlist.size(); i++)
+    {
+        const int &tid = b_tlist[i]->getTrackId();
+        if (stracks.count(tid) != 0)
+        {
+            stracks.erase(tid);
+        }
+    }
+
+    std::vector<STrackPtr> res;
+    std::map<int, STrackPtr>::iterator it;
+    for (it = stracks.begin(); it != stracks.end(); ++it)
+    {
+        res.push_back(it->second);
+    }
+
+    return res;
+}
+
+void ByteTrack::BYTETracker::removeDuplicateStracks(const std::vector<STrackPtr> &a_stracks,
+                                                     const std::vector<STrackPtr> &b_stracks,
+                                                     std::vector<STrackPtr> &a_res,
+                                                     std::vector<STrackPtr> &b_res) const
+{
+    const auto ious = calcIouDistance(a_stracks, b_stracks);
+
+    std::vector<std::pair<size_t, size_t>> overlapping_combinations;
+    for (size_t i = 0; i < ious.size(); i++)
+    {
+        for (size_t j = 0; j < ious[i].size(); j++)
+        {
+            if (ious[i][j] < 0.15)
+            {
+                overlapping_combinations.emplace_back(i, j);
+            }
+        }
+    }
+
+    std::vector<bool> a_overlapping(a_stracks.size(), false), b_overlapping(b_stracks.size(), false);
+    for (const auto &[a_idx, b_idx] : overlapping_combinations)
+    {
+        const int timep = a_stracks[a_idx]->getFrameId() - a_stracks[a_idx]->getStartFrameId();
+        const int timeq = b_stracks[b_idx]->getFrameId() - b_stracks[b_idx]->getStartFrameId();
+        if (timep > timeq)
+        {
+            b_overlapping[b_idx] = true;
+        }
+        else
+        {
+            a_overlapping[a_idx] = true;
+        }
+    }
+
+    for (size_t ai = 0; ai < a_stracks.size(); ai++)
+    {
+        if (!a_overlapping[ai])
+        {
+            a_res.push_back(a_stracks[ai]);
+        }
+    }
+
+    for (size_t bi = 0; bi < b_stracks.size(); bi++)
+    {
+        if (!b_overlapping[bi])
+        {
+            b_res.push_back(b_stracks[bi]);
+        }
+    }
+}
+
+void ByteTrack::BYTETracker::linearAssignment(const std::vector<std::vector<float>> &cost_matrix,
+                                               const int &cost_matrix_size,
+                                               const int &cost_matrix_size_size,
+                                               const float &thresh,
+                                               std::vector<std::vector<int>> &matches,
+                                               std::vector<int> &a_unmatched,
+                                               std::vector<int> &b_unmatched) const
+{
+    if (cost_matrix.size() == 0)
+    {
+        for (int i = 0; i < cost_matrix_size; i++)
+        {
+            a_unmatched.push_back(i);
+        }
+        for (int i = 0; i < cost_matrix_size_size; i++)
+        {
+            b_unmatched.push_back(i);
+        }
+        return;
+    }
+
+    std::vector<int> rowsol; std::vector<int> colsol;
+    execLapjv(cost_matrix, rowsol, colsol, true, thresh);
+    for (size_t 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
+        {
+            a_unmatched.push_back(i);
+        }
+    }
+
+    for (size_t i = 0; i < colsol.size(); i++)
+    {
+        if (colsol[i] < 0)
+        {
+            b_unmatched.push_back(i);
+        }
+    }
+}
+
+std::vector<std::vector<float>> ByteTrack::BYTETracker::calcIous(const std::vector<Rect<float>> &a_rect,
+                                                                  const std::vector<Rect<float>> &b_rect) const
+{
+    std::vector<std::vector<float>> ious;
+    if (a_rect.size() * b_rect.size() == 0)
+    {
+        return ious;
+    }
+
+    ious.resize(a_rect.size());
+    for (size_t i = 0; i < ious.size(); i++)
+    {
+        ious[i].resize(b_rect.size());
+    }
+
+    for (size_t bi = 0; bi < b_rect.size(); bi++)
+    {
+        for (size_t ai = 0; ai < a_rect.size(); ai++)
+        {
+            ious[ai][bi] = b_rect[bi].calcIoU(a_rect[ai]);
+        }
+    }
+    return ious;
+}
+
+std::vector<std::vector<float> > ByteTrack::BYTETracker::calcIouDistance(const std::vector<STrackPtr> &a_tracks,
+                                                                          const std::vector<STrackPtr> &b_tracks) const
+{
+    std::vector<ByteTrack::Rect<float>> a_rects, b_rects;
+    for (size_t i = 0; i < a_tracks.size(); i++)
+    {
+        a_rects.push_back(a_tracks[i]->getRect());
+    }
+
+    for (size_t i = 0; i < b_tracks.size(); i++)
+    {
+        b_rects.push_back(b_tracks[i]->getRect());
+    }
+
+    const auto ious = calcIous(a_rects, b_rects);
+
+    std::vector<std::vector<float>> cost_matrix;
+    for (size_t i = 0; i < ious.size(); i++)
+    {
+        std::vector<float> iou;
+        for (size_t j = 0; j < ious[i].size(); j++)
+        {
+            iou.push_back(1 - ious[i][j]);
+        }
+        cost_matrix.push_back(iou);
+    }
+
+    return cost_matrix;
+}
+
+double ByteTrack::BYTETracker::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) const
+{
+    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)
+        {
+			return -1;
+        }
+    }
+
+    if (extend_cost || cost_limit < std::numeric_limits<float>::max())
+    {
+        n = n_rows + n_cols;
+        cost_c_extended.resize(n);
+        for (size_t i = 0; i < cost_c_extended.size(); i++)
+            cost_c_extended[i].resize(n);
+
+        if (cost_limit < std::numeric_limits<float>::max())
+        {
+            for (size_t i = 0; i < cost_c_extended.size(); i++)
+            {
+                for (size_t j = 0; j < cost_c_extended[i].size(); j++)
+                {
+                    cost_c_extended[i][j] = cost_limit / 2.0;
+                }
+            }
+        }
+        else
+        {
+            float cost_max = -1;
+            for (size_t i = 0; i < cost_c.size(); i++)
+            {
+                for (size_t j = 0; j < cost_c[i].size(); j++)
+                {
+                    if (cost_c[i][j] > cost_max)
+                        cost_max = cost_c[i][j];
+                }
+            }
+            for (size_t i = 0; i < cost_c_extended.size(); i++)
+            {
+                for (size_t j = 0; j < cost_c_extended[i].size(); j++)
+                {
+                    cost_c_extended[i][j] = cost_max + 1;
+                }
+            }
+        }
+
+        for (size_t i = n_rows; i < cost_c_extended.size(); i++)
+        {
+            for (size_t 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 (size_t i = 0; i < rowsol.size(); i++)
+            {
+                if (rowsol[i] != -1)
+                {
+                    opt += cost_ptr[i][rowsol[i]];
+                }
+            }
+        }
+    }
+    else if (return_cost)
+    {
+        for (size_t 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;
+}

modules/ANSMOT/ByteTrack/src/KalmanFilter.cpp

@@ -0,0 +1,89 @@
+#include "KalmanFilter.h"
+
+#include <cstddef>
+
+ByteTrack::KalmanFilter::KalmanFilter(const float& std_weight_position,
+                                       const float& std_weight_velocity) :
+    std_weight_position_(std_weight_position),
+    std_weight_velocity_(std_weight_velocity)
+{
+    constexpr size_t ndim = 4;
+    constexpr float dt = 1;
+
+    motion_mat_ = Eigen::MatrixXf::Identity(8, 8);
+    update_mat_ = Eigen::MatrixXf::Identity(4, 8);
+
+    for (size_t i = 0; i < ndim; i++)
+    {
+        motion_mat_(i, ndim + i) = dt;
+    }
+}
+
+void ByteTrack::KalmanFilter::initiate(StateMean &mean, StateCov &covariance, const DetectBox &measurement)
+{
+    mean.block<1, 4>(0, 0) = measurement.block<1, 4>(0, 0);
+    mean.block<1, 4>(0, 4) = Eigen::Vector4f::Zero();
+
+    StateMean 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];
+
+    StateMean tmp = std.array().square();
+    covariance = tmp.asDiagonal();
+}
+
+void ByteTrack::KalmanFilter::predict(StateMean &mean, StateCov &covariance)
+{
+    StateMean std;
+    std(0) = std_weight_position_ * mean(3);
+    std(1) = std_weight_position_ * mean(3);
+    std(2) = 1e-2;
+    std(3) = std_weight_position_ * mean(3);
+    std(4) = std_weight_velocity_ * mean(3);
+    std(5) = std_weight_velocity_ * mean(3);
+    std(6) = 1e-5;
+    std(7) = std_weight_velocity_ * mean(3);
+
+    StateMean tmp = std.array().square();
+    StateCov motion_cov = tmp.asDiagonal();
+
+    mean = motion_mat_ * mean.transpose();
+    covariance = motion_mat_ * covariance * (motion_mat_.transpose()) + motion_cov;
+}
+
+void ByteTrack::KalmanFilter::update(StateMean &mean, StateCov &covariance, const DetectBox &measurement)
+{
+    StateHMean projected_mean;
+    StateHCov projected_cov;
+    project(projected_mean, projected_cov, mean, covariance);
+
+    Eigen::Matrix<float, 4, 8> B = (covariance * (update_mat_.transpose())).transpose();
+    Eigen::Matrix<float, 8, 4> kalman_gain = (projected_cov.llt().solve(B)).transpose();
+    Eigen::Matrix<float, 1, 4> innovation = measurement - projected_mean;
+
+    const auto tmp = innovation * (kalman_gain.transpose());
+    mean = (mean.array() + tmp.array()).matrix();
+    covariance = covariance - kalman_gain * projected_cov * (kalman_gain.transpose());
+}
+
+void ByteTrack::KalmanFilter::project(StateHMean &projected_mean, StateHCov &projected_covariance,
+                                       const StateMean& mean, const StateCov& covariance)
+{
+    DetectBox std;
+    std << std_weight_position_ * mean(3),
+           std_weight_position_ * mean(3),
+           1e-1,
+           std_weight_position_ * mean(3);
+
+    projected_mean = update_mat_ * mean.transpose();
+    projected_covariance = update_mat_ * covariance * (update_mat_.transpose());
+
+    Eigen::Matrix<float, 4, 4> diag = std.asDiagonal();
+    projected_covariance += diag.array().square().matrix();
+}

modules/ANSMOT/ByteTrack/src/Object.cpp

@@ -0,0 +1,19 @@
+#include "Object.h"
+ByteTrack::Object::Object(const ByteTrack::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) : 
+						  rect(_rect), 
+						  label(_label), 
+						  prob(_prob),
+						  left(_left),
+						  top(_top),
+						  right(_right),
+						  bottom(_bottom),
+						  object_id(_object_id)
+{
+}

modules/ANSMOT/ByteTrack/src/Rect.cpp

@@ -0,0 +1,147 @@
+#include "Rect.h"
+#include <algorithm>
+template <typename T>
+ByteTrack::Rect<T>::Rect(const T &x, const T &y, const T &width, const T &height) :
+    tlwh({x, y, width, height})
+{
+}
+
+template <typename T>
+ByteTrack::Rect<T>::~Rect()
+{
+}
+
+template <typename T>
+const T& ByteTrack::Rect<T>::x() const
+{
+    return tlwh[0];
+}
+
+template <typename T>
+const T& ByteTrack::Rect<T>::y() const
+{
+    return tlwh[1];
+}
+
+template <typename T>
+const T& ByteTrack::Rect<T>::width() const
+{
+    return tlwh[2];
+}
+
+template <typename T>
+const T& ByteTrack::Rect<T>::height() const
+{
+    return tlwh[3];
+}
+
+template <typename T>
+T& ByteTrack::Rect<T>::x()
+{
+    return tlwh[0];
+}
+
+template <typename T>
+T& ByteTrack::Rect<T>::y()
+{
+    return tlwh[1];
+}
+
+template <typename T>
+T& ByteTrack::Rect<T>::width()
+{
+    return tlwh[2];
+}
+
+template <typename T>
+T& ByteTrack::Rect<T>::height()
+{
+    return tlwh[3];
+}
+
+template <typename T>
+const T& ByteTrack::Rect<T>::tl_x() const
+{
+    return tlwh[0];
+}
+
+template <typename T>
+const T& ByteTrack::Rect<T>::tl_y() const
+{
+    return tlwh[1];
+}
+
+template <typename T>
+T ByteTrack::Rect<T>::br_x() const
+{
+    return tlwh[0] + tlwh[2];
+}
+
+template <typename T>
+T ByteTrack::Rect<T>::br_y() const
+{
+    return tlwh[1] + tlwh[3];
+}
+
+template <typename T>
+ByteTrack::Tlbr<T> ByteTrack::Rect<T>::getTlbr() const
+{
+    return {
+        tlwh[0],
+        tlwh[1],
+        tlwh[0] + tlwh[2],
+        tlwh[1] + tlwh[3],
+    };
+}
+
+template <typename T>
+ByteTrack::Xyah<T> ByteTrack::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 ByteTrack::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>
+ByteTrack::Rect<T> ByteTrack::generate_rect_by_tlbr(const ByteTrack::Tlbr<T>& tlbr)
+{
+    return ByteTrack::Rect<T>(tlbr[0], tlbr[1], tlbr[2] - tlbr[0], tlbr[3] - tlbr[1]);
+}
+
+template<typename T>
+ByteTrack::Rect<T> ByteTrack::generate_rect_by_xyah(const ByteTrack::Xyah<T>& xyah)
+{
+    const auto width = xyah[2] * xyah[3];
+    return ByteTrack::Rect<T>(xyah[0] - width / 2, xyah[1] - xyah[3] / 2, width, xyah[3]);
+}
+
+// explicit instantiation
+template class ByteTrack::Rect<int>;
+template class ByteTrack::Rect<float>;
+
+template ByteTrack::Rect<int> ByteTrack::generate_rect_by_tlbr<int>(const ByteTrack::Tlbr<int>&);
+template ByteTrack::Rect<float> ByteTrack::generate_rect_by_tlbr<float>(const ByteTrack::Tlbr<float>&);
+
+template ByteTrack::Rect<int> ByteTrack::generate_rect_by_xyah<int>(const ByteTrack::Xyah<int>&);
+template ByteTrack::Rect<float> ByteTrack::generate_rect_by_xyah<float>(const ByteTrack::Xyah<float>&);

modules/ANSMOT/ByteTrack/src/STrack.cpp

@@ -0,0 +1,191 @@
+#include "STrack.h"
+#include <cstddef>
+
+ByteTrack::STrack::STrack(const Rect<float>& rect,
+                         const float& score,
+                         int _class_id,
+                         float _left,
+                         float _top,
+                         float _right,
+                         float _bottom,
+                         std::string _object_id) :
+    kalman_filter_(),
+    mean_(),
+    covariance_(),
+    rect_(rect),
+    state_(STrackState::New),
+    is_activated_(false),
+    score_(score),
+    track_id_(0),
+    frame_id_(0),
+    start_frame_id_(0),
+    tracklet_len_(0),
+    class_id(_class_id),
+    left(_left),
+    right(_right),
+    top(_top),
+    bottom(_bottom),
+    object_id(_object_id)
+{
+    class_id_scores_[_class_id] = score;
+    detection_count_ = 1;
+    class_id_locked_ = false;
+}
+
+ByteTrack::STrack::~STrack()
+{
+}
+
+void ByteTrack::STrack::voteClassId(int new_class_id, float score)
+{
+    if (class_id_locked_) return; // class_id is locked, no further changes
+
+    class_id_scores_[new_class_id] += score;
+    detection_count_++;
+
+    // Pick the class_id with the highest accumulated score
+    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;
+
+    // Lock after enough detections
+    if (detection_count_ >= CLASS_ID_LOCK_FRAMES)
+    {
+        class_id_locked_ = true;
+    }
+}
+
+const ByteTrack::Rect<float>& ByteTrack::STrack::getRect() const
+{
+    return rect_;
+}
+
+const ByteTrack::STrackState& ByteTrack::STrack::getSTrackState() const
+{
+    return state_;
+}
+
+const bool& ByteTrack::STrack::isActivated() const
+{
+    return is_activated_;
+}
+const float& ByteTrack::STrack::getScore() const
+{
+    return score_;
+}
+
+const size_t& ByteTrack::STrack::getTrackId() const
+{
+    return track_id_;
+}
+
+const size_t& ByteTrack::STrack::getFrameId() const
+{
+    return frame_id_;
+}
+
+const size_t& ByteTrack::STrack::getStartFrameId() const
+{
+    return start_frame_id_;
+}
+
+const size_t& ByteTrack::STrack::getTrackletLength() const
+{
+    return tracklet_len_;
+}
+
+void ByteTrack::STrack::activate(const size_t& frame_id, const size_t& track_id)
+{
+    kalman_filter_.initiate(mean_, covariance_, rect_.getXyah());
+
+    updateRect();
+
+    state_ = STrackState::Tracked;
+    if (frame_id == 1)
+    {
+        is_activated_ = true;
+    }
+    track_id_ = track_id;
+    frame_id_ = frame_id;
+    start_frame_id_ = frame_id;
+    tracklet_len_ = 0;
+}
+
+void ByteTrack::STrack::reActivate(const STrack &new_track, const size_t &frame_id, const int &new_track_id)
+{
+    kalman_filter_.update(mean_, covariance_, new_track.getRect().getXyah());
+
+    updateRect();
+
+    state_ = STrackState::Tracked;
+    is_activated_ = true;
+    score_ = new_track.getScore();
+    if (0 <= new_track_id)
+    {
+        track_id_ = new_track_id;
+    }
+    frame_id_ = frame_id;
+    tracklet_len_ = 0;
+    this->score_ = new_track.score_;
+    voteClassId(new_track.class_id, new_track.score_); // score-weighted voting until locked
+    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 ByteTrack::STrack::predict()
+{
+    if (state_ != STrackState::Tracked)
+    {
+        mean_[7] = 0; // zero height velocity only; keep x/y drift for moving objects
+    }
+    kalman_filter_.predict(mean_, covariance_);
+}
+
+void ByteTrack::STrack::update(const STrack &new_track, const size_t &frame_id)
+{
+    kalman_filter_.update(mean_, covariance_, new_track.getRect().getXyah());
+
+    updateRect();
+
+    state_ = STrackState::Tracked;
+    is_activated_ = true;
+    score_ = new_track.getScore();
+    frame_id_ = frame_id;
+    tracklet_len_++;
+    this->score_ = new_track.score_;
+    voteClassId(new_track.class_id, new_track.score_); // score-weighted voting until locked
+    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 ByteTrack::STrack::markAsLost()
+{
+    state_ = STrackState::Lost;
+}
+
+void ByteTrack::STrack::markAsRemoved()
+{
+    state_ = STrackState::Removed;
+}
+
+void ByteTrack::STrack::updateRect()
+{
+    rect_.width() = mean_[2] * mean_[3];
+    rect_.height() = mean_[3];
+    rect_.x() = mean_[0] - rect_.width() / 2;
+    rect_.y() = mean_[1] - rect_.height() / 2;
+}

modules/ANSMOT/ByteTrack/src/lapjv.cpp

@@ -0,0 +1,338 @@
+#include "lapjv.h"
+#include <cstddef>
+#include <cstring>
+#include <stdexcept>
+
+#define LAPJV_CPP_NEW(x, t, n) if ((x = (t *)malloc(sizeof(t) * (n))) == 0) { return -1; }
+#define LAPJV_CPP_FREE(x) if (x != 0) { free(x); x = 0; }
+#define LAPJV_CPP_SWAP_INDICES(a, b) { int _temp_index = a; a = b; b = _temp_index; }
+
+namespace
+{
+constexpr size_t LARGE = 1000000;
+
+enum class fp_t {
+    FP_1 = 1,
+    FP_2 = 2,
+    FP_DYNAMIC = 3,
+};
+
+/** Column-reduction and reduction transfer for a dense cost matrix.
+*/
+int _ccrrt_dense(const size_t n, double *cost[],
+    int *free_rows, int *x, int *y, double *v)
+{
+    int n_free_rows;
+    bool *unique;
+
+    for (size_t i = 0; i < n; i++) {
+        x[i] = -1;
+        v[i] = LARGE;
+        y[i] = 0;
+    }
+    for (size_t i = 0; i < n; i++) {
+        for (size_t j = 0; j < n; j++) {
+            const double c = cost[i][j];
+            if (c < v[j]) {
+                v[j] = c;
+                y[j] = i;
+            }
+        }
+    }
+    LAPJV_CPP_NEW(unique, bool, n);
+    memset(unique, true, n);
+    {
+        int j = n;
+        do {
+            j--;
+            const int 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 (size_t i = 0; i < n; i++) {
+        if (x[i] < 0) {
+            free_rows[n_free_rows++] = i;
+        }
+        else if (unique[i]) {
+            const int j = x[i];
+            double min = LARGE;
+            for (size_t j2 = 0; j2 < n; j2++) {
+                if (j2 == (size_t)j) {
+                    continue;
+                }
+                const double c = cost[i][j2] - v[j2];
+                if (c < min) {
+                    min = c;
+                }
+            }
+            v[j] -= min;
+        }
+    }
+    LAPJV_CPP_FREE(unique);
+    return n_free_rows;
+}
+
+
+/** Augmenting row reduction for a dense cost matrix.
+ */
+int _carr_dense(
+    const size_t n, double *cost[],
+    const size_t n_free_rows,
+    int *free_rows, int *x, int *y, double *v)
+{
+    size_t current = 0;
+    int new_free_rows = 0;
+    size_t rr_cnt = 0;
+    while (current < n_free_rows) {
+        int i0;
+        int j1, j2;
+        double v1, v2, v1_new;
+        bool v1_lowers;
+
+        rr_cnt++;
+        const int free_i = free_rows[current++];
+        j1 = 0;
+        v1 = cost[free_i][0] - v[0];
+        j2 = -1;
+        v2 = LARGE;
+        for (size_t j = 1; j < n; j++) {
+            const double 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];
+        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 {
+            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.
+ */
+size_t _find_dense(const size_t n, size_t lo, double *d, int *cols, int *y)
+{
+    size_t hi = lo + 1;
+    double mind = d[cols[lo]];
+    for (size_t k = hi; k < n; k++) {
+        int 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 _scan_dense(const size_t n, double *cost[],
+    size_t *plo, size_t*phi,
+    double *d, int *cols, int *pred,
+    int *y, double *v)
+{
+    size_t lo = *plo;
+    size_t hi = *phi;
+    double h, cred_ij;
+
+    while (lo != hi) {
+        int j = cols[lo++];
+        const int i = y[j];
+        const double mind = d[j];
+        h = cost[i][j] - v[j] - mind;
+        // For all columns in TODO
+        for (size_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 find_path_dense(
+    const size_t n, double *cost[],
+    const int start_i,
+    int *y, double *v,
+    int *pred)
+{
+    size_t lo = 0, hi = 0;
+    int final_j = -1;
+    size_t n_ready = 0;
+    int *cols;
+    double *d;
+
+    LAPJV_CPP_NEW(cols, int, n);
+    LAPJV_CPP_NEW(d, double, n);
+
+    for (size_t i = 0; i < n; i++) {
+        cols[i] = i;
+        pred[i] = start_i;
+        d[i] = cost[start_i][i] - v[i];
+    }
+
+    while (final_j == -1) {
+        // No columns left on the SCAN list.
+        if (lo == hi) {
+            n_ready = lo;
+            hi = _find_dense(n, lo, d, cols, y);
+            for (size_t k = lo; k < hi; k++) {
+                const int j = cols[k];
+                if (y[j] < 0) {
+                    final_j = j;
+                }
+            }
+        }
+        if (final_j == -1) {
+            final_j = _scan_dense(
+                n, cost, &lo, &hi, d, cols, pred, y, v);
+        }
+    }
+
+    {
+        const double mind = d[cols[lo]];
+        for (size_t k = 0; k < n_ready; k++) {
+            const int j = cols[k];
+            v[j] += d[j] - mind;
+        }
+    }
+
+    LAPJV_CPP_FREE(cols);
+    LAPJV_CPP_FREE(d);
+
+    return final_j;
+}
+
+
+/** Augment for a dense cost matrix.
+ */
+int _ca_dense(
+    const size_t n, double *cost[],
+    const size_t n_free_rows,
+    int *free_rows, int *x, int *y, double *v)
+{
+    int *pred;
+
+    LAPJV_CPP_NEW(pred, int, n);
+
+    for (int *pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) {
+        int i = -1, j;
+        size_t k = 0;
+
+        j = find_path_dense(n, cost, *pfree_i, y, v, pred);
+        if (j < 0)
+        {
+            throw std::runtime_error("Error occured in _ca_dense(): j < 0");
+        }
+        if (j >= static_cast<int>(n))
+        {
+            throw std::runtime_error("Error occured in _ca_dense(): j >= n");
+        }
+        while (i != *pfree_i) {
+            i = pred[j];
+            y[j] = i;
+            LAPJV_CPP_SWAP_INDICES(j, x[i]);
+            k++;
+            if (k >= n) {
+                throw std::runtime_error("Error occured in _ca_dense(): k >= n");
+            }
+        }
+    }
+    LAPJV_CPP_FREE(pred);
+    return 0;
+}
+}
+
+/** Solve dense sparse LAP. */
+int ByteTrack::lapjv_internal(
+    const size_t n, double *cost[],
+    int *x, int *y)
+{
+    int ret;
+    int *free_rows;
+    double *v;
+
+    LAPJV_CPP_NEW(free_rows, int, n);
+    LAPJV_CPP_NEW(v, double, 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);
+    }
+    LAPJV_CPP_FREE(v);
+    LAPJV_CPP_FREE(free_rows);
+    return ret;
+}
+
+#undef LAPJV_CPP_NEW
+#undef LAPJV_CPP_FREE
+#undef LAPJV_CPP_SWAP_INDICES

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
+        );
+    };
+
+}

modules/ANSMOT/ByteTrackEigen/src/EigenBYTETracker.cpp

@@ -0,0 +1,566 @@
+#include "EigenBYTETracker.h"
+#include <algorithm>
+
+namespace ByteTrackEigen {
+	/**
+	 * @brief Constructor for the BYTETracker class.
+	 *
+	 * Initializes the BYTETracker with specific tracking thresholds and parameters.
+	 * This setup is crucial for the tracking algorithm to adapt to different frame rates
+	 * and tracking conditions.
+	 *
+	 * @param track_thresh The threshold for considering a detection as a valid track.
+	 *        Detections with a score higher than this threshold will be considered for tracking.
+	 * @param track_buffer The size of the buffer to store the history of tracks.
+	 *        This parameter is used to calculate the maximum time a track can be lost.
+	 * @param match_thresh The threshold used for matching detections to existing tracks.
+	 *        A higher value requires a closer match between detection and track.
+	 * @param frame_rate The frame rate of the video being processed.
+	 *        It is used to adjust the buffer size relative to the BASE_FRAME_RATE.
+	 */
+	BYTETracker::BYTETracker(float track_thresh, int track_buffer, float match_thresh, int frame_rate) :
+		track_thresh(track_thresh),
+		match_thresh(match_thresh),
+		frame_id(0),
+		det_thresh(track_thresh + MIN_KEEP_THRESH),
+		buffer_size(static_cast<int>(frame_rate / BASE_FRAME_RATE * track_buffer)),
+		max_time_lost(std::max(5, buffer_size)),
+		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),
+		kalman_filter(KalmanFilter())
+	{
+		// Initialize tracking lists
+		tracked_tracks.clear();
+		lost_tracks.clear();
+		removed_tracks.clear();
+
+		// Reset the BaseTrack counter to ensure track IDs are unique per instance
+		BaseTrack::reset_count();
+	}
+	void BYTETracker::update_parameters(int frameRate, int trackBuffer, double trackThreshold, double highThreshold, double matchThresold, bool autoFrameRate) {
+		track_thresh = trackThreshold;
+		match_thresh = matchThresold;
+		det_thresh = det_thresh + MIN_KEEP_THRESH;
+		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;
+		buffer_size = (static_cast<int>(frameRate / BASE_FRAME_RATE * trackBuffer));
+		max_time_lost = std::max(5, buffer_size);
+		kalman_filter = KalmanFilter();
+		tracked_tracks.clear();
+		lost_tracks.clear();
+		removed_tracks.clear();
+		// Reset the BaseTrack counter to ensure track IDs are unique per instance
+		BaseTrack::reset_count();
+	}
+
+	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;
+
+		// Ignore unreasonable gaps (likely pauses, not real frame intervals)
+		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_buffer_size = static_cast<int>(estimated_fps_ / BASE_FRAME_RATE * track_buffer_);
+			int new_max_time_lost = std::max(5, new_buffer_size);
+
+			double ratio = static_cast<double>(new_max_time_lost) / static_cast<double>(max_time_lost);
+			if (ratio > 1.1 || ratio < 0.9) {
+				buffer_size = new_buffer_size;
+				max_time_lost = new_max_time_lost;
+			}
+		}
+	}
+
+
+	/**
+	 * @brief Extracts Kalman bounding box tracks from detections.
+	 *
+	 * This method processes detection data and converts it into a vector of KalmanBBoxTrack objects.
+	 * Each detection is represented by a bounding box and associated confidence score.
+	 *
+	 * @param dets A matrix where each row represents a detected bounding box in the format (top-left and width-height coordinates).
+	 * @param scores_keep A vector of scores corresponding to the detections, indicating the confidence level of each detection.
+	 * @return std::vector<KalmanBBoxTrack> A vector of KalmanBBoxTrack objects representing the processed detections.
+	 */
+	std::vector<KalmanBBoxTrack> BYTETracker::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) {
+
+		std::vector<KalmanBBoxTrack> result;
+
+		// Iterate through each detection and create a KalmanBBoxTrack object
+		if (dets.rows() > 0) {
+			for (int i = 0; i < dets.rows(); ++i) {
+				Eigen::Vector4f tlwh = dets.row(i);
+				// Create a KalmanBBoxTrack object with the converted bounding box and corresponding score
+				result.push_back(KalmanBBoxTrack(std::vector<float>{ tlwh[0], tlwh[1], tlwh[2], tlwh[3]}, scores_keep[i], (int)clss_ids[i], obj_ids[i]));
+			}
+		}
+
+		return result;
+	}
+
+
+	/**
+	 * @brief Selects specific rows from a matrix based on given indices.
+	 *
+	 * This method is used to extract a subset of rows from a matrix, which is particularly
+	 * useful for processing detection data where only certain detections (rows) need to
+	 * be considered based on their scores or other criteria.
+	 *
+	 * @param matrix The input matrix from which rows will be selected.
+	 *        Each row in the matrix represents a distinct data point or detection.
+	 * @param indices A vector of indices indicating which rows should be selected from the matrix.
+	 *        The indices are zero-based and correspond to row numbers in the matrix.
+	 * @return Eigen::MatrixXf A new matrix containing only the rows specified by the indices.
+	 */
+	Eigen::MatrixXf BYTETracker::select_matrix_rows_by_indices(const Eigen::MatrixXf matrix, const std::vector<int> indices) {
+		// Create a new matrix to hold the selected rows
+		Eigen::MatrixXf result(indices.size(), matrix.cols());
+
+		// Iterate over the provided indices and copy the corresponding rows to the result matrix
+		for (int i = 0; i < indices.size(); ++i) {
+			result.row(i) = matrix.row(indices[i]);
+		}
+
+		return result;
+	}
+
+
+	std::vector<std::string> BYTETracker::select_vector_by_indices(const std::vector<std::string> obj_ids, const std::vector<int> indices) {
+		std::vector<std::string> result;
+		for (int i = 0; i < indices.size(); ++i) {
+			result.push_back(obj_ids.at(indices[i]));
+		}
+		return result;
+	}
+
+
+	/**
+	 * @brief Filters and partitions detections based on confidence scores.
+	 *
+	 * This method processes the detection results, separating them into two groups based on their confidence scores.
+	 * One group contains detections with high confidence scores (above track_thresh),
+	 * and the other group contains detections with lower confidence scores (between MIN_KEEP_THRESH and track_thresh).
+	 * This separation is essential for handling detections differently based on their likelihood of being accurate.
+	 *
+	 * @param output_results A matrix containing the detection results. Each row represents a detection,
+	 *        typically including bounding box coordinates and a confidence score.
+	 * @return std::pair<std::vector<KalmanBBoxTrack>, std::vector<KalmanBBoxTrack>> A pair of vectors of KalmanBBoxTrack objects.
+	 *         The first vector contains high-confidence detections, and the second vector contains lower-confidence detections.
+	 */
+	std::pair<std::vector<KalmanBBoxTrack>, std::vector<KalmanBBoxTrack>> BYTETracker::filter_and_partition_detections(const Eigen::MatrixXf& output_results, const std::vector<std::string> obj_ids) {
+		Eigen::VectorXf scores;
+		Eigen::MatrixXf bboxes;
+		Eigen::MatrixXf clsIs;
+
+		// Extract bounding box coordinates
+		bboxes = output_results.leftCols(4);
+
+		// Extract scores and bounding boxes from output results
+		// Assumes output_results contains bounding box coordinates followed by one score column
+		scores = output_results.col(4);
+		clsIs = output_results.col(5);
+
+		// Vectors to hold indices for high and low confidence detections
+		std::vector<int> indices_high_thresh, indices_low_thresh;
+
+		// Partition detections based on their scores
+		for (int i = 0; i < scores.size(); ++i) {
+			if (scores(i) > this->track_thresh) {
+				indices_high_thresh.push_back(i);
+			}
+			else if (MIN_KEEP_THRESH < scores(i) && scores(i) < this->track_thresh) {
+				indices_low_thresh.push_back(i);
+			}
+		}
+
+		// Extract high and low confidence detections as KalmanBBoxTrack objects
+		std::vector<KalmanBBoxTrack> detections = extract_kalman_bbox_tracks(select_matrix_rows_by_indices(bboxes, indices_high_thresh), 
+																			 select_matrix_rows_by_indices(scores, indices_high_thresh), 
+																			 select_matrix_rows_by_indices(clsIs, indices_high_thresh) , 
+																			 select_vector_by_indices(obj_ids, indices_high_thresh));
+		std::vector<KalmanBBoxTrack> detections_second = extract_kalman_bbox_tracks(select_matrix_rows_by_indices(bboxes, indices_low_thresh), 
+																			 select_matrix_rows_by_indices(scores, indices_low_thresh), 
+																			 select_matrix_rows_by_indices(clsIs, indices_low_thresh), 
+																		     select_vector_by_indices(obj_ids, indices_low_thresh));
+
+		return { detections, detections_second };
+	}
+
+	/**
+	 * @brief Partition tracks into active and inactive based on their activation status.
+	 *
+	 * This method categorizes the currently tracked objects into two groups: active and inactive.
+	 * Active tracks are those that have been successfully associated with a detection in the
+	 * current frame or recent frames, indicating they are still visible. Inactive tracks are those
+	 * that have not been matched with a detection recently, suggesting they may be occluded or lost.
+	 *
+	 * @return A pair of vectors containing shared pointers to KalmanBBoxTrack objects.
+	 *         The first vector contains inactive tracks, and the second contains active tracks.
+	 */
+	std::pair<std::vector<std::shared_ptr<KalmanBBoxTrack>>, std::vector<std::shared_ptr<KalmanBBoxTrack>>> BYTETracker::partition_tracks_by_activation() {
+		std::vector<std::shared_ptr<KalmanBBoxTrack>> inactive_tracked_tracks;
+		std::vector<std::shared_ptr<KalmanBBoxTrack>> active_tracked_tracks;
+
+		// Iterate through all tracked tracks and partition them based on their activation status
+		for (auto& track : this->tracked_tracks) {
+			if (track->get_is_activated()) {
+				active_tracked_tracks.push_back(track); // Add to active tracks if the track is activated
+			}
+			else {
+				inactive_tracked_tracks.push_back(track); // Otherwise, consider it as inactive
+			}
+		}
+
+		return { inactive_tracked_tracks, active_tracked_tracks };
+	}
+
+	/**
+	 * @brief Assigns tracks to detections based on a specified threshold.
+	 *
+	 * This method performs the critical task of associating existing tracks with new detections.
+	 * It uses the Intersection over Union (IoU) metric to measure the similarity between
+	 * each track and detection, and then applies the Hungarian algorithm (via the LinearAssignment class)
+	 * to find the optimal assignment between tracks and detections.
+	 *
+	 * @param tracks The vector of shared pointers to KalmanBBoxTrack objects representing the current tracks.
+	 * @param detections The vector of KalmanBBoxTrack objects representing the new detections for the current frame.
+	 * @param thresh The threshold for the IoU similarity measure. Pairs with an IoU below this threshold will not be assigned.
+	 * @return A tuple containing three elements:
+	 *         - A vector of pairs, where each pair contains the index of a track and the index of a matched detection.
+	 *         - A set of integers representing the indices of tracks that couldn't be paired with any detection.
+	 *         - A set of integers representing the indices of detections that couldn't be paired with any track.
+	 */
+	std::tuple<std::vector<std::pair<int, int>>, std::set<int>, std::set<int>> BYTETracker::assign_tracks_to_detections(
+		const std::vector<std::shared_ptr<KalmanBBoxTrack>> tracks,
+		const std::vector<KalmanBBoxTrack> detections,
+		double thresh
+	) {
+		// Convert shared pointers to instances for distance computation
+		std::vector<KalmanBBoxTrack> track_instances;
+		track_instances.reserve(tracks.size());
+		for (const auto& ptr : tracks) {
+			track_instances.push_back(*ptr);
+		}
+
+		// Compute the IoU distance matrix between tracks and detections
+		Eigen::MatrixXd distances = iou_distance(track_instances, detections);
+
+		// Perform linear assignment to find the best match between tracks and detections
+		return this->linear_assignment.linear_assignment(distances, thresh);
+	}
+
+	/**
+	 * @brief Updates tracks with the latest detection information.
+	 *
+	 * This method is responsible for updating the states of the tracks based on the new detections.
+	 * It involves matching detected objects to existing tracks and updating the track state accordingly.
+	 * The method also handles reactivating tracks that were previously lost and categorizing them as either
+	 * reacquired or newly activated.
+	 *
+	 * @param tracks A reference to a vector of shared pointers to KalmanBBoxTrack objects representing the current tracks.
+	 * @param detections A vector of KalmanBBoxTrack objects representing the new detections in the current frame.
+	 * @param track_detection_pair_indices A vector of pairs, where each pair contains the index of a track and
+	 *        the index of a detection that have been matched together.
+	 * @param reacquired_tracked_tracks A reference to a vector where reactivated tracks will be stored.
+	 * @param activated_tracks A reference to a vector where newly activated tracks will be stored.
+	 */
+	void BYTETracker::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
+	) {
+
+		for (const auto match : track_detection_pair_indices) {
+			if (tracks[match.first]->get_state() == TrackState::Tracked) {
+				// Update existing tracked track with the new detection
+				tracks[match.first]->update(detections[match.second], this->frame_id);
+				activated_tracks.push_back(tracks[match.first]);
+			}
+			else {
+				// Reactivate a track that was previously lost
+				tracks[match.first]->re_activate(detections[match.second], this->frame_id, false);
+				reacquired_tracked_tracks.push_back(tracks[match.first]);
+			}
+		}
+	}
+
+	/**
+	 * @brief Extracts active tracks from a given set of tracks.
+	 *
+	 * This method filters through a collection of tracks and extracts those that are actively
+	 * being tracked (i.e., their state is marked as 'Tracked'). It uses a set of indices to identify
+	 * unpaired tracks, ensuring that only relevant and currently tracked objects are considered.
+	 * This function is essential in the tracking process, where it's crucial to distinguish between
+	 * actively tracked, lost, and new objects.
+	 *
+	 * @param tracks A vector of shared pointers to KalmanBBoxTrack objects, representing all currently known tracks.
+	 * @param unpaired_track_indices A set of integers representing the indices of tracks that have not been paired
+	 *        with a detection in the current frame. This helps in identifying which tracks are still active.
+	 * @return std::vector<std::shared_ptr<KalmanBBoxTrack>> A vector of shared pointers to KalmanBBoxTrack objects
+	 *         that are actively being tracked.
+	 */
+	std::vector<std::shared_ptr<KalmanBBoxTrack>> BYTETracker::extract_active_tracks(
+		const std::vector<std::shared_ptr<KalmanBBoxTrack>>& tracks,
+		std::set<int> unpaired_track_indices
+	) {
+		std::vector<std::shared_ptr<KalmanBBoxTrack>> currently_tracked_tracks;
+		for (int i : unpaired_track_indices) {
+			if (i < tracks.size() && tracks[i]->get_state() == TrackState::Tracked) {
+				currently_tracked_tracks.push_back(tracks[i]);
+			}
+		}
+		return currently_tracked_tracks;
+	}
+
+	/**
+	 * @brief Flags unpaired tracks as lost.
+	 *
+	 * This method takes a list of currently tracked tracks and a set of unpaired track indices,
+	 * marking those unpaired tracks as 'lost'. It helps in updating the state of tracks that
+	 * are no longer detected in the current frame. This is an essential step in the tracking
+	 * process as it assists in handling temporary occlusions or missed detections.
+	 *
+	 * @param currently_tracked_tracks A vector of shared pointers to KalmanBBoxTrack objects representing
+	 *        the currently active tracks. These are the tracks that are being updated in the current frame.
+	 * @param lost_tracks A vector to which lost tracks will be added. These tracks were not matched with
+	 *        any current detection and are thus considered lost.
+	 * @param unpaired_track_indices A set of indices pointing to tracks in currently_tracked_tracks
+	 *        that have not been paired with any detection in the current frame.
+	 */
+	void BYTETracker::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
+	) {
+		for (int i : unpaired_track_indices) {
+			// Check if the index is within bounds and the track state is not already lost
+			if (i < currently_tracked_tracks.size() && currently_tracked_tracks[i]->get_state() != TrackState::Lost) {
+				// Mark the track as lost and add it to the lost_tracks vector
+				currently_tracked_tracks[i]->mark_lost();
+				lost_tracks.push_back(currently_tracked_tracks[i]);
+			}
+		}
+	}
+
+	/**
+	 * @brief Prunes and merges tracked tracks.
+	 *
+	 * This method updates the state of the tracked tracks by pruning tracks that are no longer in
+	 * the 'Tracked' state and merging the list of tracks with newly activated and reacquired tracks.
+	 * It ensures that the tracked_tracks list always contains the most up-to-date and relevant tracks.
+	 *
+	 * @param reacquired_tracked_tracks A vector of tracks that have been reacquired after being lost.
+	 *        These tracks are reintegrated into the main tracking list.
+	 * @param activated_tracks A vector of newly activated tracks that need to be added to the main tracking list.
+	 */
+	void BYTETracker::prune_and_merge_tracked_tracks(
+		std::vector<std::shared_ptr<KalmanBBoxTrack>>& reacquired_tracked_tracks,
+		std::vector<std::shared_ptr<KalmanBBoxTrack>>& activated_tracks
+	) {
+		// Update tracked_tracks to only contain tracks that are in the Tracked state
+		std::vector<std::shared_ptr<KalmanBBoxTrack>> filtered_tracked_tracks;
+		for (std::shared_ptr<KalmanBBoxTrack> track : this->tracked_tracks) {
+			if (track->get_state() == TrackState::Tracked) {
+				filtered_tracked_tracks.push_back(track);
+			}
+		}
+		this->tracked_tracks = filtered_tracked_tracks;
+
+		// Update tracked_tracks by merging with activated and reacquired tracks
+		this->tracked_tracks = join_tracks(this->tracked_tracks, activated_tracks);
+		this->tracked_tracks = join_tracks(this->tracked_tracks, reacquired_tracked_tracks);
+	}
+
+	/**
+	 * @brief Handles the updating of lost and removed track lists.
+	 *
+	 * This method updates the internal lists of lost and removed tracks based on the current frame.
+	 * Tracks that have been lost for a duration longer than the maximum allowable time (max_time_lost)
+	 * are marked as removed. The method also ensures that the lists of lost and removed tracks are
+	 * properly maintained and updated, considering the current state of tracked and lost tracks.
+	 *
+	 * @param removed_tracks A reference to a vector of shared pointers to KalmanBBoxTrack, representing
+	 *        the tracks that are currently marked as removed.
+	 * @param lost_tracks A reference to a vector of shared pointers to KalmanBBoxTrack, representing
+	 *        the tracks that are currently marked as lost.
+	 */
+	void BYTETracker::handle_lost_and_removed_tracks(
+		std::vector<std::shared_ptr<KalmanBBoxTrack>>& removed_tracks,
+		std::vector<std::shared_ptr<KalmanBBoxTrack>>& lost_tracks
+	) {
+		// Iterate over lost tracks and mark them as removed if they have been lost for too long
+		for (std::shared_ptr<KalmanBBoxTrack> track : this->lost_tracks) {
+			if (this->frame_id - track->end_frame() > this->max_time_lost) {
+				track->mark_removed();
+				removed_tracks.push_back(track);
+			}
+		}
+
+		// Update the lost_tracks list by removing tracks that are currently being tracked
+		// or have been marked as removed
+		this->lost_tracks = sub_tracks(this->lost_tracks, this->tracked_tracks);
+		this->lost_tracks.insert(this->lost_tracks.end(), lost_tracks.begin(), lost_tracks.end());
+		this->lost_tracks = sub_tracks(this->lost_tracks, this->removed_tracks);
+
+		// Clean up removed tracks
+		this->removed_tracks.clear();
+	}
+
+	/**
+	 * @brief Processes detections for a single frame and updates track states.
+	 *
+	 * This method is the core of the BYTETracker class, where detections in each frame
+	 * are processed to update the state of the tracks. It involves several key steps:
+	 * - Partitioning detections based on confidence thresholds.
+	 * - Predicting the state of existing tracks.
+	 * - Matching detections to existing tracks and updating their states.
+	 * - Handling unpaired tracks and detections.
+	 * - Managing lost and new tracks.
+	 *
+	 * @param output_results A matrix containing detection data for the current frame.
+	 *        Each row represents a detection and includes bounding box coordinates
+	 * 		  in the format (top-left and width-height coordinates) and a detection score.
+	 * @return std::vector<KalmanBBoxTrack> A vector of KalmanBBoxTrack objects representing
+	 *         the updated state of each track after processing the current frame.
+	 */
+	std::vector<KalmanBBoxTrack> BYTETracker::update(const Eigen::MatrixXf& output_results, const std::vector<std::string> obj_ids) {
+		// Auto-estimate frame rate from update() call timing
+		if (auto_frame_rate_) {
+			estimateFrameRate();
+		}
+
+		// Increment the frame counter
+		this->frame_id += 1;
+
+		// Initialize containers for various track states
+		std::vector<std::shared_ptr<KalmanBBoxTrack>> reacquired_tracked_tracks, activated_tracks, lost_tracks, removed_tracks;
+
+		// Filter and partition detections based on confidence thresholds
+		auto [high_confidence_detections, lower_confidence_detections] = filter_and_partition_detections(output_results, obj_ids);
+
+		// Partition existing tracks into active and inactive ones
+		auto [inactive_tracked_tracks, active_tracked_tracks] = partition_tracks_by_activation();
+
+		// Prepare track pool for matching and update state prediction for each track
+		std::vector<std::shared_ptr<KalmanBBoxTrack>> track_pool = join_tracks(active_tracked_tracks, this->lost_tracks);
+
+		// 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++) {
+			KalmanBBoxTrack::multi_predict(track_pool);
+		}
+
+		// Adaptive matching: relax threshold during frame skips
+		float effective_match_thresh = this->match_thresh;
+		if (num_predicts > 1) {
+			effective_match_thresh = std::min(this->match_thresh + 0.005f * (num_predicts - 1), 0.99f);
+		}
+
+		// Match tracks to high confidence detections and update their states
+		auto [track_detection_pair_indices, unpaired_track_indices, unpaired_detection_indices] = assign_tracks_to_detections(track_pool, high_confidence_detections, effective_match_thresh);
+		update_tracks_from_detections(track_pool, high_confidence_detections, track_detection_pair_indices, reacquired_tracked_tracks, activated_tracks);
+
+		// Extract currently tracked tracks from the pool
+		auto currently_tracked_tracks = extract_active_tracks(track_pool, unpaired_track_indices);
+
+		// Match currently tracked tracks to lower confidence detections and update states
+		std::tie(track_detection_pair_indices, unpaired_track_indices, std::ignore) = assign_tracks_to_detections(currently_tracked_tracks, lower_confidence_detections, LOWER_CONFIDENCE_MATCHING_THRESHOLD);
+		update_tracks_from_detections(currently_tracked_tracks, lower_confidence_detections, track_detection_pair_indices, reacquired_tracked_tracks, activated_tracks);
+
+		// Flag unpaired tracks as lost
+		flag_unpaired_tracks_as_lost(currently_tracked_tracks, lost_tracks, unpaired_track_indices);
+
+		// Update unconfirmed tracks
+		std::vector<KalmanBBoxTrack> filtered_detections;
+		for (int i : unpaired_detection_indices) {
+			filtered_detections.push_back(high_confidence_detections[i]);
+		}
+		high_confidence_detections = filtered_detections;
+
+		// Match inactive tracks to high confidence detections for reactivation
+		std::tie(track_detection_pair_indices, unpaired_track_indices, unpaired_detection_indices) = assign_tracks_to_detections(inactive_tracked_tracks, high_confidence_detections, ACTIVATION_MATCHING_THRESHOLD);
+		for (auto [track_idx, det_idx] : track_detection_pair_indices) {
+			inactive_tracked_tracks[track_idx]->update(high_confidence_detections[det_idx], this->frame_id);
+			activated_tracks.push_back(inactive_tracked_tracks[track_idx]);
+		}
+
+		// Handle tracks that remain inactive
+		for (int i : unpaired_track_indices) {
+			inactive_tracked_tracks[i]->mark_removed();
+			removed_tracks.push_back(inactive_tracked_tracks[i]);
+		}
+
+		// Handle new tracks from unpaired detections
+		for (int i : unpaired_detection_indices) {
+			if (high_confidence_detections[i].get_score() >= this->det_thresh) {
+				high_confidence_detections[i].activate(this->kalman_filter, this->frame_id);
+				activated_tracks.push_back(std::make_shared<KalmanBBoxTrack>(high_confidence_detections[i]));
+			}
+		}
+
+		// Merge and prune tracked tracks
+		prune_and_merge_tracked_tracks(reacquired_tracked_tracks, activated_tracks);
+
+		// Handle lost tracks and remove outdated ones
+		handle_lost_and_removed_tracks(removed_tracks, lost_tracks);
+
+		// Consolidate and clean up track lists
+		this->removed_tracks.insert(this->removed_tracks.end(), removed_tracks.begin(), removed_tracks.end());
+
+		// Remove duplicate tracks
+		std::tie(this->tracked_tracks, this->lost_tracks) = remove_duplicate_tracks(this->tracked_tracks, this->lost_tracks);
+
+		// Prepare the list of tracks to be returned
+		std::vector<KalmanBBoxTrack> return_tracks;
+		for (std::shared_ptr<KalmanBBoxTrack> track : this->tracked_tracks) {
+			
+			return_tracks.push_back(*track);
+			//if (track->get_is_activated()) {
+			//	return_tracks.push_back(*track);
+			//}
+		}
+		return return_tracks;
+	}
+
+}

modules/ANSMOT/ByteTrackEigen/src/EigenBaseTrack.cpp

@@ -0,0 +1,97 @@
+#include "EigenBaseTrack.h"
+namespace ByteTrackEigen {
+    // Initialize the static member variable _count to 0.
+    int BaseTrack::_count = 0;
+
+    // Default constructor: Initializes a new track with default values.
+    BaseTrack::BaseTrack() :
+        track_id(0),
+        is_activated(false),
+        state(TrackState::New),
+        curr_feature(0),
+        score(0),
+        start_frame(0),
+        frame_id(0),
+        time_since_update(0),
+        location(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity())
+    {}
+
+    // Constructor with score: Initializes a new track with a specified score.
+    BaseTrack::BaseTrack(float score) :
+        track_id(0),
+        is_activated(false),
+        state(TrackState::New),
+        curr_feature(0),
+        score(score),
+        start_frame(0),
+        frame_id(0),
+        time_since_update(0),
+        location(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity())
+    {}
+
+    // end_frame: Returns the current frame ID of the track.
+    int BaseTrack::end_frame() const {
+        return this->frame_id;
+    }
+
+    // next_id: Increments and returns the next unique track ID.
+    int BaseTrack::next_id() {
+        return ++_count;
+    }
+
+    // activate: Placeholder for activating the track. Throws a runtime error if not implemented.
+    void BaseTrack::activate() {
+        throw std::runtime_error("NotImplementedError");
+    }
+
+    // predict: Placeholder for predicting the next state of the track. Throws a runtime error if not implemented.
+    void BaseTrack::predict() {
+        throw std::runtime_error("NotImplementedError");
+    }
+
+    // update: Placeholder for updating the track. Throws a runtime error if not implemented.
+    void BaseTrack::update() {
+        throw std::runtime_error("NotImplementedError");
+    }
+
+    // mark_lost: Marks the track as lost.
+    void BaseTrack::mark_lost() {
+        this->state = TrackState::Lost;
+    }
+
+    // mark_removed: Marks the track as removed.
+    void BaseTrack::mark_removed() {
+        this->state = TrackState::Removed;
+    }
+
+    // resetCount: Resets the static track ID counter to 0.
+    void BaseTrack::reset_count() {
+        _count = 0;
+    }
+
+    // Getter functions below provide safe access to the track's properties.
+
+    bool BaseTrack::get_is_activated() const {
+        return this->is_activated;
+    }
+
+    TrackState BaseTrack::get_state() const {
+        return this->state;
+    }
+
+    float BaseTrack::get_score() const {
+        return this->score;
+    }
+
+    int BaseTrack::get_start_frame() const {
+        return this->start_frame;
+    }
+
+    int BaseTrack::get_frame_id() const {
+        return this->frame_id;
+    }
+
+    int BaseTrack::get_track_id() const {
+        return this->track_id;
+    }
+}

modules/ANSMOT/ByteTrackEigen/src/EigenBoundingBoxIoUMatching.cpp

@@ -0,0 +1,134 @@
+#include "EigenBoundingBoxIoUMatching.h"
+namespace ByteTrackEigen {
+    /**
+     * Calculates the Intersection over Union (IoU) for pairs of bounding boxes.
+     *
+     * @param track_boxes Matrix of tracking boxes, expected to be in the format (N, 4).
+     * @param detection_boxes Matrix of detection boxes, expected to be in the format (M, 4).
+     * @return Matrix of IoU values, with dimensions (N, M).
+     */
+    Eigen::MatrixXd box_iou_batch(const Eigen::MatrixXd& track_boxes, const Eigen::MatrixXd& detection_boxes) {
+        // Validate the shape of input matrices
+        if (track_boxes.cols() != 4 || detection_boxes.cols() != 4) {
+            throw std::invalid_argument("Input matrices must have 4 columns each.");
+        }
+
+        int N = (int)track_boxes.rows();
+        int M = (int)detection_boxes.rows();
+
+        // Calculate areas of the track boxes and detection boxes
+        Eigen::VectorXd track_areas = (track_boxes.col(2) - track_boxes.col(0))
+            .cwiseProduct(track_boxes.col(3) - track_boxes.col(1));
+        Eigen::VectorXd detection_areas = (detection_boxes.col(2) - detection_boxes.col(0))
+            .cwiseProduct(detection_boxes.col(3) - detection_boxes.col(1));
+
+        // Initialize the IoU matrix
+        Eigen::MatrixXd iou_matrix(N, M);
+
+        // Compute IoU for each pair of boxes
+        for (int i = 0; i < N; ++i) {
+            for (int j = 0; j < M; ++j) {
+                // Calculate intersection box coordinates
+                double inter_x_min = std::max(track_boxes(i, 0), detection_boxes(j, 0));
+                double inter_y_min = std::max(track_boxes(i, 1), detection_boxes(j, 1));
+                double inter_x_max = std::min(track_boxes(i, 2), detection_boxes(j, 2));
+                double inter_y_max = std::min(track_boxes(i, 3), detection_boxes(j, 3));
+
+                // Compute intersection area
+                double inter_width = std::max(inter_x_max - inter_x_min, 0.0);
+                double inter_height = std::max(inter_y_max - inter_y_min, 0.0);
+                double inter_area = inter_width * inter_height;
+
+                // Calculate IoU
+                iou_matrix(i, j) = inter_area / (track_areas(i) + detection_areas(j) - inter_area);
+            }
+        }
+
+        return iou_matrix;
+    }
+
+    /**
+     * Computes the IoU-based distance matrix for tracking purposes.
+     *
+     * @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
+    ) {
+        size_t m = track_list_a.size();
+        size_t n = track_list_b.size();
+
+        // Extract bounding boxes from tracks
+        Eigen::MatrixXd tlbr_list_a(m, 4);
+        for (size_t i = 0; i < m; i++) {
+            tlbr_list_a.row(i) = track_list_a[i].tlbr();
+        }
+
+        Eigen::MatrixXd tlbr_list_b(n, 4);
+        for (size_t i = 0; i < n; i++) {
+            tlbr_list_b.row(i) = track_list_b[i].tlbr();
+        }
+
+        // Calculate IoUs and form the cost matrix
+        Eigen::MatrixXd ious;
+        if (tlbr_list_a.rows() == 0 || tlbr_list_b.rows() == 0) {
+            ious = Eigen::MatrixXd::Zero(tlbr_list_a.rows(), tlbr_list_b.rows());
+        }
+        else {
+            ious = box_iou_batch(tlbr_list_a, tlbr_list_b);
+        }
+        Eigen::MatrixXd cost_matrix = Eigen::MatrixXd::Ones(m, n) - ious;
+
+        return cost_matrix;
+    }
+
+    /**
+     * Matches detections to tracks based on the highest IoU.
+     *
+     * @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
+    ) {
+        size_t m = tracks.size();
+        size_t n = tlbr_boxes.rows();
+
+        // Clone the input track_ids to operate on
+        std::vector<int> track_ids(n, -1);
+
+        // Extract bounding boxes from tracks
+        Eigen::MatrixXd track_boxes(m, 4);
+        for (size_t i = 0; i < m; i++) {
+            track_boxes.row(i) = tracks[i].tlbr();
+        }
+
+        // Calculate IoU matrix
+        Eigen::MatrixXd iou = box_iou_batch(track_boxes, tlbr_boxes);
+
+        // Match detections with tracks based on IoU
+        for (size_t i = 0; i < m; i++) {
+            int idx_max = -1;
+            double max_val = 0;
+            for (size_t j = 0; j < n; j++) {
+                if (iou(i, j) > max_val) {
+                    max_val = iou(i, j);
+                    idx_max = (int)j;
+                }
+            }
+
+            // Assign track IDs based on highest IoU
+            if (max_val > 0) {
+                track_ids[idx_max] = tracks[i].get_track_id();
+            }
+        }
+
+        return track_ids;
+    }
+
+}

modules/ANSMOT/ByteTrackEigen/src/EigenBoundingBoxTrackUtils.cpp

@@ -0,0 +1,143 @@
+#include "EigenBoundingBoxTrackUtils.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) {
+
+        std::map<int, std::shared_ptr<KalmanBBoxTrack>> unique_tracks;
+
+        // Populate the map with tracks from track_list_a
+        for (auto& track : track_list_a) {
+            unique_tracks[track->get_track_id()] = track;
+        }
+
+        // Insert or overwrite with tracks from track_list_b
+        for (auto& track : track_list_b) {
+            unique_tracks[track->get_track_id()] = track;
+        }
+
+        // Convert the unique tracks in the map to a vector
+        std::vector<std::shared_ptr<KalmanBBoxTrack>> result;
+        for (auto [key, value] : unique_tracks) {
+            result.push_back(value);
+        }
+
+        return result;
+    }
+
+    /**
+     * @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) {
+
+        std::unordered_set<int> track_ids_b;
+
+        // Populate set with track IDs from track_list_b for quick lookup
+        for (const auto& track : track_list_b) {
+            track_ids_b.insert(track->get_track_id());
+        }
+
+        // Collect tracks from track_list_a not found in track_list_b
+        std::vector<std::shared_ptr<KalmanBBoxTrack>> result;
+        for (const auto& track : track_list_a) {
+            if (track_ids_b.find(track->get_track_id()) == track_ids_b.end()) {
+                result.push_back(track);
+            }
+        }
+
+        return result;
+    }
+
+    /**
+     * @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) {
+
+        // Creating instance vectors to store de-referenced shared pointers from the input lists.
+        // This is necessary for the IOU distance calculation.
+        std::vector<KalmanBBoxTrack> s_tracks_a_instances;
+        s_tracks_a_instances.reserve(track_list_a.size());
+        for (const auto& ptr : track_list_a) {
+            s_tracks_a_instances.push_back(*ptr);
+        }
+
+        std::vector<KalmanBBoxTrack> s_tracks_b_instances;
+        s_tracks_b_instances.reserve(track_list_b.size());
+        for (const auto& ptr : track_list_b) {
+            s_tracks_b_instances.push_back(*ptr);
+        }
+
+        // Calculating pairwise IOU distance between tracks in both lists.
+        Eigen::MatrixXd pairwise_distance = iou_distance(s_tracks_a_instances, s_tracks_b_instances);
+
+        // Sets to track indices of duplicates in both lists.
+        std::unordered_set<int> duplicates_a, duplicates_b;
+
+        // Loop through the matrix of pairwise distances to identify duplicates.
+        for (int i = 0; i < pairwise_distance.rows(); ++i) {
+            for (int j = 0; j < pairwise_distance.cols(); ++j) {
+                // If IOU distance is below the threshold, consider the tracks as duplicates.
+                if (pairwise_distance(i, j) < 0.15) {
+                    // Calculate track age to determine which track to keep.
+                    int time_a = track_list_a[i]->get_frame_id() - track_list_a[i]->get_start_frame();
+                    int time_b = track_list_b[j]->get_frame_id() - track_list_b[j]->get_start_frame();
+
+                    // Retain the older track and mark the newer one as a duplicate.
+                    if (time_a > time_b) {
+                        duplicates_b.insert(j);
+                    }
+                    else {
+                        duplicates_a.insert(i);
+                    }
+                }
+            }
+        }
+
+        // Constructing the result lists, excluding the identified duplicates.
+        std::vector<std::shared_ptr<KalmanBBoxTrack>> result_a, result_b;
+        for (int i = 0; i < track_list_a.size(); ++i) {
+            if (duplicates_a.find(i) == duplicates_a.end()) {
+                result_a.push_back(track_list_a[i]);
+            }
+        }
+        for (int j = 0; j < track_list_b.size(); ++j) {
+            if (duplicates_b.find(j) == duplicates_b.end()) {
+                result_b.push_back(track_list_b[j]);
+            }
+        }
+
+        return { result_a, result_b };
+    }
+
+
+}

modules/ANSMOT/ByteTrackEigen/src/EigenHungarianAlgorithm.cpp

@@ -0,0 +1,343 @@
+
+#include "EigenHungarianAlgorithm.h"
+namespace ByteTrackEigen {
+    /// <summary>
+    /// Default constructor for the HungarianAlgorithmEigen class.
+    /// </summary>
+    HungarianAlgorithmEigen::HungarianAlgorithmEigen() {}
+
+    /// <summary>
+    /// Destructor for the HungarianAlgorithmEigen class.
+    /// </summary>
+    HungarianAlgorithmEigen::~HungarianAlgorithmEigen() {}
+
+    /// <summary>
+    /// Finds the index of the first starred zero in a specified column.
+    /// </summary>
+    /// <param name="col">The column index to search within. Must be in the range of [0, number of columns in star_matrix).</param>
+    /// <returns>The row index of the starred zero, or NOT_FOUND_VALUE if no starred zero is found.</returns>
+    Eigen::Index HungarianAlgorithmEigen::find_star_in_column(int col)
+    {
+        for (Eigen::Index i = 0; i < star_matrix.rows(); ++i) {
+            if (star_matrix(i, col)) {
+                return i; // Return the row index where the star was found.
+            }
+        }
+        return NOT_FOUND_VALUE;
+    }
+
+    /// <summary>
+    /// Finds the index of the first primed zero in a specified row.
+    /// </summary>
+    /// <param name="row">The row index to search within.</param>
+    /// <returns>The column index of the primed zero, or NOT_FOUND_VALUE if no primed zero is found.</returns>
+    Eigen::Index HungarianAlgorithmEigen::find_prime_in_row(int row)
+    {
+        for (Eigen::Index j = 0; j < prime_matrix.cols(); ++j) {
+            if (prime_matrix(row, j)) {
+                return j; // Return the column index where the prime was found.
+            }
+        }
+        return NOT_FOUND_VALUE;
+    }
+
+    /// <summary>
+    /// Updates the star and prime matrices based on the given row and column.
+    /// It searches for a chain of alternating primed and starred zeros and
+    /// rebuilds the star matrix to include the new starred zero at the provided location.
+    /// </summary>
+    /// <param name="row">The row index of the primed zero to start with.</param>
+    /// <param name="col">The column index of the primed zero to start with.</param>
+    void HungarianAlgorithmEigen::update_star_and_prime_matrices(int row, int col)
+    {
+        // Make a working copy of star_matrix
+        new_star_matrix = star_matrix;
+
+        // Star the current zero.
+        new_star_matrix(row, col) = true;
+
+        // Loop to update stars and primes based on the newly starred zero.
+        while (true)
+        {
+            Eigen::Index star_row, prime_col;
+
+            // If there are no starred zeros in the current column, break the loop.
+            if (!star_matrix.col(col).any()) {
+                break;
+            }
+
+            // Find the starred zero in the column and unstar it.
+            star_row = find_star_in_column(col);
+            new_star_matrix(star_row, col) = false;
+
+            // If there are no primed zeros in the row of the starred zero, break the loop.
+            if (!prime_matrix.row(star_row).any()) {
+                break;
+            }
+
+            // Find the primed zero in the row and star it.
+            prime_col = find_prime_in_row(star_row);
+            new_star_matrix(star_row, prime_col) = true;
+
+            // Move to the column of the newly starred zero.
+            col = prime_col;
+        }
+
+        // Apply the changes from the working copy to the actual star_matrix.
+        star_matrix = new_star_matrix;
+        // Clear the prime_matrix and covered rows for the next steps.
+        prime_matrix.setConstant(false);
+        covered_rows.setConstant(false);
+    }
+
+    /// <summary>
+    /// Reduces the distance matrix by the smallest uncovered value and adjusts the
+    /// matrix to prepare for further steps of the Hungarian algorithm.
+    /// </summary>
+    void HungarianAlgorithmEigen::reduce_matrix_by_minimum_value()
+    {
+        // Determine the dimensions for operations.
+        int num_rows = covered_rows.size();
+        int num_columns = covered_columns.size();
+
+        // Create a masked array with high values for covered rows/columns.
+        Eigen::ArrayXXd masked_array = dist_matrix.array() + DBL_MAX
+            * (covered_rows.replicate(1, num_columns).cast<double>() + covered_columns.transpose().replicate(num_rows, 1).cast<double>());
+
+        // Find the minimum value in the uncovered elements.
+        double min_uncovered_value = masked_array.minCoeff();
+
+        // Adjust the matrix values based on uncovered rows and columns.
+        Eigen::ArrayXXd row_adjustments = covered_rows.cast<double>() * min_uncovered_value;
+        Eigen::ArrayXXd col_adjustments = (1.0 - covered_columns.cast<double>()) * min_uncovered_value;
+        dist_matrix += (row_adjustments.replicate(1, num_columns) - col_adjustments.transpose().replicate(num_rows, 1)).matrix();
+    }
+
+    /// <summary>
+    /// Covers columns that lack a starred zero and performs adjustments to the
+    /// distance matrix, primed and starred zeros until all columns are covered.
+    /// </summary>
+    void HungarianAlgorithmEigen::cover_columns_lacking_stars()
+    {
+        // Retrieve the dimensions of the matrix.
+        int num_rows = dist_matrix.rows();
+        int num_columns = dist_matrix.cols();
+
+        // Flag to check if uncovered zeros are found in the iteration.
+        bool zeros_found = true;
+        while (zeros_found)
+        {
+            zeros_found = false;
+            for (int col = 0; col < num_columns; col++)
+            {
+                // Skip already covered columns.
+                if (covered_columns(col)) continue;
+
+                // Identify uncovered zeros in the current column.
+                Eigen::Array<bool, Eigen::Dynamic, 1> uncovered_zeros_in_column = (dist_matrix.col(col).array().abs() < DBL_EPSILON) && !covered_rows;
+
+                Eigen::Index row;
+                // Check if there is an uncovered zero in the column.
+                double max_in_uncovered_zeros = uncovered_zeros_in_column.cast<double>().maxCoeff(&row);
+
+                // If an uncovered zero is found, prime it.
+                if (max_in_uncovered_zeros == 1.0)
+                {
+                    prime_matrix(row, col) = true;
+
+                    // Check for a star in the same row.
+                    Eigen::Index star_col;
+                    bool has_star = star_matrix.row(row).maxCoeff(&star_col);
+                    if (!has_star)
+                    {
+                        // If no star is found, update the star and prime matrices, and covered columns.
+                        update_star_and_prime_matrices(row, col);
+                        covered_columns = (star_matrix.colwise().any()).transpose();
+                        return;
+                    }
+                    else
+                    {
+                        // If a star is found, cover the row and uncover the column where the star is found.
+                        covered_rows(row) = true;
+                        covered_columns(star_col) = false;
+                        zeros_found = true; // Continue the while loop.
+                        break;
+                    }
+                }
+            }
+        }
+
+        // If no more uncovered zeros are found, reduce the matrix and try again.
+        reduce_matrix_by_minimum_value();
+        cover_columns_lacking_stars();
+    }
+
+    /// <summary>
+    /// Executes the main steps of the Hungarian algorithm. It reduces the distance
+    /// matrix, stars zeros to form an initial feasible solution, and iteratively
+    /// improves the solution until an optimal assignment is found.
+    /// </summary>
+    void HungarianAlgorithmEigen::execute_hungarian_algorithm()
+    {
+        // If there are fewer rows than columns, we operate on rows first.
+        if (dist_matrix.rows() <= dist_matrix.cols())
+        {
+            for (int row = 0; row < dist_matrix.rows(); ++row)
+            {
+                // Subtract the minimum value in the row to create zeros.
+                double min_value = dist_matrix.row(row).minCoeff();
+                dist_matrix.row(row).array() -= min_value;
+
+                // Identify zeros that are not covered by any line (row or column).
+                Eigen::ArrayXd current_row = dist_matrix.row(row).array();
+                Eigen::Array<bool, Eigen::Dynamic, 1> uncovered_zeros = (current_row.abs() < DBL_EPSILON) && !covered_columns;
+
+                Eigen::Index col;
+                // Star a zero if it is the only uncovered zero in its row.
+                double max_in_uncovered_zeros = uncovered_zeros.cast<double>().maxCoeff(&col);
+                if (max_in_uncovered_zeros == 1.0)
+                {
+                    star_matrix(row, col) = true;
+                    covered_columns(col) = true; // Cover the column containing the starred zero.
+                }
+            }
+        }
+        else // If there are more rows than columns, we operate on columns first.
+        {
+            for (int col = 0; col < dist_matrix.cols(); ++col)
+            {
+                // Subtract the minimum value in the column to create zeros.
+                double min_value = dist_matrix.col(col).minCoeff();
+                dist_matrix.col(col).array() -= min_value;
+
+                // Identify zeros that are not covered by any line.
+                Eigen::ArrayXd current_column = dist_matrix.col(col).array();
+                Eigen::Array<bool, Eigen::Dynamic, 1> uncovered_zeros = (current_column.abs() < DBL_EPSILON) && !covered_rows;
+
+                Eigen::Index row;
+                // Star a zero if it is the only uncovered zero in its column.
+                double max_in_uncovered_zeros = uncovered_zeros.cast<double>().maxCoeff(&row);
+                if (max_in_uncovered_zeros == 1.0)
+                {
+                    star_matrix(row, col) = true;
+                    covered_columns(col) = true;
+                    covered_rows(row) = true; // Temporarily cover the row to avoid multiple stars in one row.
+                }
+            }
+
+            // Uncover all rows for the next step.
+            for (int row = 0; row < dist_matrix.rows(); ++row)
+            {
+                covered_rows(row) = false;
+            }
+        }
+
+        // If not all columns are covered, move to the next step to cover them.
+        if (covered_columns.count() != std::min(dist_matrix.rows(), dist_matrix.cols()))
+        {
+            cover_columns_lacking_stars();
+        }
+    }
+
+    /// <summary>
+    /// Initializes helper arrays required for the Hungarian algorithm, setting the
+    /// appropriate sizes and default values.
+    /// </summary>
+    /// <param name="num_rows">The number of rows in the distance matrix.</param>
+    /// <param name="num_columns">The number of columns in the distance matrix.</param>
+    void HungarianAlgorithmEigen::init_helper_arrays(int num_rows, int num_columns) {
+        covered_columns = Eigen::Array<bool, Eigen::Dynamic, 1>::Constant(num_columns, false);
+        covered_rows = Eigen::Array<bool, Eigen::Dynamic, 1>::Constant(num_rows, false);
+        star_matrix = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>::Constant(num_rows, num_columns, false);
+        new_star_matrix = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>::Constant(num_rows, num_columns, false);
+        prime_matrix = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>::Constant(num_rows, num_columns, false);
+    }
+
+    /// <summary>
+    /// Constructs the assignment vector from the star matrix, providing the optimal
+    /// assignment of rows to columns.
+    /// </summary>
+    /// <param name="assignment">Reference to the vector where the result of the assignment will be stored.</param>
+    void HungarianAlgorithmEigen::construct_assignment_vector(Eigen::VectorXi& assignment)
+    {
+        // Iterate over each row to determine the assignment for that row.
+        for (int row = 0; row < star_matrix.rows(); ++row)
+        {
+            Eigen::Index col;
+            // Check if there is a starred zero in the current row.
+            bool has_star = star_matrix.row(row).maxCoeff(&col);
+            if (has_star)
+            {
+                // If there is a starred zero, assign the corresponding column to this row.
+                assignment[row] = col;
+            }
+            else
+            {
+                // If there is no starred zero, indicate that the row is not assigned.
+                assignment[row] = DEFAULT_ASSIGNMENT_VALUE;
+            }
+        }
+    }
+
+
+    /// <summary>
+    /// Calculates the total cost of the assignment stored in the provided assignment vector.
+    /// </summary>
+    /// <param name="assignment">The vector containing the assignment for which to calculate the total cost.</param>
+    /// <returns>The total cost of the assignment.</returns>
+    double HungarianAlgorithmEigen::calculate_total_cost(Eigen::VectorXi& assignment)
+    {
+        double total_cost = 0.0; // Initialize the total cost to zero.
+
+        // Iterate over each assignment to calculate the total cost.
+        for (int row = 0; row < dist_matrix.rows(); ++row)
+        {
+            // Check if the current row has a valid assignment.
+            if (assignment(row) >= 0)
+            {
+                // Add the cost of the assigned column in the current row to the total cost.
+                total_cost += dist_matrix(row, assignment(row));
+            }
+            // Note: If the assignment is not valid (indicated by DEFAULT_ASSIGNMENT_VALUE),
+            // it is not included in the total cost calculation.
+        }
+
+        return total_cost; // Return the calculated total cost.
+    }
+
+    /// <summary>
+    /// Public interface to solve the assignment problem using the Hungarian algorithm.
+    /// This method sets up the problem using the given distance matrix and solves it,
+    /// returning the minimum total cost and filling the assignment vector with the
+    /// optimal assignment of rows to columns.
+    /// </summary>
+    /// <param name="dist_matrix">The distance matrix representing the cost of assigning rows to columns.</param>
+    /// <param name="assignment">Reference to the vector where the result of the assignment will be stored.</param>
+    /// <returns>The minimum total cost of the assignment.</returns>
+    double HungarianAlgorithmEigen::solve_assignment_problem(Eigen::MatrixXd& dist_matrix, Eigen::VectorXi& assignment)
+    {
+        // Ensure that the distance matrix contains only non-negative values.
+        if (dist_matrix.array().minCoeff() < 0)
+        {
+            std::cerr << "All matrix elements have to be non-negative." << std::endl;
+        }
+
+        // Copy the input distance matrix into the local member variable for manipulation.
+        this->dist_matrix = dist_matrix;
+
+        // Initialize helper arrays used in the algorithm, such as the star and prime matrices.
+        init_helper_arrays(dist_matrix.rows(), dist_matrix.cols());
+
+        // Execute the main steps of the Hungarian algorithm to find the optimal assignment.
+        execute_hungarian_algorithm();
+
+        // Set all elements of the assignment vector to a default value to indicate no assignment.
+        assignment.setConstant(DEFAULT_ASSIGNMENT_VALUE);
+
+        // Construct the assignment vector from the star matrix indicating the optimal assignment.
+        construct_assignment_vector(assignment);
+
+        // Calculate and return the total cost associated with the optimal assignment.
+        return calculate_total_cost(assignment);
+    }
+}

modules/ANSMOT/ByteTrackEigen/src/EigenKalmanBBoxTrack.cpp

@@ -0,0 +1,270 @@
+#include "EigenKalmanBBoxTrack.h"
+
+namespace ByteTrackEigen {
+    // Initializing the static shared Kalman filter
+    KalmanFilter KalmanBBoxTrack::shared_kalman;
+
+    /**
+     * @brief Default constructor for KalmanBBoxTrack.
+     *        Initializes member variables to their default state.
+     */
+    KalmanBBoxTrack::KalmanBBoxTrack() : BaseTrack(),
+        _tlwh(Eigen::Vector4d::Zero()), // Initializes the bounding box to a zero vector
+        kalman_filter(KalmanFilter()),  // Default initialization of the Kalman filter
+        mean(Eigen::VectorXd()),        // Initializes the mean state vector to default
+        covariance(Eigen::MatrixXd()),  // Initializes the covariance matrix to default
+        tracklet_len(0)                 // Initializes the tracklet length to zero
+    { }
+
+    /**
+     * @brief Constructor for KalmanBBoxTrack with initial bounding box and detection score.
+     *        Throws an exception if the tlwh vector does not contain exactly 4 values.
+     *
+     * @param tlwh Initial bounding box in top-left width-height format.
+     * @param score Detection score associated with the bounding box.
+     */
+
+    KalmanBBoxTrack::KalmanBBoxTrack(const std::vector<float> tlwh, float score, int class_id_,std::string object_id_):
+        BaseTrack(score),
+        _tlwh(tlwh.size() == 4 ? Eigen::Vector4d(tlwh[0], tlwh[1], tlwh[2], tlwh[3]) : Eigen::Vector4d::Zero()), // Ensures the bounding box has 4 elements
+        kalman_filter(KalmanFilter()), // Initializes the Kalman filter
+        mean(Eigen::VectorXd()),       // Initializes the mean state vector
+        covariance(Eigen::MatrixXd()), // Initializes the covariance matrix
+        tracklet_len(0),                // Initializes the tracklet length to zero
+        class_id(class_id_),
+        object_id(object_id_),
+        detection_count_(1),
+        class_id_locked_(false)
+    {
+        // Validation of tlwh size
+        if (tlwh.size() != 4) {
+            throw std::invalid_argument("tlwh vector must contain exactly 4 values.");
+        }
+        class_id_scores_[class_id_] = score;
+    }
+
+    void KalmanBBoxTrack::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;
+        }
+    }
+
+    /**
+     * @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.
+     */
+    void KalmanBBoxTrack::multi_predict(std::vector<std::shared_ptr<KalmanBBoxTrack>>& tracks) {
+        if (tracks.empty()) {
+            return; // Early exit if no tracks to process
+        }
+
+        // Extract mean and covariance for each track
+        std::vector<Eigen::VectorXd> multi_means;
+        std::vector<Eigen::MatrixXd> multi_covariances;
+
+        for (const auto track : tracks) {
+            multi_means.push_back(track->mean);  // Eigen performs deep copy by default
+            multi_covariances.push_back(track->covariance);
+        }
+
+        // For each track, set velocity to 0 if it's not in Tracked state
+        for (size_t i = 0; i < tracks.size(); ++i) {
+            if (tracks[i]->get_state() != TrackState::Tracked) {
+                multi_means[i](7) = 0;  // Zero out the velocity if not tracked
+            }
+        }
+
+        // Convert vectors to Eigen matrices for Kalman filter processing
+        Eigen::MatrixXd means_matrix(multi_means.size(), multi_means[0].size());
+        for (size_t i = 0; i < multi_means.size(); ++i) {
+            means_matrix.row(i) = multi_means[i];
+        }
+        int n = (int)multi_covariances[0].rows();  // Assuming square matrices for covariance
+        Eigen::MatrixXd covariances_matrix(n, n * multi_covariances.size());
+        for (size_t i = 0; i < multi_covariances.size(); ++i) {
+            covariances_matrix.middleCols(i * n, n) = multi_covariances[i];
+        }
+
+        // Use shared kalman filter for prediction
+        Eigen::MatrixXd predicted_means, predicted_covariances;
+        std::tie(predicted_means, predicted_covariances) = shared_kalman.multi_predict(means_matrix, covariances_matrix);
+
+        // Update each track with the predicted mean and covariance
+        for (size_t i = 0; i < tracks.size(); ++i) {
+            tracks[i]->mean = predicted_means.col(i);
+        }
+
+        int pcr = (int)predicted_covariances.rows();  // Assuming the block matrix is composed of square matrices
+        size_t num_matrices = predicted_covariances.cols() / pcr;
+
+        for (size_t i = 0; i < num_matrices; ++i) {
+            Eigen::MatrixXd block = predicted_covariances.middleCols(i * pcr, pcr);
+            tracks[i]->covariance = block;
+        }
+    }
+
+    /**
+     * @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 KalmanBBoxTrack::tlwh_to_xyah(Eigen::VectorXd tlwh) {
+        Eigen::VectorXd ret = tlwh;
+        ret.head(2) += ret.segment(2, 2) / 2.0; // Adjust x, y to be the center of the bounding box
+        if (ret(3) > 0)
+            ret(2) /= ret(3); // Update aspect ratio
+        else
+            ret(2) = 0;
+        return ret;
+    }
+
+    /**
+     * @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 KalmanBBoxTrack::activate(KalmanFilter& kalman_filter, int frame_id) {
+        // Link to the provided Kalman filter
+        this->kalman_filter = kalman_filter;
+
+        // Initialize track ID
+        this->track_id = BaseTrack::next_id();
+
+        // Convert bounding box to the xyah format and initiate the Kalman filter
+        std::tie(this->mean, this->covariance) = this->kalman_filter.initiate(this->tlwh_to_xyah(this->_tlwh));
+
+        this->tracklet_len = 0;
+        this->state = TrackState::Tracked;
+
+        // Check if track is activated in the first frame
+        this->is_activated = (frame_id == 1);
+        this->frame_id = frame_id;
+        this->start_frame = 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 KalmanBBoxTrack::update_track(const KalmanBBoxTrack& new_track, int frame_id, bool new_id) {
+        // Track update logic
+        this->frame_id = frame_id;
+        this->tracklet_len++;
+
+        std::tie(this->mean, this->covariance) = this->kalman_filter.update(this->mean, this->covariance, this->tlwh_to_xyah(new_track.tlwh()));
+
+        this->state = TrackState::Tracked;
+        this->is_activated = true;
+
+        // Assigning a new ID if necessary
+        if (new_id) {
+            this->track_id = BaseTrack::next_id();
+        }
+
+        // Update the score and vote on class_id
+        this->score = new_track.get_score();
+        voteClassId(new_track.class_id, new_track.get_score());
+        this->object_id = new_track.object_id;
+    }
+
+    /**
+     * @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 KalmanBBoxTrack::re_activate(const KalmanBBoxTrack& new_track, int frame_id, bool new_id) {
+        update_track(new_track, frame_id, new_id);
+    }
+
+    /**
+     * @brief Updates the track with a new detection without changing its ID.
+     *        Wrapper for the update_track method without the new_id flag.
+     *
+     * @param new_track The new detection to update the track with.
+     * @param frame_id Frame ID of the update.
+     */
+    void KalmanBBoxTrack::update(const KalmanBBoxTrack& new_track, int frame_id) {
+        update_track(new_track, 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.
+     */
+    Eigen::Vector4d KalmanBBoxTrack::tlwh_to_tlbr(const Eigen::Vector4d tlwh) {
+        Eigen::Vector4d ret = tlwh;
+        ret.tail<2>() += ret.head<2>();
+        return ret;
+    }
+
+    /**
+     * @brief Converts tlbr bounding box to tlwh format.
+     *
+     * @param tlbr Bounding box in tlbr format.
+     * @return Eigen::Vector4d Bounding box in tlwh format.
+     */
+    Eigen::Vector4d KalmanBBoxTrack::tlbr_to_tlwh(const Eigen::Vector4d tlbr) {
+        Eigen::Vector4d ret = tlbr;
+        ret.tail<2>() -= ret.head<2>();
+        return ret;
+    }
+
+    /**
+     * @brief Get current bounding box in tlwh format
+     *
+     * @return Eigen::Vector4d Bounding box in tlwh format.
+     */
+    Eigen::Vector4d KalmanBBoxTrack::tlwh() const {
+        if (mean.isZero(0)) { // Checking if 'mean' is uninitialized or zero
+            return _tlwh;
+        }
+
+        Eigen::Vector4d ret = mean.head(4);
+        ret[2] *= ret[3];
+        ret[0] -= ret[2] / 2.0;
+        ret[1] -= ret[3] / 2.0;
+
+        return ret;
+    }
+
+    /**
+     * @brief Returns the bounding box in tlbr format.
+     *
+     * @return Eigen::Vector4d Bounding box in tlbr format.
+     */
+    Eigen::Vector4d KalmanBBoxTrack::tlbr() const {
+        return this->tlwh_to_tlbr(this->tlwh());
+    }
+}

modules/ANSMOT/ByteTrackEigen/src/EigenKalmanFilter.cpp

@@ -0,0 +1,144 @@
+#include "EigenKalmanFilter.h"
+
+namespace ByteTrackEigen {
+    /// Constructor for KalmanFilter.
+    /// Initializes the filter with default parameters for tracking bounding boxes in image space.
+    KalmanFilter::KalmanFilter() {
+        ndim = 4;  // State space dimension (x, y, width, height)
+        double dt = 1.0;  // Time step, assuming time in seconds
+
+        // Initializing the motion matrix for the Kalman Filter.
+        // This matrix is used to predict the next state of the tracked object.
+        motion_mat = Eigen::MatrixXd::Identity(2 * ndim, 2 * ndim);
+        motion_mat.block(0, ndim, ndim, ndim) = dt * Eigen::MatrixXd::Identity(ndim, ndim);
+
+        // The update matrix is used for updating the state with a new measurement.
+        update_mat = Eigen::MatrixXd::Identity(ndim, 2 * ndim);
+
+        // Setting standard deviation weights for position and velocity.
+        // These weights help model the uncertainty in the measurements.
+        std_weight_position = 1. / 20.;
+        std_weight_velocity = 1. / 160.;
+    }
+
+    /// Destructor for KalmanFilter.
+    KalmanFilter::~KalmanFilter() {}
+
+    /// Compute standard deviations based on the mean.
+    /// This function creates a standard deviation vector for the state uncertainty.
+    /// @param mean The mean vector of the state.
+    /// @return Vector of standard deviations.
+    Eigen::VectorXd KalmanFilter::create_std(const Eigen::VectorXd& mean) {
+        Eigen::VectorXd std_devs(8);
+        // Setting standard deviations for each state dimension based on the weights.
+        std_devs(0) = std_weight_position * mean(3);
+        std_devs(1) = std_weight_position * mean(3);
+        std_devs(2) = 1e-2;  // Small fixed standard deviation for width
+        std_devs(3) = std_weight_position * mean(3);
+        std_devs(4) = std_weight_velocity * mean(3);
+        std_devs(5) = std_weight_velocity * mean(3);
+        std_devs(6) = 1e-5;  // Small fixed standard deviation for velocity in width
+        std_devs(7) = std_weight_velocity * mean(3);
+
+        return std_devs;
+    }
+
+    /// Initialize a new track from an unassociated 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> KalmanFilter::initiate(const Eigen::VectorXd& measurement) {
+
+        Eigen::VectorXd mean = Eigen::VectorXd::Zero(2 * ndim);
+        mean.head(ndim) = measurement;  // Initial state mean is set to the measurement
+
+        // Calculating initial covariance matrix based on the standard deviations
+        Eigen::VectorXd std_devs = create_std(mean);
+        Eigen::MatrixXd covariance = Eigen::MatrixXd::Zero(2 * ndim, 2 * ndim);
+        for (int i = 0; i < 2 * ndim; i++) {
+            covariance(i, i) = std_devs(i) * std_devs(i);
+        }
+
+        return { mean, covariance };
+    }
+
+    /// Project the state distribution to the measurement space.
+    /// @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> KalmanFilter::project(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance) {
+        Eigen::VectorXd projected_std = create_std(mean).head(ndim);
+        Eigen::MatrixXd innovation_cov = Eigen::MatrixXd::Zero(ndim, ndim);
+        for (int i = 0; i < ndim; i++) {
+            innovation_cov(i, i) = projected_std(i) * projected_std(i);
+        }
+
+        // Predicting the new mean and covariance matrix for the measurement space
+        Eigen::VectorXd new_mean = this->update_mat * mean;
+        Eigen::MatrixXd new_cov = this->update_mat * covariance * this->update_mat.transpose() + innovation_cov;
+
+        return { new_mean, new_cov };
+    }
+
+    /// Run the Kalman filter prediction step for multiple measurements.
+    /// @param means The mean matrix of all current states.
+    /// @param covariances 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> KalmanFilter::multi_predict(const Eigen::MatrixXd& means, const Eigen::MatrixXd& covariances) {
+
+        int n_tracks = means.rows();  // Number of tracks
+
+        // Prepare matrices to hold new means and covariances after prediction.
+        Eigen::MatrixXd new_means(2 * ndim, n_tracks);
+        Eigen::MatrixXd new_covs(2 * ndim, 2 * ndim * n_tracks);  // Stacking covariances along the last two dimensions
+
+        for (int i = 0; i < n_tracks; ++i) {
+            // Calculating motion standard deviation and covariance for each track
+            Eigen::VectorXd motion_std = create_std(means.row(i));
+            Eigen::MatrixXd motion_cov = Eigen::MatrixXd::Zero(2 * ndim, 2 * ndim);
+            for (int j = 0; j < 2 * ndim; j++) {
+                motion_cov(j, j) = motion_std(j) * motion_std(j);
+            }
+
+            // Updating means and covariances for each track
+            new_means.col(i) = motion_mat * means.row(i).transpose();
+            new_covs.block(0, 2 * ndim * i, 2 * ndim, 2 * ndim) = motion_mat * covariances.block(0, 2 * ndim * i, 2 * ndim, 2 * ndim) * motion_mat.transpose() + motion_cov;
+        }
+
+        return { new_means, new_covs };
+    }
+
+    /// Run the Kalman filter correction step.
+    /// @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> KalmanFilter::update(
+        const Eigen::VectorXd& mean,
+        const Eigen::MatrixXd& covariance,
+        const Eigen::VectorXd& measurement)
+    {
+        // Projecting the state to the measurement space
+        Eigen::VectorXd projected_mean;
+        Eigen::MatrixXd projected_cov;
+        std::tie(projected_mean, projected_cov) = project(mean, covariance);
+
+        // Performing Cholesky decomposition
+        Eigen::LLT<Eigen::MatrixXd> cho_factor(projected_cov);
+        if (cho_factor.info() != Eigen::Success) {
+            // Handling decomposition failure
+            throw std::runtime_error("Decomposition failed!");
+        }
+
+        // Calculating the Kalman gain
+        // This represents how much the predictions should be corrected based on the new measurement.
+        Eigen::MatrixXd kalman_gain = cho_factor.solve(this->update_mat * covariance).transpose();
+
+        // Updating the mean and covariance based on the new measurement and Kalman gain
+        Eigen::VectorXd new_mean = mean + kalman_gain * (measurement - projected_mean);
+        Eigen::MatrixXd new_covariance = covariance - kalman_gain * projected_cov * kalman_gain.transpose();
+
+        return { new_mean, new_covariance };
+    }
+
+
+}

modules/ANSMOT/ByteTrackEigen/src/EigenLinearAssignment.cpp

@@ -0,0 +1,106 @@
+#include "EigenLinearAssignment.h"
+
+namespace ByteTrackEigen {
+    /**
+     * @brief Generates matches based on a cost matrix and a set of indices, considering a threshold.
+     *
+     * This function iterates through the provided indices and checks if the corresponding cost in the
+     * cost matrix is below the specified threshold. If so, the index pair is considered a match. It also
+     * keeps track of unmatched indices in both dimensions of the cost matrix.
+     *
+     * @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.
+     * @return A tuple containing matched indices, and sets of unmatched indices in both dimensions.
+     */
+    std::tuple<std::vector<std::pair<int, int>>, std::set<int>, std::set<int>> LinearAssignment::indices_to_matches(
+        const Eigen::MatrixXd& cost_matrix, const Eigen::MatrixXi& indices, double thresh)
+    {
+        if (cost_matrix.rows() <= 0 || cost_matrix.cols() <= 0) {
+            throw std::invalid_argument("Cost matrix dimensions must be positive.");
+        }
+
+        std::vector<std::pair<int, int>> matches;
+        std::set<int> unmatched_a, unmatched_b;
+
+        int num_rows = cost_matrix.rows();
+        int num_cols = cost_matrix.cols();
+
+        // Initialize unmatched indices for both dimensions.
+        for (int i = 0; i < num_rows; i++)
+            unmatched_a.insert(i);
+
+        for (int j = 0; j < num_cols; j++)
+            unmatched_b.insert(j);
+
+        // Iterate through the indices to find valid matches.
+        for (int k = 0; k < indices.rows(); k++)
+        {
+            int i = indices(k, 0);
+            int j = indices(k, 1);
+            if (i != -1 && j != -1) {
+                if (cost_matrix(i, j) <= thresh)
+                {
+                    matches.push_back({ i, j });
+                    unmatched_a.erase(i);
+                    unmatched_b.erase(j);
+                }
+            }
+        }
+
+        return { matches, unmatched_a, unmatched_b };
+    }
+
+    /**
+     * @brief Solves the linear assignment problem for a given cost matrix and threshold.
+     *
+     * This function first checks if the cost matrix is empty. If not, it modifies the cost matrix
+     * to mark values above the threshold as effectively infinite. Then it calls the Hungarian Algorithm
+     * to solve the assignment problem and converts the results into indices. These indices are then passed
+     * to `indices_to_matches` to extract the actual matches and unmatched indices.
+     *
+     * @param cost_matrix The matrix representing the cost of assigning each pair of elements.
+     * @param thresh A threshold value to determine acceptable assignments.
+     * @return A tuple containing matched indices, and sets of unmatched indices in both dimensions.
+     */
+    std::tuple<std::vector<std::pair<int, int>>, std::set<int>, std::set<int>> LinearAssignment::linear_assignment(
+        const Eigen::MatrixXd& cost_matrix, double thresh)
+    {
+        int num_rows = cost_matrix.rows();
+        int num_cols = cost_matrix.cols();
+
+        // Handle empty cost matrix scenario.
+        if (num_rows == 0 || num_cols == 0)
+        {
+            std::set<int> unmatched_indices_first;
+            std::set<int> unmatched_indices_second;
+
+            for (int i = 0; i < num_rows; i++) {
+                unmatched_indices_first.insert(i);
+            }
+            for (int i = 0; i < num_cols; i++) {
+                unmatched_indices_second.insert(i);
+            }
+            return { {}, unmatched_indices_first, unmatched_indices_second };
+        }
+
+        // Modify the cost matrix to mark values above the threshold.
+        Eigen::MatrixXd modified_cost_matrix = cost_matrix.unaryExpr([thresh](double val) {
+            return (val > thresh) ? thresh + 1e-4 : val;
+            });
+
+        Eigen::VectorXi assignment = Eigen::VectorXi::Constant(modified_cost_matrix.rows(), -1);
+
+        // Solve the assignment problem using the Hungarian Algorithm.
+        this->hungarian.solve_assignment_problem(modified_cost_matrix, assignment);
+
+        // Convert the solution to indices format.
+        Eigen::MatrixXi indices = Eigen::MatrixXi::Zero(num_rows, 2);
+        indices.col(0) = Eigen::VectorXi::LinSpaced(num_rows, 0, num_rows - 1);
+        indices.col(1) = assignment;
+
+        // Use indices_to_matches to get the final matching result.
+        return indices_to_matches(cost_matrix, indices, thresh);
+    }
+
+}

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);
+//}
+
+	
+}

modules/ANSMOT/CMakeLists.txt

@@ -0,0 +1,30 @@
+# ANSMOT — Multi-Object Tracking DLL (ByteTrack, OC-SORT, UCMC)
+file(GLOB_RECURSE ANSMOT_HEADERS "*.h")
+file(GLOB_RECURSE ANSMOT_SOURCES "*.cpp")
+
+add_library(ANSMOT SHARED ${ANSMOT_HEADERS} ${ANSMOT_SOURCES})
+
+target_include_directories(ANSMOT PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+# ByteTrack, OC-SORT, UCMC all have include subdirectories
+target_include_directories(ANSMOT PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/ByteTrack/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/ByteTrackEigen/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/ByteTrackNCNN/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/OCSort/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/UCMC/include
+)
+
+# Match original vcxproj: labview, anslicensing, ANSLicensingSystem
+target_link_libraries(ANSMOT
+    PRIVATE ANSLicensingSystem
+    PRIVATE anslicensing
+    PRIVATE labview
+    PRIVATE spdlog_dep
+    PRIVATE opencv
+)
+
+target_compile_definitions(ANSMOT PRIVATE UNICODE _UNICODE ANSMOT_EXPORTS)
+target_precompile_headers(ANSMOT PRIVATE pch.h)

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

modules/ANSMOT/OCSort/src/Association.cpp

@@ -0,0 +1,190 @@
+#include "Association.h"
+#include <iomanip>
+#include <iostream>
+
+namespace ANSOCSort {
+    std::tuple<Eigen::MatrixXf, Eigen::MatrixXf> speed_direction_batch(const Eigen::MatrixXf& dets,
+        const Eigen::MatrixXf& tracks) {
+        Eigen::VectorXf CX1 = (dets.col(0) + dets.col(2)) / 2.0;
+        Eigen::VectorXf CY1 = (dets.col(1) + dets.col(3)) / 2.f;
+        Eigen::MatrixXf CX2 = (tracks.col(0) + tracks.col(2)) / 2.f;
+        Eigen::MatrixXf CY2 = (tracks.col(1) + tracks.col(3)) / 2.f;
+        Eigen::MatrixXf dx = CX1.transpose().replicate(tracks.rows(), 1) - CX2.replicate(1, dets.rows());
+        Eigen::MatrixXf dy = CY1.transpose().replicate(tracks.rows(), 1) - CY2.replicate(1, dets.rows());
+        Eigen::MatrixXf norm = (dx.array().square() + dy.array().square()).sqrt() + 1e-6f;
+        dx = dx.array() / norm.array();
+        dy = dy.array() / norm.array();
+        return std::make_tuple(dy, dx);
+    }
+    Eigen::MatrixXf iou_batch(const Eigen::MatrixXf& bboxes1, const Eigen::MatrixXf& bboxes2) {
+        Eigen::Matrix<float, Eigen::Dynamic, 1> a = bboxes1.col(0);// bboxes1[..., 0] (n1,1)
+        Eigen::Matrix<float, 1, Eigen::Dynamic> b = bboxes2.col(0);// bboxes2[..., 0] (1,n2)
+        Eigen::MatrixXf xx1 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
+        a = bboxes1.col(1);// bboxes1[..., 1]
+        b = bboxes2.col(1);// bboxes2[..., 1]
+        Eigen::MatrixXf yy1 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
+        a = bboxes1.col(2);// bboxes1[..., 2]
+        b = bboxes2.col(2);// bboxes1[..., 2]
+        Eigen::MatrixXf xx2 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
+        a = bboxes1.col(3);// bboxes1[..., 3]
+        b = bboxes2.col(3);// bboxes1[..., 3]
+        Eigen::MatrixXf yy2 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
+        Eigen::MatrixXf w = (xx2 - xx1).cwiseMax(0);
+        Eigen::MatrixXf h = (yy2 - yy1).cwiseMax(0);
+        Eigen::MatrixXf wh = w.array() * h.array();
+        a = (bboxes1.col(2) - bboxes1.col(0)).array() * (bboxes1.col(3) - bboxes1.col(1)).array();
+        b = (bboxes2.col(2) - bboxes2.col(0)).array() * (bboxes2.col(3) - bboxes2.col(1)).array();
+        Eigen::MatrixXf part1_ = a.replicate(1, b.cols());
+        Eigen::MatrixXf part2_ = b.replicate(a.rows(), 1);
+        Eigen::MatrixXf Sum = (part1_ + part2_ - wh).cwiseMax(1e-7f);
+        return wh.cwiseQuotient(Sum);
+    }
+
+
+    Eigen::MatrixXf giou_batch(const Eigen::MatrixXf& bboxes1, const Eigen::MatrixXf& bboxes2) {
+        Eigen::Matrix<float, Eigen::Dynamic, 1> a = bboxes1.col(0);// bboxes1[..., 0] (n1,1)
+        Eigen::Matrix<float, 1, Eigen::Dynamic> b = bboxes2.col(0);// bboxes2[..., 0] (1,n2)
+        Eigen::MatrixXf xx1 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
+        a = bboxes1.col(1);// bboxes1[..., 1]
+        b = bboxes2.col(1);// bboxes2[..., 1]
+        Eigen::MatrixXf yy1 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
+        a = bboxes1.col(2);// bboxes1[..., 2]
+        b = bboxes2.col(2);// bboxes1[..., 2]
+        Eigen::MatrixXf xx2 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
+        a = bboxes1.col(3);// bboxes1[..., 3]
+        b = bboxes2.col(3);// bboxes1[..., 3]
+        Eigen::MatrixXf yy2 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
+        Eigen::MatrixXf w = (xx2 - xx1).cwiseMax(0);
+        Eigen::MatrixXf h = (yy2 - yy1).cwiseMax(0);
+        Eigen::MatrixXf wh = w.array() * h.array();
+        a = (bboxes1.col(2) - bboxes1.col(0)).array() * (bboxes1.col(3) - bboxes1.col(1)).array();
+        b = (bboxes2.col(2) - bboxes2.col(0)).array() * (bboxes2.col(3) - bboxes2.col(1)).array();
+        Eigen::MatrixXf part1_ = a.replicate(1, b.cols());
+        Eigen::MatrixXf part2_ = b.replicate(a.rows(), 1);
+        Eigen::MatrixXf Sum = (part1_ + part2_ - wh).cwiseMax(1e-7f);
+        Eigen::MatrixXf iou = wh.cwiseQuotient(Sum);
+
+        a = bboxes1.col(0);
+        b = bboxes2.col(0);
+        Eigen::MatrixXf xxc1 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
+        a = bboxes1.col(1);// bboxes1[..., 1]
+        b = bboxes2.col(1);// bboxes2[..., 1]
+        Eigen::MatrixXf yyc1 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
+        a = bboxes1.col(2);// bboxes1[..., 2]
+        b = bboxes2.col(2);// bboxes1[..., 2]
+        Eigen::MatrixXf xxc2 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
+        a = bboxes1.col(3);// bboxes1[..., 3]
+        b = bboxes2.col(3);// bboxes1[..., 3]
+        Eigen::MatrixXf yyc2 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
+
+        Eigen::MatrixXf wc = xxc2 - xxc1;
+        Eigen::MatrixXf hc = yyc2 - yyc1;
+        if ((wc.array() > 0).all() && (hc.array() > 0).all())
+            return iou;
+        else {
+            Eigen::MatrixXf area_enclose = (wc.array() * hc.array()).cwiseMax(1e-7f);
+            Eigen::MatrixXf giou = iou.array() - (area_enclose.array() - wh.array()) / area_enclose.array();
+            giou = (giou.array() + 1) / 2.0;
+            return giou;
+        }
+    }
+
+    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) {
+        if (trackers.rows() == 0) {
+            std::vector<int> unmatched_dets;
+            for (int i = 0; i < detections.rows(); i++) {
+                unmatched_dets.push_back(i);
+            }
+            return std::make_tuple(std::vector<Eigen::Matrix<int, 1, 2>>(), unmatched_dets, std::vector<int>());
+        }
+        Eigen::MatrixXf Y, X;
+        auto result = speed_direction_batch(detections, previous_obs_);
+        Y = std::get<0>(result);
+        X = std::get<1>(result);
+        Eigen::MatrixXf inertia_Y = velocities.col(0);
+        Eigen::MatrixXf inertia_X = velocities.col(1);
+        Eigen::MatrixXf inertia_Y_ = inertia_Y.replicate(1, Y.cols());
+        Eigen::MatrixXf inertia_X_ = inertia_X.replicate(1, X.cols());
+        Eigen::MatrixXf diff_angle_cos = inertia_X_.array() * X.array() + inertia_Y_.array() * Y.array();
+        diff_angle_cos = (diff_angle_cos.array().min(1).max(-1)).matrix();
+        Eigen::MatrixXf diff_angle = Eigen::acos(diff_angle_cos.array());
+        diff_angle = (m_pi / 2.0 - diff_angle.array().abs()).array() / (m_pi);
+        Eigen::Array<bool, 1, Eigen::Dynamic> valid_mask = Eigen::Array<bool, Eigen::Dynamic, 1>::Ones(previous_obs_.rows());
+        valid_mask = valid_mask.array() * ((previous_obs_.col(4).array() >= 0).transpose()).array();
+        Eigen::MatrixXf iou_matrix = iou_batch(detections, trackers);
+        Eigen::MatrixXf scores = detections.col(detections.cols() - 2).replicate(1, trackers.rows());
+        Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> valid_mask_ = (valid_mask.transpose()).replicate(1, X.cols());
+        Eigen::MatrixXf angle_diff_cost = ((((valid_mask_.cast<float>()).array() * diff_angle.array()).array() * vdc_weight)
+            .transpose())
+            .array() *
+            scores.array();
+        Eigen::Matrix<float, Eigen::Dynamic, 2> matched_indices(0, 2);
+        if (std::min(iou_matrix.cols(), iou_matrix.rows()) > 0) {
+            Eigen::MatrixXf a = (iou_matrix.array() > iou_threshold).cast<float>();
+            float sum1 = (a.rowwise().sum()).maxCoeff();
+            float sum0 = (a.colwise().sum()).maxCoeff();
+            if ((fabs(sum1 - 1) < 1e-12) && (fabs(sum0 - 1) < 1e-12)) {
+                for (int i = 0; i < a.rows(); i++) {
+                    for (int j = 0; j < a.cols(); j++) {
+                        if (a(i, j) > 0) {
+                            Eigen::RowVectorXf row(2);
+                            row << i, j;
+                            matched_indices.conservativeResize(matched_indices.rows() + 1, Eigen::NoChange);
+                            matched_indices.row(matched_indices.rows() - 1) = row;
+                        }
+                    }
+                }
+            }
+            else {
+                Eigen::MatrixXf cost_matrix = iou_matrix.array() + angle_diff_cost.array();
+                std::vector<std::vector<float>> cost_iou_matrix(cost_matrix.rows(), std::vector<float>(cost_matrix.cols()));
+                for (int i = 0; i < cost_matrix.rows(); i++) {
+                    for (int j = 0; j < cost_matrix.cols(); j++) {
+                        cost_iou_matrix[i][j] = -cost_matrix(i, j);
+                    }
+                }
+                std::vector<int> rowsol, colsol;
+                float MIN_cost = execLapjv(cost_iou_matrix, rowsol, colsol, true, 0.01, true);
+                for (int i = 0; i < rowsol.size(); i++) {
+                    if (rowsol.at(i) >= 0) {
+                        Eigen::RowVectorXf row(2);
+                        row << colsol.at(rowsol.at(i)), rowsol.at(i);
+                        matched_indices.conservativeResize(matched_indices.rows() + 1, Eigen::NoChange);
+                        matched_indices.row(matched_indices.rows() - 1) = row;
+                    }
+                }
+            }
+        }
+        else {
+            matched_indices = Eigen::MatrixXf(0, 2);
+        }
+        std::vector<int> unmatched_detections;
+        for (int i = 0; i < detections.rows(); i++) {
+            if ((matched_indices.col(0).array() == i).sum() == 0) {
+                unmatched_detections.push_back(i);
+            }
+        }
+        std::vector<int> unmatched_trackers;
+        for (int i = 0; i < trackers.rows(); i++) {
+            if ((matched_indices.col(1).array() == i).sum() == 0) {
+                unmatched_trackers.push_back(i);
+            }
+        }
+        std::vector<Eigen::Matrix<int, 1, 2>> matches;
+        Eigen::Matrix<int, 1, 2> tmp;
+        for (int i = 0; i < matched_indices.rows(); i++) {
+            tmp = (matched_indices.row(i)).cast<int>();
+            if (iou_matrix(tmp(0), tmp(1)) < iou_threshold) {
+                unmatched_detections.push_back(tmp(0));
+                unmatched_trackers.push_back(tmp(1));
+            }
+            else {
+                matches.push_back(tmp);
+            }
+        }
+        if (matches.size() == 0) {
+            matches.clear();
+        }
+        return std::make_tuple(matches, unmatched_detections, unmatched_trackers);
+    }
+}// namespace ocsort

modules/ANSMOT/OCSort/src/KalmanBoxTracker.cpp

@@ -0,0 +1,168 @@
+#include <utility>
+#include "KalmanBoxTracker.h"
+namespace ANSOCSort {
+    int KalmanBoxTracker::count = 0;
+
+    KalmanBoxTracker::~KalmanBoxTracker() {
+        if (kf) {
+            delete kf;
+            kf = nullptr;
+        }
+    }
+
+    KalmanBoxTracker::KalmanBoxTracker(const KalmanBoxTracker& other)
+        : bbox(other.bbox),
+          kf(other.kf ? new KalmanFilterNew(*other.kf) : nullptr),
+          time_since_update(other.time_since_update),
+          id(other.id),
+          history(other.history),
+          hits(other.hits),
+          hit_streak(other.hit_streak),
+          age(other.age),
+          last_observation(other.last_observation),
+          observations(other.observations),
+          history_observations(other.history_observations),
+          velocity(other.velocity),
+          delta_t(other.delta_t)
+    {
+    }
+
+    KalmanBoxTracker& KalmanBoxTracker::operator=(const KalmanBoxTracker& other) {
+        if (this != &other) {
+            delete kf;
+            kf = other.kf ? new KalmanFilterNew(*other.kf) : nullptr;
+            bbox = other.bbox;
+            time_since_update = other.time_since_update;
+            id = other.id;
+            history = other.history;
+            hits = other.hits;
+            hit_streak = other.hit_streak;
+            age = other.age;
+            last_observation = other.last_observation;
+            observations = other.observations;
+            history_observations = other.history_observations;
+            velocity = other.velocity;
+            delta_t = other.delta_t;
+        }
+        return *this;
+    }
+
+    KalmanBoxTracker::KalmanBoxTracker(KalmanBoxTracker&& other) noexcept
+        : bbox(std::move(other.bbox)),
+          kf(other.kf),
+          time_since_update(other.time_since_update),
+          id(other.id),
+          history(std::move(other.history)),
+          hits(other.hits),
+          hit_streak(other.hit_streak),
+          age(other.age),
+          last_observation(std::move(other.last_observation)),
+          observations(std::move(other.observations)),
+          history_observations(std::move(other.history_observations)),
+          velocity(std::move(other.velocity)),
+          delta_t(other.delta_t)
+    {
+        other.kf = nullptr;
+    }
+
+    KalmanBoxTracker& KalmanBoxTracker::operator=(KalmanBoxTracker&& other) noexcept {
+        if (this != &other) {
+            delete kf;
+            bbox = std::move(other.bbox);
+            kf = other.kf;
+            other.kf = nullptr;
+            time_since_update = other.time_since_update;
+            id = other.id;
+            history = std::move(other.history);
+            hits = other.hits;
+            hit_streak = other.hit_streak;
+            age = other.age;
+            last_observation = std::move(other.last_observation);
+            observations = std::move(other.observations);
+            history_observations = std::move(other.history_observations);
+            velocity = std::move(other.velocity);
+            delta_t = other.delta_t;
+        }
+        return *this;
+    }
+
+    KalmanBoxTracker::KalmanBoxTracker(Eigen::VectorXf bbox_, int delta_t_) {
+        bbox = std::move(bbox_);
+        delta_t = delta_t_;
+        kf = new KalmanFilterNew(7, 4);//(7;4)
+        kf->F << 1, 0, 0, 0, 1, 0, 0,
+                 0, 1, 0, 0, 0, 1, 0,
+                 0, 0, 1, 0, 0, 0, 1,
+                 0, 0, 0, 1, 0, 0, 0,
+                 0, 0, 0, 0, 1, 0, 0,
+                 0, 0, 0, 0, 0, 1, 0,
+                 0, 0, 0, 0, 0, 0, 1;
+
+        kf->H << 1, 0, 0, 0, 0, 0, 0,
+                 0, 1, 0, 0, 0, 0, 0, 0,
+                 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+        
+        kf->R.block(2, 2, 2, 2) *= 10.0;
+        kf->P.block(4, 4, 3, 3) *= 1000.0;
+        kf->P *= 10.0;
+        kf->Q.bottomRightCorner(1, 1)(0, 0) *= 0.01;
+        kf->Q.block(4, 4, 3, 3) *= 0.01;
+        kf->x.head<4>() = convert_bbox_to_z(bbox);
+        
+        time_since_update = 0;
+        id = KalmanBoxTracker::count;
+        KalmanBoxTracker::count += 1;
+        history.clear();
+        hits = 0;      
+        hit_streak = 0; 
+        age = 0;        
+        last_observation.fill(-1);  
+        observations.clear();        
+        history_observations.clear();
+        velocity.fill(0);            
+    }
+
+    void KalmanBoxTracker::update(Eigen::Matrix<float, 9, 1>* bbox_) {
+        if (bbox_ != nullptr) {
+            if (int(last_observation.sum()) >= 0) {
+                Eigen::VectorXf previous_box_tmp;
+                for (int i = 0; i < delta_t; ++i) {
+                    int dt = delta_t - i;
+                    if (observations.count(age - dt) > 0) {
+                        previous_box_tmp = observations[age - dt];
+                        break;
+                    }
+                }
+                if (0 == previous_box_tmp.size()) {     
+                    previous_box_tmp = last_observation;
+                }
+                velocity = speed_direction(previous_box_tmp, *bbox_);
+            }
+            last_observation = *bbox_; 
+            observations[age] = *bbox_;
+            history_observations.push_back(*bbox_);
+            time_since_update = 0;
+            history.clear();
+            hits += 1;
+            hit_streak += 1;
+            Eigen::VectorXf tmp = convert_bbox_to_z(*bbox_);
+            kf->update(&tmp);
+        }
+        else {
+            kf->update(nullptr);
+        }
+    }
+
+    Eigen::RowVectorXf KalmanBoxTracker::predict() {
+        if (kf->x[6] + kf->x[2] <= 0) kf->x[6] *= 0.0;
+        kf->predict();
+        age += 1;
+        if (time_since_update > 0) hit_streak = 0;
+        time_since_update += 1;
+        history.push_back(convert_x_to_bbox(kf->x));
+        return convert_x_to_bbox(kf->x);
+    }
+    Eigen::VectorXf KalmanBoxTracker::get_state() {
+        return convert_x_to_bbox(kf->x);
+    }
+}// namespace ocsort

modules/ANSMOT/OCSort/src/OCSort.cpp

@@ -0,0 +1,302 @@
+#include "OCSort.h"
+#include "iomanip"
+#include <utility>
+
+namespace ANSOCSort {
+    template<typename Matrix>
+    std::ostream& operator<<(std::ostream& os, const std::vector<Matrix>& v) {
+        os << "{";
+        for (auto it = v.begin(); it != v.end(); ++it) {
+            os << "(" << *it << ")\n";
+            if (it != v.end() - 1) os << ",";
+        }
+        os << "}\n";
+        return os;
+    }
+    OCSort::OCSort() {
+        update_parameters(0, 50, 1, 0.22136877277096445, 1, "giou", 0.3941737016672115, true);
+    }
+
+    void OCSort::update_parameters(float det_thresh_, int max_age_, int min_hits_, float iou_threshold_, int delta_t_, std::string asso_func_, float inertia_, bool use_byte_, bool autoFrameRate) {
+        max_age = max_age_;
+        min_hits = min_hits_;
+        iou_threshold = iou_threshold_;
+        trackers.clear();
+        frame_count = 0;
+        det_thresh = det_thresh_;
+        delta_t = delta_t_;
+        std::unordered_map<std::string, std::function<Eigen::MatrixXf(const Eigen::MatrixXf&, const Eigen::MatrixXf&)>> ASSO_FUNCS{
+            {"iou", iou_batch},
+            { "giou", giou_batch }};
+        ;
+        std::function<Eigen::MatrixXf(const Eigen::MatrixXf&, const Eigen::MatrixXf&)> asso_func = ASSO_FUNCS[asso_func_];
+        inertia = inertia_;
+        use_byte = use_byte_;
+        KalmanBoxTracker::count = 0;
+
+        // Frame rate auto-estimation
+        auto_frame_rate_ = autoFrameRate;
+        estimated_fps_ = 30.0f;
+        time_scale_factor_ = 1.0f;
+        fps_sample_count_ = 0;
+        has_last_update_time_ = false;
+    }
+
+    float OCSort::getEstimatedFps() const {
+        return estimated_fps_;
+    }
+
+    void OCSort::estimateFrameRate() {
+        if (!auto_frame_rate_) return;
+
+        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) return; // Skip near-zero intervals
+
+        float instant_fps = static_cast<float>(1.0 / delta_sec);
+
+        // EMA smoothing: alpha=0.3 for warmup (first 10 samples), alpha=0.1 steady state
+        fps_sample_count_++;
+        float alpha = (fps_sample_count_ <= 10) ? 0.3f : 0.1f;
+        estimated_fps_ = alpha * instant_fps + (1.0f - alpha) * estimated_fps_;
+
+        // Clamp estimated fps to a reasonable range
+        estimated_fps_ = std::max(1.0f, std::min(estimated_fps_, 120.0f));
+
+        // 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));
+    }
+    std::ostream& precision(std::ostream& os) {
+        os << std::fixed << std::setprecision(2);
+        return os;
+    }
+
+    std::vector<Eigen::RowVectorXf> OCSort::update(Eigen::MatrixXf dets) {
+        // Estimate frame rate from timing of update() calls
+        estimateFrameRate();
+
+        frame_count += 1;
+        std::vector<Eigen::RowVectorXf> ret;
+        try {
+            Eigen::Matrix<float, 1, Eigen::Dynamic> confs = dets.col(4);
+            Eigen::MatrixXf output_results = dets;
+
+            auto inds_low = confs.array() > 0.1;
+            auto inds_high = confs.array() < det_thresh;
+            auto inds_second = inds_low && inds_high;
+            Eigen::Matrix<float, Eigen::Dynamic, 9> dets_second;
+            Eigen::Matrix<bool, 1, Eigen::Dynamic> remain_inds = (confs.array() > det_thresh);
+            Eigen::Matrix<float, Eigen::Dynamic, 9> dets_first;
+            for (int i = 0; i < output_results.rows(); i++) {
+                if (true == inds_second(i)) {
+                    dets_second.conservativeResize(dets_second.rows() + 1, Eigen::NoChange);
+                    dets_second.row(dets_second.rows() - 1) = output_results.row(i);
+                }
+                if (true == remain_inds(i)) {
+                    dets_first.conservativeResize(dets_first.rows() + 1, Eigen::NoChange);
+                    dets_first.row(dets_first.rows() - 1) = output_results.row(i);
+                }
+            }
+
+            // Multi-predict: call 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);
+            }
+
+            Eigen::MatrixXf trks = Eigen::MatrixXf::Zero(trackers.size(), 5);
+            std::vector<int> to_del;
+            for (int i = 0; i < trks.rows(); i++) {
+                // First predict call captures the return value for trks
+                Eigen::RowVectorXf pos = trackers[i].predict();
+                // Additional predict calls for frame skip compensation
+                for (int p = 1; p < num_predicts; p++) {
+                    pos = trackers[i].predict();
+                }
+                trks.row(i) << pos(0), pos(1), pos(2), pos(3), 0;
+            }
+
+            // Adaptive matching: relax iou_threshold during frame skips
+            float effective_iou_threshold = iou_threshold;
+            if (num_predicts > 1) {
+                effective_iou_threshold = std::max(iou_threshold - 0.02f * (num_predicts - 1), 0.05f);
+            }
+            Eigen::MatrixXf velocities = Eigen::MatrixXf::Zero(trackers.size(), 2);
+            Eigen::MatrixXf last_boxes = Eigen::MatrixXf::Zero(trackers.size(), 9);
+            Eigen::MatrixXf k_observations = Eigen::MatrixXf::Zero(trackers.size(), 9);
+            for (int i = 0; i < trackers.size(); i++) {
+                velocities.row(i) = trackers[i].velocity;
+                last_boxes.row(i) = trackers[i].last_observation;
+                k_observations.row(i) = k_previous_obs(trackers[i].observations, trackers[i].age, delta_t);
+            }
+
+            std::vector<Eigen::Matrix<int, 1, 2>> matched;
+            std::vector<int> unmatched_dets;
+            std::vector<int> unmatched_trks;
+            auto result = associate(dets_first, trks, effective_iou_threshold, velocities, k_observations, inertia);
+            matched = std::get<0>(result);
+            unmatched_dets = std::get<1>(result);
+            unmatched_trks = std::get<2>(result);
+            for (auto m : matched) {
+                Eigen::Matrix<float, 9, 1> tmp_bbox;
+                tmp_bbox = dets_first.block<1, 9>(m(0), 0);
+                trackers[m(1)].update(&(tmp_bbox));
+            }
+
+            if (true == use_byte && dets_second.rows() > 0 && unmatched_trks.size() > 0) {
+                Eigen::MatrixXf u_trks(unmatched_trks.size(), trks.cols());
+                int index_for_u_trks = 0;
+                for (auto i : unmatched_trks) {
+                    u_trks.row(index_for_u_trks++) = trks.row(i);
+                }
+                Eigen::MatrixXf iou_left = giou_batch(dets_second, u_trks);
+                if (iou_left.maxCoeff() > effective_iou_threshold) {
+                    std::vector<std::vector<float>> iou_matrix(iou_left.rows(), std::vector<float>(iou_left.cols()));
+                    for (int i = 0; i < iou_left.rows(); i++) {
+                        for (int j = 0; j < iou_left.cols(); j++) {
+                            iou_matrix[i][j] = -iou_left(i, j);
+                        }
+                    }
+                    std::vector<int> rowsol, colsol;
+                    float MIN_cost = execLapjv(iou_matrix, rowsol, colsol, true, 0.01, true);
+                    std::vector<std::vector<int>> matched_indices;
+                    for (int i = 0; i < rowsol.size(); i++) {
+                        if (rowsol.at(i) >= 0) {
+                            matched_indices.push_back({ colsol.at(rowsol.at(i)), rowsol.at(i) });
+                        }
+                    }
+
+                    std::vector<int> to_remove_trk_indices;
+                    for (auto m : matched_indices) {
+                        int det_ind = m[0];
+                        int trk_ind = unmatched_trks[m[1]];
+                        if (iou_left(m[0], m[1]) < effective_iou_threshold) continue;
+
+                        Eigen::Matrix<float, 9, 1> tmp_box;
+                        tmp_box = dets_second.block<1, 9>(det_ind, 0);
+                        trackers[trk_ind].update(&tmp_box);
+                        to_remove_trk_indices.push_back(trk_ind);
+                    }
+                    std::vector<int> tmp_res1(unmatched_trks.size());
+                    sort(unmatched_trks.begin(), unmatched_trks.end());
+                    sort(to_remove_trk_indices.begin(), to_remove_trk_indices.end());
+                    auto end1 = set_difference(unmatched_trks.begin(), unmatched_trks.end(),
+                        to_remove_trk_indices.begin(), to_remove_trk_indices.end(),
+                        tmp_res1.begin());
+                    tmp_res1.resize(end1 - tmp_res1.begin());
+                    unmatched_trks = tmp_res1;
+                }
+            }
+
+
+            if (unmatched_dets.size() > 0 && unmatched_trks.size() > 0) {
+                Eigen::MatrixXf left_dets(unmatched_dets.size(), 9);//6
+                int inx_for_dets = 0;
+                for (auto i : unmatched_dets) {
+                    left_dets.row(inx_for_dets++) = dets_first.row(i);
+                }
+                Eigen::MatrixXf left_trks(unmatched_trks.size(), last_boxes.cols());
+                int indx_for_trk = 0;
+                for (auto i : unmatched_trks) {
+                    left_trks.row(indx_for_trk++) = last_boxes.row(i);
+                }
+                Eigen::MatrixXf iou_left = giou_batch(left_dets, left_trks);
+                if (iou_left.maxCoeff() > effective_iou_threshold) {
+                    std::vector<std::vector<float>> iou_matrix(iou_left.rows(), std::vector<float>(iou_left.cols()));
+                    for (int i = 0; i < iou_left.rows(); i++) {
+                        for (int j = 0; j < iou_left.cols(); j++) {
+                            iou_matrix[i][j] = -iou_left(i, j);
+                        }
+                    }
+                    std::vector<int> rowsol, colsol;
+                    float MIN_cost = execLapjv(iou_matrix, rowsol, colsol, true, 0.01, true);
+                    std::vector<std::vector<int>> rematched_indices;
+                    for (int i = 0; i < rowsol.size(); i++) {
+                        if (rowsol.at(i) >= 0) {
+                            rematched_indices.push_back({ colsol.at(rowsol.at(i)), rowsol.at(i) });
+                        }
+                    }
+                    std::vector<int> to_remove_det_indices;
+                    std::vector<int> to_remove_trk_indices;
+                    for (auto i : rematched_indices) {
+                        int det_ind = unmatched_dets[i.at(0)];
+                        int trk_ind = unmatched_trks[i.at(1)];
+                        if (iou_left(i.at(0), i.at(1)) < effective_iou_threshold) {
+                            continue;
+                        }
+                        Eigen::Matrix<float, 9, 1> tmp_bbox;
+                        tmp_bbox = dets_first.block<1, 9>(det_ind, 0);
+                        trackers.at(trk_ind).update(&tmp_bbox);
+                        to_remove_det_indices.push_back(det_ind);
+                        to_remove_trk_indices.push_back(trk_ind);
+                    }
+                    std::vector<int> tmp_res(unmatched_dets.size());
+                    sort(unmatched_dets.begin(), unmatched_dets.end());
+                    sort(to_remove_det_indices.begin(), to_remove_det_indices.end());
+                    auto end = set_difference(unmatched_dets.begin(), unmatched_dets.end(),
+                        to_remove_det_indices.begin(), to_remove_det_indices.end(),
+                        tmp_res.begin());
+                    tmp_res.resize(end - tmp_res.begin());
+                    unmatched_dets = tmp_res;
+                    std::vector<int> tmp_res1(unmatched_trks.size());
+                    sort(unmatched_trks.begin(), unmatched_trks.end());
+                    sort(to_remove_trk_indices.begin(), to_remove_trk_indices.end());
+                    auto end1 = set_difference(unmatched_trks.begin(), unmatched_trks.end(),
+                        to_remove_trk_indices.begin(), to_remove_trk_indices.end(),
+                        tmp_res1.begin());
+                    tmp_res1.resize(end1 - tmp_res1.begin());
+                    unmatched_trks = tmp_res1;
+                }
+            }
+
+            for (auto m : unmatched_trks) {
+                trackers.at(m).update(nullptr);
+            }
+            for (int i : unmatched_dets) {
+                Eigen::RowVectorXf tmp_bbox = dets_first.block(i, 0, 1, 9);
+                KalmanBoxTracker trk(tmp_bbox, delta_t);
+                trackers.push_back(std::move(trk));
+            }
+            int tmp_i = trackers.size();
+            for (int i = trackers.size() - 1; i >= 0; i--) {
+                Eigen::Matrix<float, 1, 4> d;
+                int last_observation_sum = trackers.at(i).last_observation.sum();
+                if (last_observation_sum < 0) {
+                    d = trackers.at(i).get_state();
+                }
+                else {
+                    d = trackers.at(i).last_observation.block(0, 0, 1, 4);
+                }
+                if (trackers.at(i).time_since_update < 1 && ((trackers.at(i).hit_streak >= min_hits) | (frame_count <= min_hits))) {
+                    Eigen::RowVectorXf tracking_res(10);
+                    tracking_res << d(0), d(1), d(2), d(3),
+                        trackers.at(i).id + 1,              // Tracker Id
+                        trackers.at(i).GetClassId(),        // Class Id
+                        trackers.at(i).GetScore(),          // Score
+                        trackers.at(i).GetDGId(),           // DG Id
+                        trackers.at(i).GetCamId(),          // Camera Id
+                        trackers.at(i).GetModelId();        // Model Id
+                    ret.push_back(tracking_res);
+                }
+
+                if (trackers.at(i).time_since_update > max_age) {
+                    trackers.erase(trackers.begin() + i);
+                }
+            }
+            return ret;
+        }
+        catch (const std::exception& e) {
+            return ret;
+        }
+    }
+}// 

modules/ANSMOT/OCSort/src/OCSortKalmanFilter.cpp

@@ -0,0 +1,155 @@
+#include "OCSortKalmanFilter.h"
+#include <iostream>
+namespace ANSOCSort {
+    KalmanFilterNew::KalmanFilterNew() {};
+    KalmanFilterNew::KalmanFilterNew(int dim_x_, int dim_z_) {
+        dim_x = dim_x_;
+        dim_z = dim_z_;
+        x = Eigen::VectorXf::Zero(dim_x_, 1);
+        P = Eigen::MatrixXf::Identity(dim_x_, dim_x_);
+        Q = Eigen::MatrixXf::Identity(dim_x_, dim_x_);
+        B = Eigen::MatrixXf::Identity(dim_x_, dim_x_);
+        F = Eigen::MatrixXf::Identity(dim_x_, dim_x_);
+        H = Eigen::MatrixXf::Zero(dim_z_, dim_x_);
+        R = Eigen::MatrixXf::Identity(dim_z_, dim_z_);
+        M = Eigen::MatrixXf::Zero(dim_x_, dim_z_);
+        z = Eigen::VectorXf::Zero(dim_z_, 1);
+        K = Eigen::MatrixXf::Zero(dim_x_, dim_z_);
+        y = Eigen::VectorXf::Zero(dim_x_, 1);
+        S = Eigen::MatrixXf::Zero(dim_z_, dim_z_);
+        SI = Eigen::MatrixXf::Zero(dim_z_, dim_z_);
+
+        x_prior = x;
+        P_prior = P;
+        x_post = x;
+        P_post = P;
+    };
+    void KalmanFilterNew::predict() {
+        x = F * x;
+        P = _alpha_sq * ((F * P), F.transpose()) + Q;
+        x_prior = x;
+        P_prior = P;
+    }
+    void KalmanFilterNew::update(Eigen::VectorXf* z_) {
+        history_obs.push_back(z_);
+        if (z_ == nullptr) {
+            if (true == observed) freeze();
+            observed = false;
+            z = Eigen::VectorXf::Zero(dim_z, 1);
+            x_post = x;
+            P_post = P;
+            y = Eigen::VectorXf::Zero(dim_z, 1);
+            return;
+        }
+        if (false == observed) unfreeze();
+        observed = true;
+        y = *z_ - H * x;
+        auto PHT = P * H.transpose();
+        S = H * PHT + R;
+        K = PHT * SI;
+        x = x + K * y;
+        auto I_KH = I - K * H;
+        P = ((I_KH * P) * I_KH.transpose()) + ((K * R) * K.transpose());
+        z = *z_;
+        x_post = x;
+        P_post = P;
+    }
+    void KalmanFilterNew::freeze() {
+        attr_saved.IsInitialized = true;
+        attr_saved.x = x;
+        attr_saved.P = P;
+        attr_saved.Q = Q;
+        attr_saved.B = B;
+        attr_saved.F = F;
+        attr_saved.H = H;
+        attr_saved.R = R;
+        attr_saved._alpha_sq = _alpha_sq;
+        attr_saved.M = M;
+        attr_saved.z = z;
+        attr_saved.K = K;
+        attr_saved.y = y;
+        attr_saved.S = S;
+        attr_saved.SI = SI;
+        attr_saved.x_prior = x_prior;
+        attr_saved.P_prior = P_prior;
+        attr_saved.x_post = x_post;
+        attr_saved.P_post = P_post;
+        attr_saved.history_obs = history_obs;
+    }
+    void KalmanFilterNew::unfreeze() {
+        if (true == attr_saved.IsInitialized) {
+            new_history = history_obs;
+            x = attr_saved.x;
+            P = attr_saved.P;
+            Q = attr_saved.Q;
+            B = attr_saved.B;
+            F = attr_saved.F;
+            H = attr_saved.H;
+            R = attr_saved.R;
+            _alpha_sq = attr_saved._alpha_sq;
+            M = attr_saved.M;
+            z = attr_saved.z;
+            K = attr_saved.K;
+            y = attr_saved.y;
+            S = attr_saved.S;
+            SI = attr_saved.SI;
+            x_prior = attr_saved.x_prior;
+            P_prior = attr_saved.P_prior;
+            x_post = attr_saved.x_post;
+            history_obs.erase(history_obs.end() - 1);
+            Eigen::VectorXf box1;           
+            Eigen::VectorXf box2;           
+            int lastNotNullIndex = -1;     
+            int secondLastNotNullIndex = -1;
+            for (int i = new_history.size() - 1; i >= 0; i--) {
+                if (new_history[i] != nullptr) {
+                    if (lastNotNullIndex == -1) {
+                        lastNotNullIndex = i;
+                        box2 = *(new_history.at(lastNotNullIndex));
+                    }
+                    else if (secondLastNotNullIndex == -1) {
+                        secondLastNotNullIndex = i;
+                        box1 = *(new_history.at(secondLastNotNullIndex));
+                        break;
+                    }
+                }
+            }
+            double time_gap = lastNotNullIndex - secondLastNotNullIndex;
+            double x1 = box1[0];
+            double x2 = box2[0];
+            double y1 = box1[1];
+            double y2 = box2[1];
+            double w1 = std::sqrt(box1[2] * box1[3]);
+            double h1 = std::sqrt(box1[2] / box1[3]);
+            double w2 = std::sqrt(box1[2] * box1[3]);
+            double h2 = std::sqrt(box1[2] / box1[3]);
+            double dx = (x2 - x1) / time_gap;
+            double dy = (y1 - y2) / time_gap;
+            double dw = (w2 - w1) / time_gap;
+            double dh = (h2 - h1) / time_gap;
+
+            for (int i = 0; i < time_gap; i++) {
+                double x = x1 + (i + 1) * dx;
+                double y = y1 + (i + 1) * dy;
+                double w = w1 + (i + 1) * dw;
+                double h = h1 + (i + 1) * dh;
+                double s = w * h;
+                double r = w / (h * 1.0);
+                Eigen::VectorXf new_box(4, 1);
+                new_box << x, y, s, r;
+                this->y = new_box - this->H * this->x;
+                auto PHT = this->P * this->H.transpose();
+                this->S = this->H * PHT + this->R;
+                this->SI = (this->S).inverse();
+                this->K = PHT * this->SI;
+                this->x = this->x + this->K * this->y;
+                auto I_KH = this->I - this->K * this->H;
+                this->P = ((I_KH * this->P) * I_KH.transpose()) + ((this->K * this->R) * (this->K).transpose());
+                this->z = new_box;
+                this->x_post = this->x;
+                this->P_post = this->P;
+                if (i != (time_gap - 1)) predict();
+            }
+        } 
+    }
+}// namespace ocsort

modules/ANSMOT/OCSort/src/OCSortObject.cpp

@@ -0,0 +1,19 @@
+#include "OCSortObject.h"
+ANSOCSort::Object::Object(const ANSOCSort::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) :
+	rect(_rect),
+	label(_label),
+	prob(_prob),
+	left(_left),
+	top(_top),
+	right(_right),
+	bottom(_bottom),
+	object_id(_object_id)
+{
+}

modules/ANSMOT/OCSort/src/OCSortRect.cpp

@@ -0,0 +1,147 @@
+#include "OCSortRect.h"
+#include <algorithm>
+template <typename T>
+ANSOCSort::Rect<T>::Rect(const T& x, const T& y, const T& width, const T& height) :
+    tlwh({ x, y, width, height })
+{
+}
+
+template <typename T>
+ANSOCSort::Rect<T>::~Rect()
+{
+}
+
+template <typename T>
+const T& ANSOCSort::Rect<T>::x() const
+{
+    return tlwh[0];
+}
+
+template <typename T>
+const T& ANSOCSort::Rect<T>::y() const
+{
+    return tlwh[1];
+}
+
+template <typename T>
+const T& ANSOCSort::Rect<T>::width() const
+{
+    return tlwh[2];
+}
+
+template <typename T>
+const T& ANSOCSort::Rect<T>::height() const
+{
+    return tlwh[3];
+}
+
+template <typename T>
+T& ANSOCSort::Rect<T>::x()
+{
+    return tlwh[0];
+}
+
+template <typename T>
+T& ANSOCSort::Rect<T>::y()
+{
+    return tlwh[1];
+}
+
+template <typename T>
+T& ANSOCSort::Rect<T>::width()
+{
+    return tlwh[2];
+}
+
+template <typename T>
+T& ANSOCSort::Rect<T>::height()
+{
+    return tlwh[3];
+}
+
+template <typename T>
+const T& ANSOCSort::Rect<T>::tl_x() const
+{
+    return tlwh[0];
+}
+
+template <typename T>
+const T& ANSOCSort::Rect<T>::tl_y() const
+{
+    return tlwh[1];
+}
+
+template <typename T>
+T ANSOCSort::Rect<T>::br_x() const
+{
+    return tlwh[0] + tlwh[2];
+}
+
+template <typename T>
+T ANSOCSort::Rect<T>::br_y() const
+{
+    return tlwh[1] + tlwh[3];
+}
+
+template <typename T>
+ANSOCSort::Tlbr<T> ANSOCSort::Rect<T>::getTlbr() const
+{
+    return {
+        tlwh[0],
+        tlwh[1],
+        tlwh[0] + tlwh[2],
+        tlwh[1] + tlwh[3],
+    };
+}
+
+template <typename T>
+ANSOCSort::Xyah<T> ANSOCSort::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 ANSOCSort::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>
+ANSOCSort::Rect<T> ANSOCSort::generate_rect_by_tlbr(const ANSOCSort::Tlbr<T>& tlbr)
+{
+    return ANSOCSort::Rect<T>(tlbr[0], tlbr[1], tlbr[2] - tlbr[0], tlbr[3] - tlbr[1]);
+}
+
+template<typename T>
+ANSOCSort::Rect<T> ANSOCSort::generate_rect_by_xyah(const ANSOCSort::Xyah<T>& xyah)
+{
+    const auto width = xyah[2] * xyah[3];
+    return ANSOCSort::Rect<T>(xyah[0] - width / 2, xyah[1] - xyah[3] / 2, width, xyah[3]);
+}
+
+// explicit instantiation
+template class ANSOCSort::Rect<int>;
+template class ANSOCSort::Rect<float>;
+
+template ANSOCSort::Rect<int> ANSOCSort::generate_rect_by_tlbr<int>(const ANSOCSort::Tlbr<int>&);
+template ANSOCSort::Rect<float> ANSOCSort::generate_rect_by_tlbr<float>(const ANSOCSort::Tlbr<float>&);
+
+template ANSOCSort::Rect<int> ANSOCSort::generate_rect_by_xyah<int>(const ANSOCSort::Xyah<int>&);
+template ANSOCSort::Rect<float> ANSOCSort::generate_rect_by_xyah<float>(const ANSOCSort::Xyah<float>&);

modules/ANSMOT/OCSort/src/OCSortUtilities.cpp

@@ -0,0 +1,41 @@
+#include "OCSortUtilities.h"
+namespace ANSOCSort {
+    Eigen::VectorXf convert_bbox_to_z(Eigen::VectorXf bbox) {
+        double w = bbox[2] - bbox[0];
+        double h = bbox[3] - bbox[1];
+        double x = bbox[0] + w / 2.0;
+        double y = bbox[1] + h / 2.0;
+        double s = w * h;
+        double r = w / (h + 1e-6);
+        Eigen::MatrixXf z(4, 1);
+        z << x, y, s, r;
+        return z;
+    }
+    Eigen::VectorXf speed_direction(Eigen::VectorXf bbox1, Eigen::VectorXf bbox2) {
+        double cx1 = (bbox1[0] + bbox1[2]) / 2.0;
+        double cy1 = (bbox1[1] + bbox1[3]) / 2.0;
+        double cx2 = (bbox2[0] + bbox2[2]) / 2.0;
+        double cy2 = (bbox2[1] + bbox2[3]) / 2.0;
+        Eigen::VectorXf speed(2, 1);
+        speed << cy2 - cy1, cx2 - cx1;
+        double norm = sqrt(pow(cy2 - cy1, 2) + pow(cx2 - cx1, 2)) + 1e-6;
+        return speed / norm;
+    }
+    Eigen::VectorXf convert_x_to_bbox(Eigen::VectorXf x) {
+        float w = std::sqrt(x(2) * x(3));
+        float h = x(2) / w;
+        Eigen::VectorXf bbox = Eigen::VectorXf::Ones(4, 1);
+        bbox << x(0) - w / 2, x(1) - h / 2, x(0) + w / 2, x(1) + h / 2;
+        return bbox;
+    }
+    Eigen::VectorXf k_previous_obs(std::unordered_map<int, Eigen::VectorXf> observations_, int cur_age, int k) {
+        if (observations_.size() == 0) return Eigen::VectorXf::Constant(5, -1.0);
+        for (int i = 0; i < k; i++) {
+            int dt = k - i;
+            if (observations_.count(cur_age - dt) > 0) return observations_.at(cur_age - dt);
+        }
+        auto iter = std::max_element(observations_.begin(), observations_.end(), [](const std::pair<int, Eigen::VectorXf>& p1, const std::pair<int, Eigen::VectorXf>& p2) { return p1.first < p2.first; });
+        int max_age = iter->first;
+        return observations_[max_age];
+    }
+}// namespace ocsort

modules/ANSMOT/OCSort/src/OCSortlapjv.cpp

@@ -0,0 +1,455 @@
+#include "OCSortlapjv.h"
+#include <limits>
+#include <stdexcept>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <vector>
+
+namespace ANSOCSort {
+    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;
+    }
+    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) {
+        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) {
+                throw std::runtime_error("The `extend_cost` variable should set True");
+            }
+        }
+
+        if (extend_cost || cost_limit < std::numeric_limits<float>::max()) {
+            n = n_rows + n_cols;
+            cost_c_extended.resize(n);
+            for (size_t i = 0; i < cost_c_extended.size(); i++)
+                cost_c_extended[i].resize(n);
+
+            if (cost_limit < std::numeric_limits<float>::max()) {
+                for (size_t i = 0; i < cost_c_extended.size(); i++) {
+                    for (size_t j = 0; j < cost_c_extended[i].size(); j++) {
+                        cost_c_extended[i][j] = cost_limit / 2.0;
+                    }
+                }
+            }
+            else {
+                float cost_max = -1;
+                for (size_t i = 0; i < cost_c.size(); i++) {
+                    for (size_t j = 0; j < cost_c[i].size(); j++) {
+                        if (cost_c[i][j] > cost_max)
+                            cost_max = cost_c[i][j];
+                    }
+                }
+                for (size_t i = 0; i < cost_c_extended.size(); i++) {
+                    for (size_t j = 0; j < cost_c_extended[i].size(); j++) {
+                        cost_c_extended[i][j] = cost_max + 1;
+                    }
+                }
+            }
+
+            for (size_t i = n_rows; i < cost_c_extended.size(); i++) {
+                for (size_t 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());
+        }
+        float** cost_ptr;
+        cost_ptr = new float* [n];
+        for (int i = 0; i < n; i++)
+            cost_ptr[i] = new float[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;
+            throw std::runtime_error("The result of lapjv_internal() is invalid.");
+        }
+        float 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 (size_t i = 0; i < rowsol.size(); i++) {
+                    if (rowsol[i] != -1) {
+                        opt += cost_ptr[i][rowsol[i]];
+                    }
+                }
+            }
+        }
+        else if (return_cost) {
+            for (size_t 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;
+    }
+}

modules/ANSMOT/UCMC/include/UCMClapjv.h

@@ -0,0 +1,72 @@
+#ifndef LAPJV_H
+#define LAPJV_H
+
+#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 UCMC{
+    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);
+
+    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);
+
+
+}
+
+
+
+#endif // LAPJV_H

modules/ANSMOT/UCMC/src/UCMClapjv.cpp

@@ -0,0 +1,349 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "UCMClapjv.h"
+#include <opencv2/opencv.hpp>
+#include <Eigen/Eigen>
+#include <Eigen/Dense>
+/** Column-reduction and reduction transfer for a dense cost matrix.
+ */
+namespace UCMC {
+    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/UCMC/src/UCMClapmod.cpp

@@ -0,0 +1,652 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "UCMClapjv.h"
+
+/** Column-reduction and reduction transfer for a sparse cost matrix.
+ */
+namespace UCMC{
+    int_t _ccrrt_sparse(const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        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 k = ii[i]; k < ii[i + 1]; k++) {
+                const int_t j = kk[k];
+                const cost_t c = cc[k];
+                if (c < v[j]) {
+                    v[j] = c;
+                    y[j] = i;
+                }
+                PRINTF("i=%d, k=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, k, 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] && (ii[i + 1] - ii[i] > 1)) {
+                const int_t j = x[i];
+                cost_t min = LARGE;
+                for (uint_t k = ii[i]; k < ii[i + 1]; k++) {
+                    const int_t j2 = kk[k];
+                    if (j2 == j) {
+                        continue;
+                    }
+                    const cost_t c = cc[k] - 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 sparse cost matrix.
+     */
+    int_t _carr_sparse(
+        const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        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++];
+            if (ii[free_i + 1] - ii[free_i] > 0) {
+                const uint_t k = ii[free_i];
+                j1 = kk[k];
+                v1 = cc[k] - v[j1];
+            }
+            else {
+                j1 = 0;
+                v1 = LARGE;
+            }
+            j2 = -1;
+            v2 = LARGE;
+            for (uint_t k = ii[free_i] + 1; k < ii[free_i + 1]; k++) {
+                PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2);
+                const int_t j = kk[k];
+                const cost_t c = cc[k] - 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_sparse_1(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;
+    }
+
+
+    /** Find columns with minimum d[j] and put them on the SCAN list.
+     */
+    int_t _find_sparse_2(cost_t* d, int_t* scan, const uint_t n_todo, int_t* todo, boolean* done)
+    {
+        int_t hi = 0;
+        cost_t mind = LARGE;
+        for (uint_t k = 0; k < n_todo; k++) {
+            int_t j = todo[k];
+            if (done[j]) {
+                continue;
+            }
+            if (d[j] <= mind) {
+                if (d[j] < mind) {
+                    hi = 0;
+                    mind = d[j];
+                }
+                scan[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_sparse_1(
+        const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        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;
+
+        int_t* rev_kk;
+        NEW(rev_kk, int_t, n);
+
+        while (lo != hi) {
+            int_t kj;
+            int_t j = cols[lo++];
+            const int_t i = y[j];
+            const cost_t mind = d[j];
+            for (uint_t k = 0; k < n; k++) {
+                rev_kk[k] = -1;
+            }
+            for (uint_t k = ii[i]; k < ii[i + 1]; k++) {
+                const int_t j = kk[k];
+                rev_kk[j] = k;
+            }
+            PRINTF("?%d kk[%d:%d]=", j, ii[i], ii[i + 1]);
+            PRINT_INDEX_ARRAY(kk + ii[i], ii[i + 1] - ii[i]);
+            kj = rev_kk[j];
+            if (kj == -1) {
+                continue;
+            }
+            ASSERT(kk[kj] == j);
+            h = cc[kj] - v[j] - mind;
+            PRINTF("i=%d j=%d kj=%d h=%f\n", i, j, kj, h);
+            // For all columns in TODO
+            for (uint_t k = hi; k < n; k++) {
+                j = cols[k];
+                PRINTF("?%d kk[%d:%d]=", j, ii[i], ii[i + 1]);
+                PRINT_INDEX_ARRAY(kk + ii[i], ii[i + 1] - ii[i]);
+                if ((kj = rev_kk[j]) == -1) {
+                    continue;
+                }
+                ASSERT(kk[kj] == j);
+                cred_ij = cc[kj] - v[j] - h;
+                if (cred_ij < d[j]) {
+                    d[j] = cred_ij;
+                    pred[j] = i;
+                    if (cred_ij == mind) {
+                        if (y[j] < 0) {
+                            FREE(rev_kk);
+                            return j;
+                        }
+                        cols[k] = cols[hi];
+                        cols[hi++] = j;
+                    }
+                }
+            }
+        }
+        *plo = lo;
+        *phi = hi;
+        FREE(rev_kk);
+        return -1;
+    }
+
+
+    /** 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_sparse_2(
+        const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        uint_t* plo, uint_t* phi,
+        cost_t* d, int_t* pred,
+        boolean* done, uint_t* pn_ready, int_t* ready, int_t* scan,
+        uint_t* pn_todo, int_t* todo, boolean* added,
+        int_t* y, cost_t* v)
+    {
+        uint_t lo = *plo;
+        uint_t hi = *phi;
+        uint_t n_todo = *pn_todo;
+        uint_t n_ready = *pn_ready;
+        cost_t h, cred_ij;
+
+        int_t* rev_kk;
+        NEW(rev_kk, int_t, n);
+
+        for (uint_t k = 0; k < n; k++) {
+            rev_kk[k] = -1;
+        }
+        while (lo != hi) {
+            int_t kj;
+            int_t j = scan[lo++];
+            const int_t i = y[j];
+            ready[n_ready++] = j;
+            const cost_t mind = d[j];
+            for (uint_t k = ii[i]; k < ii[i + 1]; k++) {
+                const int_t j = kk[k];
+                rev_kk[j] = k;
+            }
+            PRINTF("?%d kk[%d:%d]=", j, ii[i], ii[i + 1]);
+            PRINT_INDEX_ARRAY(kk + ii[i], ii[i + 1] - ii[i]);
+            kj = rev_kk[j];
+            ASSERT(kj != -1);
+            ASSERT(kk[kj] == j);
+            h = cc[kj] - v[j] - mind;
+            PRINTF("i=%d j=%d kj=%d h=%f\n", i, j, kj, h);
+            // For all columns in TODO
+            for (uint_t k = 0; k < ii[i + 1] - ii[i]; k++) {
+                j = kk[ii[i] + k];
+                if (done[j]) {
+                    continue;
+                }
+                PRINTF("?%d kk[%d:%d]=", j, ii[i], ii[i + 1]);
+                PRINT_INDEX_ARRAY(kk + ii[i], ii[i + 1] - ii[i]);
+                cred_ij = cc[ii[i] + k] - v[j] - h;
+                if (cred_ij < d[j]) {
+                    d[j] = cred_ij;
+                    pred[j] = i;
+                    if (cred_ij <= mind) {
+                        if (y[j] < 0) {
+                            FREE(rev_kk);
+                            return j;
+                        }
+                        scan[hi++] = j;
+                        done[j] = TRUE;
+                    }
+                    else if (!added[j]) {
+                        todo[n_todo++] = j;
+                        added[j] = TRUE;
+                    }
+                }
+            }
+            for (uint_t k = ii[i]; k < ii[i + 1]; k++) {
+                const int_t j = kk[k];
+                rev_kk[j] = -1;
+            }
+        }
+        *pn_todo = n_todo;
+        *pn_ready = n_ready;
+        *plo = lo;
+        *phi = hi;
+        FREE(rev_kk);
+        return -1;
+    }
+
+
+    /** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper.
+     *
+     * This version loops over all column indices (some of which might be inf).
+     *
+     * \return The closest free column index.
+     */
+    int_t find_path_sparse_1(
+        const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        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;
+            d[i] = LARGE;
+            pred[i] = start_i;
+        }
+        for (uint_t i = ii[start_i]; i < ii[start_i + 1]; i++) {
+            const int_t j = kk[i];
+            d[j] = cc[i] - v[j];
+        }
+        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_sparse_1(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_sparse_1(
+                    n, cc, ii, kk, &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;
+    }
+
+
+    /** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper.
+     *
+     * This version loops over non-inf column indices (which requires some additional bookkeeping).
+     *
+     * \return The closest free column index.
+     */
+    int_t find_path_sparse_2(
+        const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        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;
+        uint_t n_todo = (ii[start_i + 1] - ii[start_i]);
+        boolean* done, * added;
+        int_t* ready, * scan, * todo;
+        cost_t* d;
+
+        NEW(done, boolean, n);
+        NEW(added, boolean, n);
+        NEW(ready, int_t, n);
+        NEW(scan, int_t, n);
+        NEW(todo, int_t, n);
+        NEW(d, cost_t, n);
+
+        memset(done, FALSE, n);
+        memset(added, FALSE, n);
+        for (uint_t i = 0; i < n; i++) {
+            d[i] = LARGE;
+            pred[i] = start_i;
+        }
+        for (uint_t i = ii[start_i]; i < ii[start_i + 1]; i++) {
+            const int_t j = kk[i];
+            d[j] = cc[i] - v[j];
+            todo[i - ii[start_i]] = j;
+            added[j] = TRUE;
+        }
+        PRINT_COST_ARRAY(d, n);
+        PRINT_INDEX_ARRAY(pred, n);
+        PRINT_INDEX_ARRAY(done, n);
+        PRINT_INDEX_ARRAY(ready, n_ready);
+        PRINT_INDEX_ARRAY(scan + lo, hi - lo);
+        PRINT_INDEX_ARRAY(todo, n_todo);
+        PRINT_INDEX_ARRAY(added, n);
+        while (final_j == -1) {
+            // No columns left on the SCAN list.
+            if (lo == hi) {
+                PRINTF("%d..%d -> find\n", lo, hi);
+                lo = 0;
+                hi = _find_sparse_2(d, scan, n_todo, todo, done);
+                PRINTF("check %d..%d\n", lo, hi);
+                if (!hi) {
+                    // XXX: the assignment is unsolvable, lets try to return
+                    // something reasonable nevertheless.
+                    for (uint_t j = 0; j < n; j++) {
+                        if (!done[j] && y[j] < 0) {
+                            final_j = j;
+                        }
+                    }
+                    ASSERT(final_j != -1);
+                    break;
+                }
+                ASSERT(hi > lo);
+                for (uint_t k = lo; k < hi; k++) {
+                    const int_t j = scan[k];
+                    if (y[j] < 0) {
+                        final_j = j;
+                    }
+                    else {
+                        done[j] = TRUE;
+                    }
+                }
+            }
+            if (final_j == -1) {
+                PRINTF("%d..%d -> scan\n", lo, hi);
+                PRINT_INDEX_ARRAY(done, n);
+                PRINT_INDEX_ARRAY(ready, n_ready);
+                PRINT_INDEX_ARRAY(scan + lo, hi - lo);
+                PRINT_INDEX_ARRAY(todo, n_todo);
+                final_j = _scan_sparse_2(
+                    n, cc, ii, kk, &lo, &hi, d, pred,
+                    done, &n_ready, ready, scan,
+                    &n_todo, todo, added,
+                    y, v);
+                PRINT_COST_ARRAY(d, n);
+                PRINT_INDEX_ARRAY(pred, n);
+                PRINT_INDEX_ARRAY(done, n);
+                PRINT_INDEX_ARRAY(ready, n_ready);
+                PRINT_INDEX_ARRAY(scan + lo, hi - lo);
+                PRINT_INDEX_ARRAY(todo, n_todo);
+                PRINT_INDEX_ARRAY(added, n);
+            }
+        }
+
+        PRINTF("found final_j=%d\n", final_j);
+        {
+            const cost_t mind = d[scan[lo]];
+            for (uint_t k = 0; k < n_ready; k++) {
+                const int_t j = ready[k];
+                v[j] += d[j] - mind;
+            }
+        }
+
+        FREE(done);
+        FREE(added);
+        FREE(ready);
+        FREE(scan);
+        FREE(todo);
+        FREE(d);
+
+        return final_j;
+    }
+
+
+    /** Find path using one of the two find_path variants selected based on sparsity.
+     */
+    int_t find_path_sparse_dynamic(
+        const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        const int_t start_i,
+        int_t* y, cost_t* v,
+        int_t* pred)
+    {
+        const uint_t n_i = ii[start_i + 1] - ii[start_i];
+        // XXX: wouldnt it be better to decide for the whole matrix?
+        if (n_i > 0.25 * n) {
+            return find_path_sparse_1(n, cc, ii, kk, start_i, y, v, pred);
+        }
+        else {
+            return find_path_sparse_2(n, cc, ii, kk, start_i, y, v, pred);
+        }
+    }
+
+
+    typedef int_t(*fp_function_t)(
+        const uint_t, cost_t*, uint_t*, uint_t*, const int_t, int_t*, cost_t*, int_t*);
+
+    fp_function_t get_better_find_path(const uint_t n, uint_t* ii)
+    {
+        const double sparsity = ii[n] / (double)(n * n);
+        if (sparsity > 0.25) {
+            PRINTF("Using find_path_sparse_1 for sparsity=%f\n", sparsity);
+            return find_path_sparse_1;
+        }
+        else {
+            PRINTF("Using find_path_sparse_2 for sparsity=%f\n", sparsity);
+            return find_path_sparse_2;
+        }
+    }
+
+
+    /** Augment for a sparse cost matrix.
+     */
+    int_t _ca_sparse(
+        const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
+        const uint_t n_free_rows,
+        int_t* free_rows, int_t* x, int_t* y, cost_t* v,
+        int fp_version)
+    {
+        int_t* pred;
+
+        NEW(pred, int_t, n);
+
+        fp_function_t fp;
+        switch (fp_version) {
+        case FP_1: fp = find_path_sparse_1; break;
+        case FP_2: fp = find_path_sparse_2; break;
+        case FP_DYNAMIC: fp = get_better_find_path(n, ii); break;
+        default: return -2;
+        }
+
+        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 = fp(n, cc, ii, kk, *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 square sparse LAP.
+     */
+    int 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)
+    {
+        int ret;
+        int_t* free_rows;
+        cost_t* v;
+
+        NEW(free_rows, int_t, n);
+        NEW(v, cost_t, n);
+        ret = _ccrrt_sparse(n, cc, ii, kk, free_rows, x, y, v);
+        int i = 0;
+        while (ret > 0 && i < 2) {
+            ret = _carr_sparse(n, cc, ii, kk, ret, free_rows, x, y, v);
+            i++;
+        }
+        if (ret > 0) {
+            ret = _ca_sparse(n, cc, ii, kk, ret, free_rows, x, y, v, fp_version);
+        }
+        FREE(v);
+        FREE(free_rows);
+        return ret;
+    }
+
+}

modules/ANSMOT/UCMCKalman.cpp

@@ -0,0 +1,252 @@
+#pragma once
+
+#include <opencv2/opencv.hpp>
+#include <cmath>
+#include <iostream>
+#include "UCMClapjv.h"
+#include "UCMCKalman.h"
+
+namespace UCMCKalman {
+
+    void lapjv(const Eigen::MatrixXd& costMatrix, double costLimit,
+        std::vector<int>& x, std::vector<int>& y) {
+        //
+        int n_rows = costMatrix.rows();
+        int n_cols = costMatrix.cols();
+        x.clear();
+        y.clear();
+        x.resize(n_rows);
+        y.resize(n_cols);
+        const int n = n_rows + n_cols;
+
+        costLimit /= 2.;
+
+        double** cost_ptr = new double* [n];
+        for (int i = 0; i < n; i++) {
+            cost_ptr[i] = new double[n];
+            for (int j = 0; j < n; j++) {
+                if (i < n_rows && j < n_cols) {
+                    cost_ptr[i][j] = costMatrix(i, j);
+                }
+                else if (i >= n_rows && j >= n_cols) {
+                    cost_ptr[i][j] = 0.;
+                }
+                else {
+                    cost_ptr[i][j] = costLimit;
+                }
+            }
+        }
+
+        UCMC::int_t* x_c = new UCMC::int_t[n];
+        UCMC::int_t* y_c = new UCMC::int_t[n];
+        UCMC::lapjv_internal(n, cost_ptr, &x_c[0], &y_c[0]);
+
+        for (int i = 0; i < n; i++) delete[] cost_ptr[i];
+        delete[] cost_ptr;
+
+        for (int i = 0; i < n_rows; i++) {
+            x[i] = (x_c[i] >= n_cols) ? -1 : x_c[i];
+        }
+
+        for (int i = 0; i < n_cols; i++) {
+            y[i] = (y_c[i] >= n_rows) ? -1 : y_c[i];
+        }
+        delete[] x_c;
+        delete[] y_c;
+
+    }
+
+
+    void linearAssignment(const Eigen::MatrixXd& costMatrix, double thresh,
+        std::vector<std::vector<int>>& matches,
+        std::vector<int>& unmatched_a,
+        std::vector<int>& unmatched_b) {
+
+        matches.clear();
+        unmatched_a.clear();
+        unmatched_b.clear();
+
+        int n = costMatrix.rows();
+        int m = costMatrix.cols();
+
+        if (n == 0 || m == 0) {
+            for (int i = 0; i < n; i++) unmatched_a.push_back(i);
+            for (int i = 0; i < m; i++) unmatched_b.push_back(i);
+            return;
+        }
+
+        std::vector<int> x, y;
+
+        lapjv(costMatrix, thresh, x, y);
+
+        for (int i = 0; i < n; ++i) {
+            if (x[i] < 0) {
+                unmatched_a.push_back(i);
+            }
+            else {
+                std::vector<int> this_match;
+                this_match.push_back(i);
+                this_match.push_back(x[i]);
+                matches.push_back(this_match);
+            }
+        }
+        for (int j = 0; j < m; ++j) {
+            if (y[j] < 0) {
+                unmatched_b.push_back(j);
+            }
+        }
+    }
+
+
+    Filter:: Filter(int dim_x, int dim_z, int dim_u)
+            : dim_x(dim_x), dim_z(dim_z), dim_u(dim_u),
+            x(dim_x, 1), P(dim_x, dim_x), Q(dim_x, dim_x),
+            F(dim_x, dim_x), H(dim_z, dim_x), R(dim_z, dim_z),
+            _alpha_sq(1.0), M(dim_z, dim_z), z(dim_z, 1),
+            K(dim_x, dim_z), y(dim_z, 1), S(dim_z, dim_z),
+            SI(dim_z, dim_z), _I(dim_x, dim_x),
+            x_prior(dim_x, 1), P_prior(dim_x, dim_x),
+            x_post(dim_x, 1), P_post(dim_x, dim_x) {
+            x.setZero();
+            P.setIdentity();
+            Q.setIdentity();
+            F.setIdentity();
+            H.setZero();
+            R.setIdentity();
+            M.setZero();
+            z.setOnes()* std::numeric_limits<float>::quiet_NaN();
+            K.setZero();
+            y.setZero();
+            S.setZero();
+            SI.setZero();
+            _I.setIdentity();
+            x_prior = x;
+            P_prior = P;
+            x_post = x;
+            P_post = P;
+        }
+    void Filter::predict() {
+            x = F * x;
+            P = _alpha_sq * (F * P * F.transpose()) + Q;
+            x_prior = x;
+            P_prior = P;
+        }
+    void  Filter::update(const Eigen::VectorXd& z,
+            const Eigen::MatrixXd& R) {
+            if (R.rows() != dim_z || R.cols() != dim_z) {
+                throw std::invalid_argument("R matrix has incorrect dimensions");
+            }
+
+            // Update state
+            y = z - H * x;
+            Eigen::MatrixXd PHT = P_prior * H.transpose();
+            S = H * PHT + R;
+            SI = S.inverse();
+            K = PHT * S.inverse();
+            x += K * y;
+
+            // Update covariance
+            Eigen::MatrixXd I_KH = _I - K * H;
+            P = (I_KH * P_prior) * I_KH.transpose() + (K * R) * K.transpose();
+
+            // Prepare for next prediction
+            this->z = z;
+            x_post = x;
+            P_post = P;
+
+        }
+
+    Eigen::VectorXd  Filter::getState() const {
+            return x;
+        }
+    Eigen::MatrixXd  Filter::getCovariance() const {
+            return P;
+        }
+
+
+
+    void Tracker::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;
+        }
+    }
+
+    Tracker::Tracker(Eigen::Vector2d y, Eigen::Matrix2d R, double wx, double wy,
+        double vmax, double w, double h, double dt, int tracker_count)
+        : id(tracker_count), age(0), death_count(0), birth_count(0), detidx(-1),
+        w(w), h(h), status(TrackStatus::Tentative), kf(4, 2),
+        class_id_(0), detection_count_(0), class_id_locked_(false) {
+        kf.F = Eigen::Matrix4d::Identity();
+        kf.F(0, 1) = dt;
+        kf.F(2, 3) = dt;
+        kf.H = Eigen::Matrix<double, 2, 4>::Zero();
+        kf.H(0, 0) = 1;
+        kf.H(1, 2) = 1;
+        kf.R = R;
+        kf.P.setZero(); //= Eigen::Matrix<double, 4, 4>::Zero();
+        kf.P << 1, 0, 0, 0,
+            0, vmax* vmax / 3.0, 0, 0,
+            0, 0, 1, 0,
+            0, 0, 0, vmax* vmax / 3.0;
+
+        // INFO << "P:\n" << kf.P << ENDL;
+
+        Eigen::Matrix<double, 4, 2> G;
+        G << 0.5 * dt * dt, 0,
+            dt, 0,
+            0, 0.5 * dt * dt,
+            0, dt;
+        Eigen::Matrix<double, 2, 2> Q0;
+        Q0.setZero();
+        Q0(0, 0) = wx;
+        Q0(1, 1) = wy;
+
+        Eigen::MatrixXd _Q = (G * Q0) * G.transpose();
+        kf.Q = _Q;
+
+        kf.x << y(0), 0, y(1), 0;
+    }
+
+    void Tracker::update(const Eigen::Vector2d& y, Eigen::Matrix2d& R) {
+        kf.update(y, R);
+    }
+
+    Eigen::Vector2d Tracker::predict() {
+        kf.predict();
+        this->age += 1;
+        return kf.H * kf.x;
+    }
+
+    Eigen::Vector4d Tracker::getState() const {
+        return kf.x;
+    }
+
+    double Tracker::distance(const Eigen::Vector2d& y, Eigen::Matrix2d& R, bool show = false) const {
+        Eigen::Vector2d diff = y - (kf.H * kf.x);
+        Eigen::Matrix2d S = kf.H * (kf.P * kf.H.transpose()) + R;
+        Eigen::Matrix2d SI = S.inverse();
+        auto mahalanobis = diff.transpose() * (SI * diff);
+        double logdet = log(S.determinant());
+
+        return mahalanobis(0, 0) + logdet;
+    }
+}

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

modules/ANSMOT/UCMCTracker.cpp

@@ -0,0 +1,528 @@
+
+#include <opencv2/opencv.hpp>
+#include <Eigen/Eigen>
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+#include <algorithm>
+#include "UCMClapjv.h"
+#include "UCMCTracker.h"
+
+namespace UCMC 
+{
+   
+    Mapper::Mapper(std::vector<double>& _Ki, std::vector<double>& _Ko) {
+        Eigen::Map<Eigen::Matrix<double, 4, 3>> KiT(_Ki.data());
+        Eigen::Map<Eigen::Matrix4d> KoT(_Ko.data());
+
+        Eigen::Matrix<double, 3, 4> Ki = KiT.transpose();
+        Eigen::Matrix4d Ko = KoT.transpose();
+
+        A.setZero();
+        InvA.setZero();
+        KiKo = Ki * Ko;
+
+        for (int now_row = 0; now_row < 3; now_row++)
+            for (int now_col = 0; now_col < 3; now_col++) {
+                A(now_col, now_row) = KiKo(now_col, now_row);
+                if (now_row == 2) {
+                    A(now_col, now_row) = KiKo(now_col, 3);
+                }
+            }
+
+        InvA = A.inverse();
+
+    }
+
+    std::vector<double> Mapper::uvError(cv::Rect box) {
+        std::vector<double> uv;
+        uv.resize(2);
+        uv[0] = MAX(MIN(13., 0.05 * box.width), 2.);
+        uv[1] = MAX(MIN(10., 0.05 * box.height), 2.);
+        return uv;
+    }
+
+    void Mapper::uv2xy(Eigen::MatrixXd uv, Eigen::MatrixXd sigma_uv,
+        Eigen::Matrix<double, 2, 1>& xy, Eigen::Matrix2d& sigma_xy) {
+        Eigen::Matrix<double, 3, 1> uv1;
+        uv1(0, 0) = uv(0, 0);
+        uv1(1, 0) = uv(1, 0);
+        uv1(2, 0) = 1.;
+
+
+        Eigen::MatrixXd b = InvA * uv1;
+
+        if (std::abs(b(2, 0)) < 1e-10) {
+            xy.setZero();
+            sigma_xy.setZero();
+            return;
+        }
+        double gamma = 1. / b(2, 0);
+
+        Eigen::Matrix2d C = gamma * InvA.block(0, 0, 2, 2)
+            - (gamma * gamma) * b.block(0, 0, 2, 1) * InvA.block(2, 0, 1, 2);
+
+        // INFO << "C:\n" << C << ENDL;
+        // INFO << "sigma_uv:\n" << sigma_uv << ENDL;
+
+        xy = b.block(0, 0, 2, 1) * gamma;
+        sigma_xy = (C * sigma_uv) * C.transpose();
+
+    }
+
+
+    void Mapper::map_to(cv::Rect box, Eigen::Matrix<double, 2, 1>& y, Eigen::Matrix2d& R) {
+        Eigen::Matrix<double, 2, 1> uv;
+        uv(0, 0) = box.x + 0.5 * box.width;
+        uv(1, 0) = box.y + box.height;
+        std::vector<double> uv_err = uvError(box);
+        Eigen::MatrixXd sigma_uv = Eigen::Matrix2d::Identity();
+        sigma_uv(0, 0) = uv_err[1] * uv_err[1];
+        sigma_uv(1, 1) = uv_err[1] * uv_err[1];
+        uv2xy(uv, sigma_uv, y, R);
+    }
+
+    //  Tracker
+    void Tracker::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) {
+            this->params.a1 = a1;
+            this->params.a2 = a2;
+            this->params.vmax = vmax;
+            this->params.max_age = std::max(5.0, max_age);
+            this->params.high_score = high_score;
+            this->params.conf_threshold = conf_threshold;
+            this->params.dt = dt;
+            this->params.Ki = Ki;
+            this->params.Ko = Ko;
+            mapper = Mapper(params.Ki, params.Ko);
+
+            // Frame rate auto-estimation
+            auto_frame_rate_ = autoFrameRate;
+            estimated_fps_ = 30.0f;
+            time_scale_factor_ = 1.0f;
+            fps_sample_count_ = 0;
+            has_last_update_time_ = false;
+    }
+
+    float Tracker::getEstimatedFps() const {
+        return estimated_fps_;
+    }
+
+    void Tracker::estimateFrameRate() {
+        if (!auto_frame_rate_) return;
+
+        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) return; // Skip near-zero intervals
+
+        float instant_fps = static_cast<float>(1.0 / delta_sec);
+
+        // EMA smoothing: alpha=0.3 for warmup (first 10 samples), alpha=0.1 steady state
+        fps_sample_count_++;
+        float alpha = (fps_sample_count_ <= 10) ? 0.3f : 0.1f;
+        estimated_fps_ = alpha * instant_fps + (1.0f - alpha) * estimated_fps_;
+
+        // Clamp estimated fps to a reasonable range
+        estimated_fps_ = std::max(1.0f, std::min(estimated_fps_, 120.0f));
+
+        // 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));
+    }
+
+    Tracker::Tracker(Params& params)
+    {
+        this->params = params;
+        mapper = Mapper(params.Ki, params.Ko);
+    }
+
+    std::vector<Obj> Tracker::update(std::vector<Object>& det_results) {
+        // Estimate frame rate from timing of update() calls
+        estimateFrameRate();
+
+        std::vector<Obj> dets;
+        int id = 0;
+        for (Object obj : det_results) {
+            Obj this_obj;
+            this_obj.id = id++;
+            this_obj.obj = obj;
+            mapper.map_to(obj.rect, this_obj.y, this_obj.R);
+
+            dets.push_back(this_obj);
+        }
+        this->update(dets, frame_idx++);
+        return dets;
+    }
+
+    void Tracker::update(std::vector<Obj>& dets, int frame_id) {
+        this->data_association(dets, frame_id);
+        this->associate_tentative(dets);
+        this->initial_tentative(dets);
+        this->delete_old_trackers();
+        this->update_status(dets);
+    }
+
+    void Tracker::data_association(std::vector<Obj>& dets, int frame_id) {
+        std::vector<int> detidx_high, detidx_low;
+        for (size_t i = 0; i < dets.size(); ++i) {
+            if (dets[i].obj.prob >= params.high_score) {
+                detidx_high.push_back(i);
+            }
+            else {
+                detidx_low.push_back(i);
+            }
+        }
+
+        // Multi-predict: call 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++) {
+            for (auto& track : trackers) {
+                track.predict();
+            }
+        }
+
+        std::vector<int> trackidx_remain;
+        detidx_remain.clear();
+
+        // INFO << "try high score mapping" << ENDL;
+        std::vector<int> trackidx = confirmed_idx;
+        trackidx.insert(trackidx.end(), coasted_idx.begin(), coasted_idx.end());
+
+        int num_det = detidx_high.size();
+        int num_trk = trackidx.size();
+
+        for (auto& track : trackers) {
+            track.detidx = -1;
+        }
+
+        // Adaptive association thresholds: relax during frame skips
+        // Multi-predict already increases covariance (making Mahalanobis more lenient),
+        // but we add a small relaxation for extra tolerance
+        double effective_a1 = params.a1;
+        double effective_a2 = params.a2;
+        if (num_predicts > 1) {
+            effective_a1 = params.a1 * (1.0 + 0.1 * (num_predicts - 1));
+            effective_a2 = params.a2 * (1.0 + 0.1 * (num_predicts - 1));
+        }
+
+        if (num_det * num_trk > 0) {
+            Eigen::MatrixXd cost_matrix(num_det, num_trk);
+            cost_matrix.setZero();
+
+            for (int i = 0; i < num_det; ++i) {
+                int det_idx = detidx_high[i];
+                for (int j = 0; j < num_trk; ++j) {
+                    cost_matrix(i, j) = trackers[trackidx[j]].distance(
+                        dets[det_idx].y,
+                        dets[det_idx].R,
+                        i + j == 0 && debug
+                    );
+                }
+            }
+
+            // INFO << "det[0].y:\n" << dets[0].y << ENDL;
+            // INFO << "det[0].R:\n" << dets[0].R << ENDL;
+            // INFO << "cost matrix: " << cost_matrix << ENDL;
+
+            std::vector<std::vector<int>> matched_indices;
+            std::vector<int> unmatched_a;
+            std::vector<int> unmatched_b;
+            UCMCKalman::linearAssignment(cost_matrix, effective_a1,
+                matched_indices, unmatched_a, unmatched_b);
+
+            // INFO << "high: " << detidx_remain.size() << " " << unmatched_a.size() << " "
+            //      << unmatched_b.size() << ENDL;
+
+            for (int i : unmatched_a) {
+                detidx_remain.push_back(detidx_high[i]);
+            }
+            for (int i : unmatched_b) {
+                trackidx_remain.push_back(trackidx[i]);
+            }
+
+            for (size_t i = 0; i < matched_indices.size(); ++i) {
+                int det_idx = detidx_high[matched_indices[i][0]];
+                int trk_idx = trackidx[matched_indices[i][1]];
+
+                trackers[trk_idx].update(dets[det_idx].y, dets[det_idx].R);
+                trackers[trk_idx].death_count = 0;
+                trackers[trk_idx].detidx = det_idx;
+                trackers[trk_idx].status = UCMCKalman::TrackStatus::Confirmed;
+                trackers[trk_idx].voteClassId(dets[det_idx].obj.label, dets[det_idx].obj.prob);
+                dets[det_idx].obj.label = trackers[trk_idx].class_id_;
+
+                dets[det_idx].track_idx = trackers[trk_idx].id;
+
+            }
+        }
+        else {
+            detidx_remain = detidx_high;
+            trackidx_remain = trackidx;
+        }
+
+
+        int num_det_low = detidx_low.size();
+        int num_trk_remain = trackidx_remain.size();
+
+        // INFO << "try low score mapping" << ENDL;
+        if (num_det_low * num_trk_remain > 0) {
+            Eigen::MatrixXd cost_matrix(num_det_low, num_trk_remain);
+            cost_matrix.setZero();
+
+            for (int i = 0; i < num_det_low; ++i) {
+                int det_idx = detidx_low[i];
+                for (int j = 0; j < num_trk_remain; ++j) {
+                    int trk_idx = trackidx_remain[j];
+                    cost_matrix(i, j) = trackers[trk_idx].distance(
+                        dets[det_idx].y,
+                        dets[det_idx].R,
+                        i + j == 0 && debug
+                    );
+                }
+            }
+
+            // INFO << "det[0].y:\n" << dets[0].y << ENDL;
+            // INFO << "det[0].R:\n" << dets[0].R << ENDL;
+
+            std::vector<std::vector<int>> matched_indices;
+            std::vector<int> unmatched_a;
+            std::vector<int> unmatched_b;
+            UCMCKalman::linearAssignment(cost_matrix, effective_a2,
+                matched_indices, unmatched_a, unmatched_b);
+
+            for (int i : unmatched_b) {
+                int trk_idx = trackidx_remain[i];
+                trackers[trk_idx].status = UCMCKalman::TrackStatus::Coasted;
+                // trackers[trk_idx].death_count += 1; 
+                trackers[trk_idx].detidx = -1;
+            }
+
+            for (size_t i = 0; i < matched_indices.size(); ++i) {
+                int det_idx = detidx_low[matched_indices[i][0]];
+                int trk_idx = trackidx_remain[matched_indices[i][1]];
+
+                trackers[trk_idx].update(dets[det_idx].y, dets[det_idx].R);
+                trackers[trk_idx].death_count = 0;
+                trackers[trk_idx].detidx = det_idx;
+                trackers[trk_idx].status = UCMCKalman::TrackStatus::Confirmed;
+                trackers[trk_idx].voteClassId(dets[det_idx].obj.label, dets[det_idx].obj.prob);
+                dets[det_idx].obj.label = trackers[trk_idx].class_id_;
+                dets[det_idx].track_idx = trackers[trk_idx].id;
+            }
+        }
+
+    }
+
+    void Tracker::associate_tentative(std::vector<Obj>& dets) {
+        // INFO << "try associate_tentative mapping" << ENDL;
+        size_t num_det = detidx_remain.size();
+        size_t num_trk = tentative_idx.size();
+
+        Eigen::MatrixXd cost_matrix(num_det, num_trk);
+        cost_matrix.setZero();
+
+        for (int i = 0; i < num_det; ++i) {
+            int det_idx = detidx_remain[i];
+            for (int j = 0; j < num_trk; ++j) {
+                int trk_idx = tentative_idx[j];
+                cost_matrix(i, j) = trackers[trk_idx].distance(
+                    dets[det_idx].y,
+                    dets[det_idx].R,
+                    i + j == 0 && debug
+                );
+            }
+        }
+
+        // INFO << "det[0].y:\n" << dets[0].y << ENDL;
+        // INFO << "det[0].R:\n" << dets[0].R << ENDL;
+
+        std::vector<std::vector<int>> matched_indices;
+        std::vector<int> unmatched_a;
+        std::vector<int> unmatched_b;
+        UCMCKalman::linearAssignment(cost_matrix, this->params.a1,
+            matched_indices, unmatched_a, unmatched_b);
+
+        // INFO << detidx_remain.size() << " " << unmatched_a.size() << " "
+        //      << unmatched_b.size() << ENDL;
+
+        for (size_t i = 0; i < matched_indices.size(); ++i) {
+            int det_idx = detidx_remain[matched_indices[i][0]];
+            int trk_idx = tentative_idx[matched_indices[i][1]];
+
+            trackers[trk_idx].update(dets[det_idx].y, dets[det_idx].R);
+            trackers[trk_idx].death_count = 0;
+            trackers[trk_idx].birth_count++;
+            trackers[trk_idx].detidx = det_idx;
+            trackers[trk_idx].voteClassId(dets[det_idx].obj.label, dets[det_idx].obj.prob);
+            dets[det_idx].obj.label = trackers[trk_idx].class_id_;
+            dets[det_idx].track_idx = trackers[trk_idx].id;
+            if (trackers[trk_idx].birth_count >= 2) {
+                trackers[trk_idx].birth_count = 0;
+                trackers[trk_idx].status = UCMCKalman::TrackStatus::Confirmed;
+            }
+        }
+
+        for (int i : unmatched_b) {
+            int trk_idx = tentative_idx[i];
+            trackers[trk_idx].detidx--;
+        }
+
+        std::vector<int> unmatched_detidx;
+        for (int i : unmatched_a) {
+            unmatched_detidx.push_back(detidx_remain[i]);
+        }
+        detidx_remain = unmatched_detidx;
+    }
+
+    void Tracker::initial_tentative(std::vector<Obj>& dets) {
+        // INFO << detidx_remain.size() << ENDL;
+        for (int i : detidx_remain) {
+            // INFO << "detidx:" << i << "" << dets[i].obj.rect << ENDL;
+            UCMCKalman::Tracker new_obj(
+                dets[i].y,
+                dets[i].R,
+                this->params.wx,
+                this->params.wy,
+                this->params.vmax,
+                dets[i].obj.rect.width,
+                dets[i].obj.rect.height,
+                this->params.dt,
+                ++this->tracker_count
+            );
+            new_obj.status = UCMCKalman::TrackStatus::Tentative;
+            new_obj.detidx = i;
+            new_obj.class_id_ = dets[i].obj.label;
+            new_obj.voteClassId(dets[i].obj.label, dets[i].obj.prob);
+            trackers.push_back(new_obj);
+        }
+        detidx_remain.clear();
+    }
+
+    void Tracker::delete_old_trackers() {
+        std::vector<int> idx_reserve;
+        std::vector<UCMCKalman::Tracker> reserve_trackers;
+        for (int trk_idx = 0; trk_idx < trackers.size(); trk_idx++) {
+            trackers[trk_idx].death_count++;
+            if (!((trackers[trk_idx].status == UCMCKalman::TrackStatus::Coasted &&
+                trackers[trk_idx].death_count >= this->params.max_age) ||
+                (trackers[trk_idx].status == UCMCKalman::TrackStatus::Tentative &&
+                    trackers[trk_idx].death_count >= 2))) {
+                reserve_trackers.push_back(trackers.at(trk_idx));
+            }
+        }
+        trackers = reserve_trackers;
+    }
+
+    void Tracker::update_status(std::vector<Obj>& dets) {
+        confirmed_idx.clear();
+        coasted_idx.clear();
+        tentative_idx.clear();
+
+        for (int i = 0; i < trackers.size(); i++) {
+            int detidx = trackers[i].detidx;
+
+            if (detidx >= 0 && detidx < dets.size()) {
+                trackers[i].h = dets[detidx].obj.rect.height;
+                trackers[i].w = dets[detidx].obj.rect.width;
+            }
+
+            switch (trackers[i].status)
+            {
+            case UCMCKalman::TrackStatus::Confirmed:
+                confirmed_idx.push_back(i);
+                break;
+            case UCMCKalman::TrackStatus::Coasted:
+                coasted_idx.push_back(i);
+                break;
+            case UCMCKalman::TrackStatus::Tentative:
+                tentative_idx.push_back(i);
+                break;
+            default:
+                break;
+            }
+
+        }
+    }
+
+    Tracker::~Tracker() {
+        trackers.clear();
+        confirmed_idx.clear();
+        coasted_idx.clear();
+        tentative_idx.clear();
+        detidx_remain.clear();
+    }
+
+
+
+    static cv::Scalar get_color(int index) {
+        index %= 20;
+        return cv::Scalar(color_list[index][2], color_list[index][1], color_list[index][0]);
+    }
+    static cv::Mat draw_boxes(cv::Mat image,
+        std::vector<UCMC::Obj>& objects,
+        std::vector<std::string>& names,
+        int draw_size,
+        bool draw_label) {
+        cv::Mat d_img = image.clone();
+        cv::Scalar color;
+        cv::Scalar txt_color;
+        cv::Scalar txt_bk_color;
+        cv::Size label_size;
+        int baseLine = 0;
+        int x, y, out_point_y, w, h;
+        int line_thickness = static_cast<int>(std::round(static_cast<double>(draw_size) / 10.0));
+
+        for (int k = 0; k < objects.size(); k++) {
+
+            if (!objects.at(k).track_idx) continue;
+            color = get_color(objects.at(k).obj.label);
+
+            x = objects.at(k).obj.rect.x;
+            y = objects.at(k).obj.rect.y;
+            w = objects.at(k).obj.rect.width;
+            h = objects.at(k).obj.rect.height;
+
+            cv::rectangle(d_img,
+                objects.at(k).obj.rect,
+                color,
+                line_thickness);
+
+            if (draw_label) {
+                txt_color = (cv::mean(color)[0] > 127) ? cv::Scalar(0, 0, 0) : cv::Scalar(255, 255, 255);
+                std::string label = std::to_string(objects.at(k).track_idx) + " " + names.at(objects.at(k).obj.label) + " " + std::to_string(objects.at(k).obj.prob).substr(0, 4);
+                label_size = cv::getTextSize(label.c_str(), cv::LINE_AA, static_cast<double>(draw_size) / 30.0, (line_thickness > 1) ? line_thickness - 1 : 1, &baseLine);
+                txt_bk_color = color; // * 0.7;
+                y = (y > d_img.rows) ? d_img.rows : y + 1;
+                out_point_y = y - label_size.height - baseLine;
+                if (out_point_y >= 0) y = out_point_y;
+                cv::rectangle(d_img, cv::Rect(cv::Point(x - (line_thickness - 1), y), cv::Size(label_size.width, label_size.height + baseLine)),
+                    txt_bk_color, -1);
+                cv::putText(d_img, label, cv::Point(x, y + label_size.height),
+                    cv::LINE_AA, static_cast<double>(draw_size) / 30.0, txt_color, (line_thickness > 1) ? line_thickness - 1 : 1);
+            }
+
+        }
+        return d_img;
+    }
+
+}
+
+
+

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

modules/ANSMOT/dllmain.cpp

@@ -0,0 +1,281 @@
+// dllmain.cpp : Defines the entry point for the DLL application.
+#include "pch.h"
+#include "ANSMOT.h"
+#include "ANSByteTrackNCNN.h"
+#include "ANSByteTrack.h"
+#include "ANSByteTrackEigen.h"
+#include "ANSOCSortTrack.h"
+#include "ANSUCMC.h"
+#include <unordered_map>
+#include <condition_variable>
+#include <mutex>
+#include <cstdint>
+
+// Handle registry with refcount — prevents use-after-free when
+// ReleaseANSMOTHandle is called while an operation is still running.
+static std::unordered_map<ANSCENTER::ANSMOT*, int>& MOTHandleRegistry() {
+	static std::unordered_map<ANSCENTER::ANSMOT*, int> s;
+	return s;
+}
+static std::mutex& MOTHandleRegistryMutex() {
+	static std::mutex m;
+	return m;
+}
+static std::condition_variable& MOTHandleRegistryCV() {
+	static std::condition_variable cv;
+	return cv;
+}
+
+static void RegisterMOTHandle(ANSCENTER::ANSMOT* h) {
+	std::lock_guard<std::mutex> lk(MOTHandleRegistryMutex());
+	MOTHandleRegistry()[h] = 1;
+}
+
+static ANSCENTER::ANSMOT* AcquireMOTHandle(ANSCENTER::ANSMOT* h) {
+	std::lock_guard<std::mutex> lk(MOTHandleRegistryMutex());
+	auto it = MOTHandleRegistry().find(h);
+	if (it == MOTHandleRegistry().end()) return nullptr;
+	it->second++;
+	return h;
+}
+
+static bool ReleaseMOTHandleRef(ANSCENTER::ANSMOT* h) {
+	std::lock_guard<std::mutex> lk(MOTHandleRegistryMutex());
+	auto it = MOTHandleRegistry().find(h);
+	if (it == MOTHandleRegistry().end()) return false;
+	it->second--;
+	if (it->second <= 0) {
+		MOTHandleRegistry().erase(it);
+		MOTHandleRegistryCV().notify_all();
+		return true;
+	}
+	return false;
+}
+
+static bool UnregisterMOTHandle(ANSCENTER::ANSMOT* h) {
+	std::unique_lock<std::mutex> lk(MOTHandleRegistryMutex());
+	auto it = MOTHandleRegistry().find(h);
+	if (it == MOTHandleRegistry().end()) return false;
+	it->second--;
+	bool ok = MOTHandleRegistryCV().wait_for(lk, std::chrono::seconds(30), [&]() {
+		auto it2 = MOTHandleRegistry().find(h);
+		return it2 == MOTHandleRegistry().end() || it2->second <= 0;
+	});
+	if (!ok) {
+		OutputDebugStringA("WARNING: UnregisterMOTHandle timed out waiting for in-flight operations\n");
+	}
+	MOTHandleRegistry().erase(h);
+	return true;
+}
+
+// RAII guard — ensures ReleaseMOTHandleRef is always called
+class MOTHandleGuard {
+	ANSCENTER::ANSMOT* engine;
+public:
+	explicit MOTHandleGuard(ANSCENTER::ANSMOT* e) : engine(e) {}
+	~MOTHandleGuard() { if (engine) ReleaseMOTHandleRef(engine); }
+	ANSCENTER::ANSMOT* get() const { return engine; }
+	explicit operator bool() const { return engine != nullptr; }
+	MOTHandleGuard(const MOTHandleGuard&) = delete;
+	MOTHandleGuard& operator=(const MOTHandleGuard&) = delete;
+};
+
+BOOL APIENTRY DllMain( HMODULE hModule,
+                       DWORD  ul_reason_for_call,
+                       LPVOID lpReserved
+                     )
+{
+    switch (ul_reason_for_call)
+    {
+    case DLL_PROCESS_ATTACH:
+    case DLL_THREAD_ATTACH:
+    case DLL_THREAD_DETACH:
+    case DLL_PROCESS_DETACH:
+        break;
+    }
+    return TRUE;
+}
+/*
+   enum MOTEType {
+		BYTETRACKNCNN = 0,
+		BYTETRACK = 1,
+		BYTETRACKEIGEN=2,
+		OCSORT=3
+	};
+*/
+extern "C" __declspec(dllexport) int      __cdecl CreateANSMOTHandle(ANSCENTER::ANSMOT **Handle, int motType) {
+	if (Handle == nullptr) return 0;
+	try {
+		// Release existing handle if called twice (prevents leak from LabVIEW)
+		if (*Handle) {
+			if (UnregisterMOTHandle(*Handle)) {
+				(*Handle)->Destroy();
+				delete *Handle;
+			}
+			*Handle = nullptr;
+		}
+
+		switch (motType) {
+		case 0: //BYTETRACKNCNN =0
+			(*Handle) = new ANSCENTER::ANSByteTrack();// ANSByteTrackNCNN();
+			break;
+		case 1: //BYTETRACK = 1
+			(*Handle) = new ANSCENTER::ANSByteTrack();
+			break;
+		case 2://BYTETRACKEIGEN = 2
+			(*Handle) = new ANSCENTER::ANSByteTrack();// ANSByteTrackEigen();
+			break;
+		case 3://UCMC = 3
+			(*Handle) = new ANSCENTER::ANSUCMCTrack();
+			break;
+		case 4://OCSORT = 4
+			(*Handle) = new ANSCENTER::ANSOCSortTrack();
+			break;
+		default:
+			(*Handle) = new ANSCENTER::ANSByteTrack();// ANSByteTrackNCNN();
+			break;
+		}
+		if (*Handle == nullptr) {
+			return 0;
+		}
+		else {
+			RegisterMOTHandle(*Handle);
+			return 1;
+		}
+	}
+	catch (std::exception& e) {
+		return 0;
+	}
+	catch (...) {
+		return 0;
+	}
+}
+static int ReleaseANSMOTHandle_Impl(ANSCENTER::ANSMOT** Handle) {
+	try {
+		if (!Handle || !*Handle) return 1;
+		if (!UnregisterMOTHandle(*Handle)) {
+			*Handle = nullptr;
+			return 1;
+		}
+		(*Handle)->Destroy();
+		delete *Handle;
+		*Handle = nullptr;
+		return 1;
+	}
+	catch (...) {
+		if (Handle) *Handle = nullptr;
+		return 0;
+	}
+}
+
+extern "C" __declspec(dllexport) int      __cdecl ReleaseANSMOTHandle(ANSCENTER::ANSMOT **Handle) {
+	__try {
+		return ReleaseANSMOTHandle_Impl(Handle);
+	}
+	__except (EXCEPTION_EXECUTE_HANDLER) {
+		if (Handle) *Handle = nullptr;
+		return 0;
+	}
+}
+
+extern "C" __declspec(dllexport) int     __cdecl UpdateANSMOTParameters(ANSCENTER::ANSMOT * *Handle, const char* trackerParameter) {
+	if (Handle == nullptr || *Handle == nullptr) return -1;
+	MOTHandleGuard guard(AcquireMOTHandle(*Handle));
+	if (!guard) return -1;
+	try {
+		bool result = guard.get()->UpdateParameters(trackerParameter);
+		if (result) return 1;
+		else return 0;
+	}
+	catch (std::exception& e) {
+		return 0;
+	}
+	catch (...) {
+		return 0;
+	}
+}
+
+extern "C" __declspec(dllexport) int __cdecl UpdateANSMOT(ANSCENTER::ANSMOT * *Handle, int modelId, const char* detectionData, std::string & result) {
+	if (Handle == nullptr || *Handle == nullptr) return -1;
+	MOTHandleGuard guard(AcquireMOTHandle(*Handle));
+	if (!guard) return -1;
+	try {
+		result = guard.get()->Update(modelId, detectionData);
+		if (result.empty())return 0;
+		else return 1;
+	}
+	catch (std::exception& e) {
+		return 0;
+	}
+	catch (...) {
+		return 0;
+	}
+}
+extern "C" __declspec(dllexport) int            __cdecl UpdateANSMOT_LV(ANSCENTER::ANSMOT * *Handle, int modelId, const char* detectionData, LStrHandle trackerResult) {
+	if (Handle == nullptr || *Handle == nullptr) return -1;
+	MOTHandleGuard guard(AcquireMOTHandle(*Handle));
+	if (!guard) return -1;
+	try {
+		std::string st = guard.get()->Update(modelId, detectionData);
+		int size = static_cast<int>(st.length());
+		MgErr error;
+		error = DSSetHandleSize(trackerResult, sizeof(int32) + size * sizeof(uChar));
+		if (error == noErr)
+		{
+			(*trackerResult)->cnt = size;
+			memcpy((*trackerResult)->str, st.c_str(), size);
+			return 1;
+		}
+		else return 0;
+	}
+	catch (std::exception& e) {
+		return 0;
+	}
+	catch (...) {
+		return 0;
+	}
+}
+
+extern "C" __declspec(dllexport) int            __cdecl UpdateANSMOTTracker(ANSCENTER::ANSMOT** Handle, int modelId, const std::vector<ANSCENTER::TrackerObject>& detectionObjects, std::vector<ANSCENTER::TrackerObject>& trackedObjects)
+{
+	if (Handle == nullptr || *Handle == nullptr) return -1;
+	MOTHandleGuard guard(AcquireMOTHandle(*Handle));
+	if (!guard) return -1;
+	try {
+		trackedObjects = guard.get()->UpdateTracker(modelId, detectionObjects);
+		if (trackedObjects.empty()) return 0;
+		else return 1;
+	}
+	catch (std::exception& e) {
+		return 0;
+	}
+	catch (...) {
+		return 0;
+	}
+}
+
+// ============================================================================
+// V2 LabVIEW API — Accept handle as uint64_t by value.
+// Eliminates Handle** pointer-to-pointer instability when LabVIEW calls
+// concurrently from multiple tasks.
+// LabVIEW CLFN: set Handle parameter to Numeric / Unsigned Pointer-sized Integer / Pass: Value
+// ============================================================================
+
+extern "C" __declspec(dllexport) int __cdecl UpdateANSMOT_LV_V2(uint64_t handleVal, int modelId, const char* detectionData, LStrHandle trackerResult) {
+	ANSCENTER::ANSMOT* directHandle = reinterpret_cast<ANSCENTER::ANSMOT*>(handleVal);
+	if (directHandle == nullptr) return -1;
+	MOTHandleGuard guard(AcquireMOTHandle(directHandle));
+	if (!guard) return -1;
+	try {
+		std::string st = guard.get()->Update(modelId, detectionData);
+		int size = static_cast<int>(st.length());
+		MgErr error = DSSetHandleSize(trackerResult, sizeof(int32) + size * sizeof(uChar));
+		if (error == noErr) {
+			(*trackerResult)->cnt = size;
+			memcpy((*trackerResult)->str, st.c_str(), size);
+			return 1;
+		}
+		else return 0;
+	}
+	catch (...) { return 0; }
+}

modules/ANSMOT/framework.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#define WIN32_LEAN_AND_MEAN             // Exclude rarely-used stuff from Windows headers
+#define NOMINMAX                        // Prevent windows.h from defining min/max macros
+                                        // which break std::min / std::max (C2589)
+// Windows Header Files
+#include <windows.h>

modules/ANSMOT/pch.cpp

@@ -0,0 +1,5 @@
+// pch.cpp: source file corresponding to the pre-compiled header
+
+#include "pch.h"
+
+// When you are using pre-compiled headers, this source file is necessary for compilation to succeed.

modules/ANSMOT/pch.h

@@ -0,0 +1,13 @@
+// pch.h: This is a precompiled header file.
+// Files listed below are compiled only once, improving build performance for future builds.
+// This also affects IntelliSense performance, including code completion and many code browsing features.
+// However, files listed here are ALL re-compiled if any one of them is updated between builds.
+// Do not add files here that you will be updating frequently as this negates the performance advantage.
+
+#ifndef PCH_H
+#define PCH_H
+
+// add headers that you want to pre-compile here
+#include "framework.h"
+
+#endif //PCH_H