From e56f7abd2f5107db99e2d74b236bdf63212b16e2 Mon Sep 17 00:00:00 2001 From: iChizer0 <62390647+iChizer0@users.noreply.github.com> Date: Sun, 28 Apr 2024 15:05:03 +0800 Subject: [PATCH] feat: bytetrack * feat: add optimized bytetrack * chore: add libeigen --- .gitmodules | 3 + third_party/ByteTrack/BYTETracker.cpp | 213 ++++++++++++++ third_party/ByteTrack/BYTETracker.h | 75 +++++ third_party/ByteTrack/LICENSE | 21 ++ third_party/ByteTrack/STrack.cpp | 173 +++++++++++ third_party/ByteTrack/STrack.h | 60 ++++ third_party/ByteTrack/dataType.h | 54 ++++ third_party/ByteTrack/kalmanFilter.cpp | 113 +++++++ third_party/ByteTrack/kalmanFilter.h | 31 ++ third_party/ByteTrack/lapjv.cpp | 311 ++++++++++++++++++++ third_party/ByteTrack/lapjv.h | 72 +++++ third_party/ByteTrack/utils.cpp | 388 +++++++++++++++++++++++++ 12 files changed, 1514 insertions(+) create mode 100644 .gitmodules create mode 100644 third_party/ByteTrack/BYTETracker.cpp create mode 100644 third_party/ByteTrack/BYTETracker.h create mode 100644 third_party/ByteTrack/LICENSE create mode 100644 third_party/ByteTrack/STrack.cpp create mode 100644 third_party/ByteTrack/STrack.h create mode 100644 third_party/ByteTrack/dataType.h create mode 100644 third_party/ByteTrack/kalmanFilter.cpp create mode 100644 third_party/ByteTrack/kalmanFilter.h create mode 100644 third_party/ByteTrack/lapjv.cpp create mode 100644 third_party/ByteTrack/lapjv.h create mode 100644 third_party/ByteTrack/utils.cpp diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..e427387e --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "third_party/Eigen3"] + path = third_party/Eigen3 + url = https://gitlab.com/libeigen/eigen.git diff --git a/third_party/ByteTrack/BYTETracker.cpp b/third_party/ByteTrack/BYTETracker.cpp new file mode 100644 index 00000000..24f2aba4 --- /dev/null +++ b/third_party/ByteTrack/BYTETracker.cpp @@ -0,0 +1,213 @@ +/* + * MIT License + * Copyright (c) 2021 Yifu Zhang + * + * Modified by nullptr, Apr 15, 2024, Seeed Technology Co.,Ltd +*/ + +#include "BYTETracker.h" + +#include +#include +#include + +using namespace std; + +BYTETracker::BYTETracker(int frame_rate, int track_buffer) { + track_thresh = 0.5; + high_thresh = 0.6; + match_thresh = 0.8; + + frame_id = 0; + max_time_lost = int(frame_rate / 30.0 * track_buffer); +} + +BYTETracker::~BYTETracker() {} + +vector BYTETracker::update(const vector& objects) { + ////////////////// Step 1: Get detections ////////////////// + this->frame_id += 1; + + vector activated_stracks; + vector refind_stracks; + vector removed_stracks; + vector lost_stracks; + vector detections; + vector detections_low; + + vector detections_cp; + vector tracked_stracks_swap; + vector resa, resb; + vector output_stracks; + + vector unconfirmed; + vector tracked_stracks; + vector strack_pool; + vector r_tracked_stracks; + + for (const auto& obj : objects) { + vector tlwh_; + tlwh_.resize(4); + + tlwh_[0] = obj.rect.x; + tlwh_[1] = obj.rect.y; + tlwh_[2] = obj.rect.width; + tlwh_[3] = obj.rect.height; + + float score = obj.prob; + if (score >= track_thresh) { + detections.emplace_back(move(tlwh_), score, obj.id); + } else { + detections_low.emplace_back(move(tlwh_), score, obj.id); + } + } + + // Add newly detected tracklets to tracked_stracks + for (size_t 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); + STrack::multi_predict(strack_pool, this->kalman_filter); + + vector > dists; + int dist_size = 0, dist_size_size = 0; + dists = iou_distance(strack_pool, detections, dist_size, dist_size_size); + + vector > matches; + vector u_track, u_detection; + linear_assignment(dists, dist_size, dist_size_size, 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(move(detections[u_detection[i]])); + } + detections.clear(); + detections.assign(detections_low.begin(), detections_low.end()); + + for (int i = 0; i < u_track.size(); ++i) { + auto idx = u_track[i]; + auto st = strack_pool[idx]; + if (st->state == TrackState::Tracked) { + r_tracked_stracks.push_back(st); + } + } + + 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(); + vector 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->activate(this->kalman_filter, this->frame_id); + 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); + + 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) { + if (this->tracked_stracks[i].is_activated) { + output_stracks.push_back(this->tracked_stracks[i]); + } + } + return output_stracks; +} \ No newline at end of file diff --git a/third_party/ByteTrack/BYTETracker.h b/third_party/ByteTrack/BYTETracker.h new file mode 100644 index 00000000..105545ef --- /dev/null +++ b/third_party/ByteTrack/BYTETracker.h @@ -0,0 +1,75 @@ +/* + * MIT License + * Copyright (c) 2021 Yifu Zhang + * + * Modified by nullptr, Apr 15, 2024, Seeed Technology Co.,Ltd +*/ + +#pragma once + +#include +#include +#include +#include + +#include "STrack.h" + +class BYTETracker { + public: + struct Object { + Rect4f rect; + int label; + float prob; + int id; + }; + + public: + BYTETracker(int frame_rate = 30, int track_buffer = 30); + ~BYTETracker(); + + std::vector update(const std::vector& objects); + + private: + std::vector joint_stracks(std::vector& tlista, std::vector& tlistb); + std::vector joint_stracks(std::vector& tlista, std::vector& tlistb); + + std::vector sub_stracks(std::vector& tlista, std::vector& tlistb); + void remove_duplicate_stracks(std::vector& resa, + std::vector& resb, + std::vector& stracksa, + std::vector& stracksb); + + void linear_assignment(std::vector >& cost_matrix, + int cost_matrix_size, + int cost_matrix_size_size, + float thresh, + std::vector >& matches, + std::vector& unmatched_a, + std::vector& unmatched_b); + std::vector > iou_distance(std::vector& atracks, + std::vector& btracks, + int& dist_size, + int& dist_size_size); + std::vector > iou_distance(std::vector& atracks, std::vector& btracks); + std::vector > ious(std::vector >& atlbrs, + std::vector >& btlbrs); + + double lapjv(const std::vector >& cost, + std::vector& rowsol, + std::vector& colsol, + bool extend_cost = false, + float cost_limit = LONG_MAX, + bool return_cost = true); + + private: + float track_thresh; + float high_thresh; + float match_thresh; + int frame_id; + int max_time_lost; + + std::vector tracked_stracks; + std::vector lost_stracks; + std::vector removed_stracks; + byte_kalman::KalmanFilter kalman_filter; +}; \ No newline at end of file diff --git a/third_party/ByteTrack/LICENSE b/third_party/ByteTrack/LICENSE new file mode 100644 index 00000000..dd56da73 --- /dev/null +++ b/third_party/ByteTrack/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 Yifu Zhang + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. \ No newline at end of file diff --git a/third_party/ByteTrack/STrack.cpp b/third_party/ByteTrack/STrack.cpp new file mode 100644 index 00000000..50769ea8 --- /dev/null +++ b/third_party/ByteTrack/STrack.cpp @@ -0,0 +1,173 @@ +/* + * MIT License + * Copyright (c) 2021 Yifu Zhang + * + * Modified by nullptr, Apr 15, 2024, Seeed Technology Co.,Ltd +*/ + +#include "STrack.h" + +using namespace std; + +STrack::STrack(vector tlwh_, float score, int id) { + _tlwh.resize(4); + _tlwh.assign(tlwh_.begin(), tlwh_.end()); + + is_activated = false; + track_id = 0; + state = TrackState::New; + + tlwh.resize(4); + tlbr.resize(4); + + static_tlwh(); + static_tlbr(); + + frame_id = 0; + tracklet_len = 0; + this->score = score; + start_frame = 0; + + this->id = id; +} + +STrack::~STrack() {} + +void STrack::activate(byte_kalman::KalmanFilter& kalman_filter, int frame_id) { + this->kalman_filter = kalman_filter; + this->track_id = this->next_id(); + + vector _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]; + + vector 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->frame_id = frame_id; + this->start_frame = frame_id; +} + +void STrack::re_activate(STrack& new_track, int frame_id, bool new_id) { + vector 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; + 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++; + + vector 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; +} + +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]; +} + +vector STrack::tlwh_to_xyah(vector tlwh_tmp) { + tlwh_tmp[0] += tlwh_tmp[2] / 2; + tlwh_tmp[1] += tlwh_tmp[3] / 2; + tlwh_tmp[2] /= tlwh_tmp[3]; + return tlwh_tmp; +} + +vector STrack::to_xyah() { return tlwh_to_xyah(tlwh); } + +vector STrack::tlbr_to_tlwh(vector& 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() { + static int _count = 0; + return ++_count; +} + +int STrack::end_frame() { return this->frame_id; } + +void STrack::multi_predict(vector& stracks, byte_kalman::KalmanFilter& kalman_filter) { + for (int i = 0; i < stracks.size(); ++i) { + stracks[i]->mean[7] = !(stracks[i]->state ^ TrackState::Tracked); + kalman_filter.predict(stracks[i]->mean, stracks[i]->covariance); + stracks[i]->static_tlwh(); + stracks[i]->static_tlbr(); + } +} \ No newline at end of file diff --git a/third_party/ByteTrack/STrack.h b/third_party/ByteTrack/STrack.h new file mode 100644 index 00000000..338e6881 --- /dev/null +++ b/third_party/ByteTrack/STrack.h @@ -0,0 +1,60 @@ +/* + * MIT License + * Copyright (c) 2021 Yifu Zhang + * + * Modified by nullptr, Apr 15, 2024, Seeed Technology Co.,Ltd +*/ + +#pragma once + +#include +#include +#include +#include + +#include "kalmanFilter.h" + +enum TrackState { New = 0, Tracked, Lost, Removed }; + +class STrack { + public: + STrack(std::vector tlwh_, float score, int id); + ~STrack(); + + std::vector static tlbr_to_tlwh(std::vector& tlbr); + void static multi_predict(std::vector& stracks, byte_kalman::KalmanFilter& kalman_filter); + void static_tlwh(); + void static_tlbr(); + std::vector tlwh_to_xyah(std::vector tlwh_tmp); + std::vector to_xyah(); + void mark_lost(); + void mark_removed(); + int next_id(); + int end_frame(); + + void activate(byte_kalman::KalmanFilter& kalman_filter, int frame_id); + 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; + + std::vector _tlwh; + std::vector tlwh; + std::vector tlbr; + + int frame_id; + int tracklet_len; + int start_frame; + + KAL_MEAN mean; + KAL_COVA covariance; + float score; + + int id; + + private: + byte_kalman::KalmanFilter kalman_filter; +}; \ No newline at end of file diff --git a/third_party/ByteTrack/dataType.h b/third_party/ByteTrack/dataType.h new file mode 100644 index 00000000..484f8aae --- /dev/null +++ b/third_party/ByteTrack/dataType.h @@ -0,0 +1,54 @@ +/* + * MIT License + * Copyright (c) 2021 Yifu Zhang + * + * Modified by nullptr, Apr 15, 2024, Seeed Technology Co.,Ltd +*/ + +#pragma once + +#include +#include +#include +#include + +#include +#include + +typedef Eigen::Matrix DETECTBOX; +typedef Eigen::Matrix DETECTBOXSS; +typedef Eigen::Matrix FEATURE; +typedef Eigen::Matrix FEATURESS; + +//Kalmanfilter +typedef Eigen::Matrix KAL_MEAN; +typedef Eigen::Matrix KAL_COVA; +typedef Eigen::Matrix KAL_HMEAN; +typedef Eigen::Matrix KAL_HCOVA; +using KAL_DATA = std::pair; +using KAL_HDATA = std::pair; + +//main +using RESULT_DATA = std::pair; + +//tracker +using TRACKER_DATA = std::pair; +using MATCH_DATA = std::pair; + +//linear_assignment +typedef Eigen::Matrix DYNAMICM; + +struct Rect4f { + float x; + float y; + float width; + float height; +}; + +struct Scalar3u { + Scalar3u(unsigned int v1, unsigned int v2, unsigned int v3) : val1(v1), val2(v2), val3(v3) {} + + unsigned int val1; + unsigned int val2; + unsigned int val3; +}; diff --git a/third_party/ByteTrack/kalmanFilter.cpp b/third_party/ByteTrack/kalmanFilter.cpp new file mode 100644 index 00000000..49543133 --- /dev/null +++ b/third_party/ByteTrack/kalmanFilter.cpp @@ -0,0 +1,113 @@ +/* + * MIT License + * Copyright (c) 2021 Yifu Zhang + * + * Modified by nullptr, Apr 15, 2024, Seeed Technology Co.,Ltd +*/ +#include "kalmanFilter.h" + +#include +#include +#include + +#include + +namespace byte_kalman { + +const double KalmanFilter::chi2inv95[10] = {0, 3.8415, 5.9915, 7.8147, 9.4877, 11.070, 12.592, 14.067, 15.507, 16.919}; + +KalmanFilter::KalmanFilter() { + 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 KalmanFilter::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 KalmanFilter::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 KalmanFilter::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 diag = std.asDiagonal(); + diag = diag.array().square().matrix(); + covariance1 += diag; + return std::make_pair(mean1, covariance1); +} + +KAL_DATA +KalmanFilter::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 B = (covariance * (_update_mat.transpose())).transpose(); + Eigen::Matrix kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4 + Eigen::Matrix 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); +} + +} // namespace byte_kalman \ No newline at end of file diff --git a/third_party/ByteTrack/kalmanFilter.h b/third_party/ByteTrack/kalmanFilter.h new file mode 100644 index 00000000..278d4050 --- /dev/null +++ b/third_party/ByteTrack/kalmanFilter.h @@ -0,0 +1,31 @@ +/* + * MIT License + * Copyright (c) 2021 Yifu Zhang + * + * Modified by nullptr, Apr 15, 2024, Seeed Technology Co.,Ltd +*/ + +#pragma once + +#include "dataType.h" + +namespace byte_kalman { +class KalmanFilter { + public: + static const double chi2inv95[10]; + + KalmanFilter(); + + 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); + + private: + Eigen::Matrix _motion_mat; + Eigen::Matrix _update_mat; + + float _std_weight_position; + float _std_weight_velocity; +}; +} // namespace byte_kalman \ No newline at end of file diff --git a/third_party/ByteTrack/lapjv.cpp b/third_party/ByteTrack/lapjv.cpp new file mode 100644 index 00000000..c6d88af9 --- /dev/null +++ b/third_party/ByteTrack/lapjv.cpp @@ -0,0 +1,311 @@ +#include "lapjv.h" + +#include +#include +#include + +/** 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; +} \ No newline at end of file diff --git a/third_party/ByteTrack/lapjv.h b/third_party/ByteTrack/lapjv.h new file mode 100644 index 00000000..4df8f2b2 --- /dev/null +++ b/third_party/ByteTrack/lapjv.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; \ + } + +#ifdef LAPJV_DEBUG + #include + #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 \ No newline at end of file diff --git a/third_party/ByteTrack/utils.cpp b/third_party/ByteTrack/utils.cpp new file mode 100644 index 00000000..7f8f731f --- /dev/null +++ b/third_party/ByteTrack/utils.cpp @@ -0,0 +1,388 @@ +/* + * MIT License + * Copyright (c) 2021 Yifu Zhang + * + * Modified by nullptr, Apr 15, 2024, Seeed Technology Co.,Ltd +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BYTETracker.h" +#include "lapjv.h" + +using namespace std; + +vector BYTETracker::joint_stracks(vector& tlista, vector& tlistb) { + map exists; + vector res; + for (int i = 0; i < tlista.size(); i++) { + exists.insert(pair(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; +} + +vector BYTETracker::joint_stracks(vector& tlista, vector& tlistb) { + map exists; + vector res; + for (int i = 0; i < tlista.size(); i++) { + exists.insert(pair(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; +} + +vector BYTETracker::sub_stracks(vector& tlista, vector& tlistb) { + map stracks; + for (int i = 0; i < tlista.size(); i++) { + stracks.insert(pair(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); + } + } + + vector res; + std::map::iterator it; + for (it = stracks.begin(); it != stracks.end(); ++it) { + res.push_back(it->second); + } + + return res; +} + +void BYTETracker::remove_duplicate_stracks(vector& resa, + vector& resb, + vector& stracksa, + vector& stracksb) { + vector > pdist = iou_distance(stracksa, stracksb); + vector > 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(pair(i, j)); + } + } + } + + vector 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++) { + vector::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++) { + vector::iterator iter = find(dupb.begin(), dupb.end(), i); + if (iter == dupb.end()) { + resb.push_back(stracksb[i]); + } + } +} + +void BYTETracker::linear_assignment(vector >& cost_matrix, + int cost_matrix_size, + int cost_matrix_size_size, + float thresh, + vector >& matches, + vector& unmatched_a, + vector& 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; + } + + vector rowsol; + vector colsol; + + lapjv(cost_matrix, rowsol, colsol, true, thresh); + + auto rowsol_size = rowsol.size(); + for (int i = 0; i < rowsol_size; i++) { + if (rowsol[i] >= 0) { + matches.emplace_back(vector{i, rowsol[i]}); + } else { + unmatched_a.push_back(i); + } + } + + auto colsol_size = colsol.size(); + for (int i = 0; i < colsol_size; i++) { + if (colsol[i] < 0) { + unmatched_b.push_back(i); + } + } +} + +vector > BYTETracker::ious(vector >& atlbrs, vector >& btlbrs) { + vector > ious; + if (atlbrs.size() * btlbrs.size() == 0) return ious; + + ious.resize(atlbrs.size()); + auto ious_size = ious.size(); + for (int i = 0; i < ious_size; i++) { + ious[i].resize(btlbrs.size()); + } + + //bbox_ious + auto btlbrs_size = btlbrs.size(); + auto atlbrs_size = atlbrs.size(); + for (int k = 0; k < btlbrs_size; k++) { + 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 = min(atlbrs[n][2], btlbrs[k][2]) - max(atlbrs[n][0], btlbrs[k][0]) + 1; + if (iw > 0) { + float ih = min(atlbrs[n][3], btlbrs[k][3]) - 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; +} + +vector > BYTETracker::iou_distance(vector& atracks, + vector& btracks, + int& dist_size, + int& dist_size_size) { + vector > cost_matrix; + if (atracks.size() * btracks.size() == 0) { + dist_size = atracks.size(); + dist_size_size = btracks.size(); + return cost_matrix; + } + + auto atracks_size = atracks.size(); + auto btracks_size = btracks.size(); + + vector > atlbrs, btlbrs; + atlbrs.resize(atracks_size); + btlbrs.resize(btracks_size); + for (int i = 0; i < atracks_size; i++) { + atlbrs[i] = atracks[i]->tlbr; + } + for (int i = 0; i < btracks_size; i++) { + btlbrs[i] = btracks[i].tlbr; + } + + dist_size = atracks.size(); + dist_size_size = btracks.size(); + + vector > _ious{ious(atlbrs, btlbrs)}; + auto _ious_size = _ious.size(); + cost_matrix.resize(_ious_size); + + for (int i = 0; i < _ious_size; i++) { + vector _iou; + auto _ious_i_size = _ious[i].size(); + _iou.resize(_ious_i_size); + for (int j = 0; j < _ious_i_size; j++) { + _iou[j] = 1 - _ious[i][j]; + } + cost_matrix[i] = move(_iou); + } + + return cost_matrix; +} + +vector > BYTETracker::iou_distance(vector& atracks, vector& btracks) { + auto atracks_size = atracks.size(); + auto btracks_size = btracks.size(); + + static vector > atlbrs, btlbrs; + + atlbrs.resize(atracks_size); + btlbrs.resize(btracks_size); + for (int i = 0; i < atracks_size; i++) { + atlbrs[i] = atracks[i].tlbr; + } + for (int i = 0; i < btracks_size; i++) { + btlbrs[i] = btracks[i].tlbr; + } + + vector > _ious{ious(atlbrs, btlbrs)}; + auto _ious_size = _ious.size(); + vector > cost_matrix; + cost_matrix.resize(_ious_size); + + for (int i = 0; i < _ious_size; i++) { + vector _iou; + auto _ious_i_size = _ious[i].size(); + _iou.resize(_ious_i_size); + for (int j = 0; j < _ious_i_size; j++) { + _iou[j] = 1 - _ious[i][j]; + } + cost_matrix[i] = move(_iou); + } + + return cost_matrix; +} + +double BYTETracker::lapjv(const vector >& cost, + vector& rowsol, + vector& colsol, + bool extend_cost, + float cost_limit, + bool return_cost) { + vector > cost_c; + cost_c.assign(cost.begin(), cost.end()); + + vector > 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) { + assert(0 && "set extend_cost=True"); + } + } + + if (extend_cost || cost_limit < LONG_MAX) { + n = n_rows + n_cols; + cost_c_extended.resize(n); + for (int i = 0; i < n; i++) cost_c_extended[i].resize(n); + + if (cost_limit < LONG_MAX) { + for (int i = 0; i < n; 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.0; + 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 < n; 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 < n; 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.swap(cost_c_extended); + } + + double** cost_ptr; + cost_ptr = new double*[sizeof(double*) * n]; + for (int i = 0; i < n; i++) cost_ptr[i] = new double[sizeof(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[sizeof(int) * n]; + int* y_c = new int[sizeof(int) * n]; + + double opt = 0.0; + + int ret = lapjv_internal(n, cost_ptr, x_c, y_c); + if (ret != 0) { + puts("lapjv_internal failed"); + goto ReleaseMemory; + } + + 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) { + opt += cost_ptr[i][rowsol[i]]; + } + } + } + } else if (return_cost) { + for (int i = 0; i < rowsol.size(); i++) { + opt += cost_ptr[i][rowsol[i]]; + } + } + +ReleaseMemory: + for (int i = 0; i < n; i++) { + delete[] cost_ptr[i]; + } + delete[] cost_ptr; + delete[] x_c; + delete[] y_c; + + return opt; +}