A std::priority_queue by default is just a std::vector that is kept in a sorted state. It can still be expensive to insert and remove elements from the queue, and as you noticed, you would potentially need to update all of the Costelements in the queue when you insert or remove an element from matrix in order to relect the new positions. However, you can make that a bit more efficient by making the priority queue two-dimensional as well, something that looks like:
std::priority_queue<std::priority_queue<Costelement, ...>, ...> cost_matrix;
Basically, the inner priority queue sort the cost of the columns of a single row, the outer priority queue should then sort the cost of whole rows. Let's create ColumnCost and RowCost structs:
struct ColumnCost {
int column;
double cost;
friend bool operator<(const ColumnCost &a, const ColumnCost &b) {
return a.cost > b.cost;
}
};
struct RowCost {
int row;
std::priority_queue<ColumnCost> columns;
friend bool operator<(const RowCost &a, const RowCost &b) {
return a.columns.top() > b.columns.top();
}
};
std::priority_queue<RowCost> cost_matrix;
Now you can easily get the lowest cost element from costmatrix, which returns the RowCost which contains the lowest cost element, and then you get the ColumnCost with the lowest cost from that one:
const auto &lowest_row = cost_matrix.top();
const auto &lowest_column = lowest_row.columns.top();
int row = lowest_row.row;
int column = lowest_column.column;
When you now insert or delete an element from matrix, you insert or delete from cost_matrix in the same way. You still need to update row or column coordinates, but now it is much less work. The only thing to be aware of is that if you update add or remove an element to the priority queue of a RowCost, you need to delete and re-insert that whole row from cost_matrix to ensure the outer priority queue is kept correctly sorted.
Another possible optimization is to use a std::priority_queue to keep the rows sorted, but use std::min_element() to keep track of the minimum of each individual row. This greatly reduces the amount of memory necessary to store the cost_matrix, and you would only need to call std::min_element() to recalculate the minimum cost element of a row when you change that row.