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