ANSCENTER/ANSCORE
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor project structure

Browse Source
This commit is contained in:
Tuan Nghia Nguyen
2026-03-28 19:56:39 +11:00
parent 1d267378b2
commit 8a2e721058
511 changed files with 59 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

566
modules/ANSMOT/ByteTrackEigen/src/EigenBYTETracker.cpp Normal file
View File

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

97
modules/ANSMOT/ByteTrackEigen/src/EigenBaseTrack.cpp Normal file
View File

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

134
modules/ANSMOT/ByteTrackEigen/src/EigenBoundingBoxIoUMatching.cpp Normal file
View File

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

143
modules/ANSMOT/ByteTrackEigen/src/EigenBoundingBoxTrackUtils.cpp Normal file
View File

@@ -0,0 +1,143 @@
#include "EigenBoundingBoxTrackUtils.h"
namespace ByteTrackEigen {
/**
* @brief Joins two lists of KalmanBBoxTrack pointers, ensuring unique track IDs.
*
* This function combines track lists A and B. If a track ID is present in both lists,
* the track from list B is used in the final list. This ensures that each track ID is
* unique in the resulting list.
*
* @param track_list_a First list of shared pointers to KalmanBBoxTrack objects.
* @param track_list_b Second list of shared pointers to KalmanBBoxTrack objects.
* @return std::vector<std::shared_ptr<KalmanBBoxTrack>> A list with unique tracks based on track IDs.
*/
std::vector<std::shared_ptr<KalmanBBoxTrack>> join_tracks(
const std::vector<std::shared_ptr<KalmanBBoxTrack>> track_list_a,
const std::vector<std::shared_ptr<KalmanBBoxTrack>> track_list_b) {
std::map<int, std::shared_ptr<KalmanBBoxTrack>> unique_tracks;
// Populate the map with tracks from track_list_a
for (auto& track : track_list_a) {
unique_tracks[track->get_track_id()] = track;
}
// Insert or overwrite with tracks from track_list_b
for (auto& track : track_list_b) {
unique_tracks[track->get_track_id()] = track;
}
// Convert the unique tracks in the map to a vector
std::vector<std::shared_ptr<KalmanBBoxTrack>> result;
for (auto [key, value] : unique_tracks) {
result.push_back(value);
}
return result;
}
/**
* @brief Subtracts tracks in track_list_b from track_list_a based on track IDs.
*
* This function creates a new list of tracks from track_list_a excluding those
* tracks whose IDs are found in track_list_b, effectively performing a set subtraction.
*
* @param track_list_a First list of shared pointers to KalmanBBoxTrack objects.
* @param track_list_b Second list of shared pointers to KalmanBBoxTrack objects.
* @return std::vector<std::shared_ptr<KalmanBBoxTrack>> A list of tracks present in track_list_a but not in track_list_b.
*/
std::vector<std::shared_ptr<KalmanBBoxTrack>> sub_tracks(
const std::vector<std::shared_ptr<KalmanBBoxTrack>>& track_list_a,
const std::vector<std::shared_ptr<KalmanBBoxTrack>>& track_list_b) {
std::unordered_set<int> track_ids_b;
// Populate set with track IDs from track_list_b for quick lookup
for (const auto& track : track_list_b) {
track_ids_b.insert(track->get_track_id());
}
// Collect tracks from track_list_a not found in track_list_b
std::vector<std::shared_ptr<KalmanBBoxTrack>> result;
for (const auto& track : track_list_a) {
if (track_ids_b.find(track->get_track_id()) == track_ids_b.end()) {
result.push_back(track);
}
}
return result;
}
/**
* @brief Removes duplicate tracks from two lists based on IOU distance and track age.
*
* This function identifies and removes duplicate tracks between two lists based on the
* Intersection Over Union (IOU) distance. If two tracks have an IOU distance less than
* a threshold (0.15), the older track is retained.
*
* @param track_list_a First list of shared pointers to KalmanBBoxTrack objects.
* @param track_list_b Second list of shared pointers to KalmanBBoxTrack objects.
* @return Pair of vectors each containing unique tracks after removing duplicates.
*/
std::pair<std::vector<std::shared_ptr<KalmanBBoxTrack>>, std::vector<std::shared_ptr<KalmanBBoxTrack>>>
remove_duplicate_tracks(const std::vector<std::shared_ptr<KalmanBBoxTrack>> track_list_a,
const std::vector<std::shared_ptr<KalmanBBoxTrack>> track_list_b) {
// Creating instance vectors to store de-referenced shared pointers from the input lists.
// This is necessary for the IOU distance calculation.
std::vector<KalmanBBoxTrack> s_tracks_a_instances;
s_tracks_a_instances.reserve(track_list_a.size());
for (const auto& ptr : track_list_a) {
s_tracks_a_instances.push_back(*ptr);
}
std::vector<KalmanBBoxTrack> s_tracks_b_instances;
s_tracks_b_instances.reserve(track_list_b.size());
for (const auto& ptr : track_list_b) {
s_tracks_b_instances.push_back(*ptr);
}
// Calculating pairwise IOU distance between tracks in both lists.
Eigen::MatrixXd pairwise_distance = iou_distance(s_tracks_a_instances, s_tracks_b_instances);
// Sets to track indices of duplicates in both lists.
std::unordered_set<int> duplicates_a, duplicates_b;
// Loop through the matrix of pairwise distances to identify duplicates.
for (int i = 0; i < pairwise_distance.rows(); ++i) {
for (int j = 0; j < pairwise_distance.cols(); ++j) {
// If IOU distance is below the threshold, consider the tracks as duplicates.
if (pairwise_distance(i, j) < 0.15) {
// Calculate track age to determine which track to keep.
int time_a = track_list_a[i]->get_frame_id() - track_list_a[i]->get_start_frame();
int time_b = track_list_b[j]->get_frame_id() - track_list_b[j]->get_start_frame();
// Retain the older track and mark the newer one as a duplicate.
if (time_a > time_b) {
duplicates_b.insert(j);
}
else {
duplicates_a.insert(i);
}
}
}
}
// Constructing the result lists, excluding the identified duplicates.
std::vector<std::shared_ptr<KalmanBBoxTrack>> result_a, result_b;
for (int i = 0; i < track_list_a.size(); ++i) {
if (duplicates_a.find(i) == duplicates_a.end()) {
result_a.push_back(track_list_a[i]);
}
}
for (int j = 0; j < track_list_b.size(); ++j) {
if (duplicates_b.find(j) == duplicates_b.end()) {
result_b.push_back(track_list_b[j]);
}
}
return { result_a, result_b };
}
}

343
modules/ANSMOT/ByteTrackEigen/src/EigenHungarianAlgorithm.cpp Normal file
View File

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

270
modules/ANSMOT/ByteTrackEigen/src/EigenKalmanBBoxTrack.cpp Normal file
View File

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

144
modules/ANSMOT/ByteTrackEigen/src/EigenKalmanFilter.cpp Normal file
View File

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

106
modules/ANSMOT/ByteTrackEigen/src/EigenLinearAssignment.cpp Normal file
View File

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