Initial setup for CLion

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

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

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

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

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

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

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

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

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

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>&);

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

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