#include "EigenHungarianAlgorithm.h"
namespace ByteTrackEigen {
///
/// Default constructor for the HungarianAlgorithmEigen class.
///
HungarianAlgorithmEigen::HungarianAlgorithmEigen() {}
///
/// Destructor for the HungarianAlgorithmEigen class.
///
HungarianAlgorithmEigen::~HungarianAlgorithmEigen() {}
///
/// Finds the index of the first starred zero in a specified column.
///
/// The column index to search within. Must be in the range of [0, number of columns in star_matrix).
/// The row index of the starred zero, or NOT_FOUND_VALUE if no starred zero is found.
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;
}
///
/// Finds the index of the first primed zero in a specified row.
///
/// The row index to search within.
/// The column index of the primed zero, or NOT_FOUND_VALUE if no primed zero is found.
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;
}
///
/// 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.
///
/// The row index of the primed zero to start with.
/// The column index of the primed zero to start with.
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);
}
///
/// Reduces the distance matrix by the smallest uncovered value and adjusts the
/// matrix to prepare for further steps of the Hungarian algorithm.
///
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() + covered_columns.transpose().replicate(num_rows, 1).cast());
// 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() * min_uncovered_value;
Eigen::ArrayXXd col_adjustments = (1.0 - covered_columns.cast()) * min_uncovered_value;
dist_matrix += (row_adjustments.replicate(1, num_columns) - col_adjustments.transpose().replicate(num_rows, 1)).matrix();
}
///
/// Covers columns that lack a starred zero and performs adjustments to the
/// distance matrix, primed and starred zeros until all columns are covered.
///
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 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().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();
}
///
/// 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.
///
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 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().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 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().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();
}
}
///
/// Initializes helper arrays required for the Hungarian algorithm, setting the
/// appropriate sizes and default values.
///
/// The number of rows in the distance matrix.
/// The number of columns in the distance matrix.
void HungarianAlgorithmEigen::init_helper_arrays(int num_rows, int num_columns) {
covered_columns = Eigen::Array::Constant(num_columns, false);
covered_rows = Eigen::Array::Constant(num_rows, false);
star_matrix = Eigen::Array::Constant(num_rows, num_columns, false);
new_star_matrix = Eigen::Array::Constant(num_rows, num_columns, false);
prime_matrix = Eigen::Array::Constant(num_rows, num_columns, false);
}
///
/// Constructs the assignment vector from the star matrix, providing the optimal
/// assignment of rows to columns.
///
/// Reference to the vector where the result of the assignment will be stored.
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;
}
}
}
///
/// Calculates the total cost of the assignment stored in the provided assignment vector.
///
/// The vector containing the assignment for which to calculate the total cost.
/// The total cost of the assignment.
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.
}
///
/// 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.
///
/// The distance matrix representing the cost of assigning rows to columns.
/// Reference to the vector where the result of the assignment will be stored.
/// The minimum total cost of the assignment.
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);
}
}