int Hungarian(){
int ret = 0;
mxMatch = 0;
ms(xy,-1);
ms(yx,-1);
initLabels();
Augment();
rep(x,N) ret += cost[x][xy[x]];
return ret;
}
void MinCost<FlowType, CostType>::Dijkstra(Node* start)
{
assert(start->excess > 0);
Node* i;
Node* j;
Arc* a;
CostType d;
Node* permanentNodes;
int FLAG0 = ++ counter; // permanently labeled nodes
int FLAG1 = ++ counter; // temporarily labeled nodes
start->parent = NULL;
start->flag = FLAG1;
queue.Reset();
queue.Add(start, 0);
permanentNodes = NULL;
while ( (i=queue.RemoveMin(d)) )
{
if (i->excess < 0)
{
FlowType delta = Augment(start, i);
cost += delta*(d - i->pi + start->pi);
for (i=permanentNodes; i; i=i->next_permanent) i->pi += d;
break;
}
i->pi -= d;
i->flag = FLAG0;
i->next_permanent = permanentNodes;
permanentNodes = i;
for (a=i->firstNonsaturated; a; a=a->next)
{
j = a->head;
if (j->flag == FLAG0) continue;
d = a->GetRCost();
if (j->flag == FLAG1)
{
if (d >= queue.GetKey(j)) continue;
queue.DecreaseKey(j, d);
}
else
{
queue.Add(j, d);
j->flag = FLAG1;
}
j->parent = a;
}
}
}
double* CLinearSystem::Solve()
{
printf("system to solve: \n");
PrettyPrint();
printf("==== solving ==== \n");
Normalize();
Augment();
Reduce();
Augment();
PrettyPrint();
if(m_result != NULL)
{
delete m_result;
}
m_result = new double[m_n];
for(int n = 0; n < m_n; n++)
{
m_result[n] = Get(n,m_m-1);
}
return m_result;
}
int CUKF::Doit()
{
// VehicleModel();
//StopWatch("start");
AddControlNoise();
//StopWatch("AddControlNoise");
Prediction();
//StopWatch("Prediction");
// PrintCvMat(XX, "after prediction : XX");
// PrintCvMat(PX, "after prediction : PX");
static double dtsum=0;
dtsum += DT_CONTROLS;
if(dtsum >= DT_OBSERVE)
{
// TRACE("xtrue : %.4f %.4f %.4f\n", xtrue->data.db[0], xtrue->data.db[1], xtrue->data.db[2]);
dtsum = 0;
GetVisibleLandmark();
//StopWatch("GetVisibleLandmark");
AddObservationNoise();
//StopWatch("AddObservationNoise");
KnownDataAssociation();
//StopWatch("KnownDataAssociation");
// PrintCvMat(XX, "before update : XX");
// PrintCvMat(PX, "before update : PX");
Update();
//StopWatch("Update");
// PrintCvMat(XX, "after update : XX");
// PrintCvMat(PX, "after update : PX");
Augment();
//StopWatch("Augment");
//PrintCvMat(XX, "after augment : XX");
//PrintCvMat(PX, "after augment : PX");
//TRACE("XX dim : %d %d\n", XX->rows, XX->cols);
//TRACE("PX dim : %d %d\n", PX->rows, PX->cols);
// TRACE("==== datable ====\n");
// for(int i=0; i<lm.size(); i++)
// TRACE("%d\t", daTable[i]);
// TRACE("=================\n");
}
EllipseFitting();
//StopWatch("EllipseFitting");
return 0;
}
void BidirectionalIBFS::IBFS() {
auto start = Clock::now();
m_forward_search = false;
m_source_tree_d = 1;
m_sink_tree_d = 0;
IBFSInit();
// Set up initial current_q and search nodes to make it look like
// we just finished scanning the sink node
NodeQueue* current_q = &(m_sink_layers[0]);
m_search_node_iter = current_q->end();
m_search_node_end = current_q->end();
while (!current_q->empty()) {
if (m_search_node_iter == m_search_node_end) {
// Swap queues and continue
if (m_forward_search) {
m_source_tree_d++;
current_q = &(m_sink_layers[m_sink_tree_d]);
} else {
m_sink_tree_d++;
current_q = &(m_source_layers[m_source_tree_d]);
}
m_search_node_iter = current_q->begin();
m_search_node_end = current_q->end();
m_forward_search = !m_forward_search;
if (!current_q->empty()) {
Node& n = *m_search_node_iter;
NodeId nodeIdx = n.id;
if (m_forward_search) {
ASSERT(n.state == NodeState::S || n.state == NodeState::S_orphan);
m_search_arc = m_graph->ArcsBegin(nodeIdx);
m_search_arc_end = m_graph->ArcsEnd(nodeIdx);
} else {
ASSERT(n.state == NodeState::T || n.state == NodeState::T_orphan);
m_search_arc = m_graph->ArcsBegin(nodeIdx);
m_search_arc_end = m_graph->ArcsEnd(nodeIdx);
}
}
continue;
}
Node& n = *m_search_node_iter;
NodeId search_node = n.id;
int distance;
if (m_forward_search) {
distance = m_source_tree_d;
} else {
distance = m_sink_tree_d;
}
ASSERT(n.dis == distance);
// Advance m_search_arc until we find a residual arc
while (m_search_arc != m_search_arc_end && !m_graph->NonzeroCap(m_search_arc, m_forward_search))
++m_search_arc;
if (m_search_arc != m_search_arc_end) {
NodeId neighbor = m_search_arc.Target();
NodeState neighbor_state = m_graph->node(neighbor).state;
if (neighbor_state == n.state) {
ASSERT(m_graph->node(neighbor).dis <= n.dis + 1);
if (m_graph->node(neighbor).dis == n.dis+1) {
auto reverseArc = m_search_arc.Reverse();
if (reverseArc < m_graph->node(neighbor).parent_arc) {
m_graph->node(neighbor).parent_arc = reverseArc;
m_graph->node(neighbor).parent = search_node;
}
}
++m_search_arc;
} else if (neighbor_state == NodeState::N) {
// Then we found an unlabeled node, add it to the tree
m_graph->node(neighbor).state = n.state;
m_graph->node(neighbor).dis = n.dis + 1;
AddToLayer(neighbor);
auto reverseArc = m_search_arc.Reverse();
m_graph->node(neighbor).parent_arc = reverseArc;
ASSERT(m_graph->NonzeroCap(m_graph->node(neighbor).parent_arc, !m_forward_search));
m_graph->node(neighbor).parent = search_node;
++m_search_arc;
} else {
// Then we found an arc to the other tree
ASSERT(neighbor_state != NodeState::S_orphan && neighbor_state != NodeState::T_orphan);
ASSERT(m_graph->NonzeroCap(m_search_arc, m_forward_search));
Augment(m_search_arc);
Adopt();
}
} else {
// No more arcs to scan from this node, so remove from queue
AdvanceSearchNode();
}
} // End while
m_totalTime += Duration{ Clock::now() - start }.count();
//std::cout << "Total time: " << m_totalTime << "\n";
//std::cout << "Init time: " << m_initTime << "\n";
//std::cout << "Augment time: " << m_augmentTime << "\n";
//std::cout << "Adopt time: " << m_adoptTime << "\n";
}
int Augmentation(std::vector<int> &free_rows) {
if(free_rows.size() < 1)
return 1;
for(unsigned int ind=0 ; ind<free_rows.size() ; ind++) {
int free_row = free_rows[ind];
double minimum_d;
std::deque<int> ready_deque, scan_deque;
std::vector<int> todo_list;
ready_deque.clear(); scan_deque.clear(); todo_list.clear();
Eigen::ArrayXi pred = Eigen::ArrayXi::Constant(n_, free_row);
Eigen::ArrayXd d = Eigen::ArrayXd::Zero(n_);
for(int col=1 ; col<=n_ ; col++) {
todo_list.push_back(col);
d(col-1) = cost_mat_(free_row-1, col-1) - v_(col-1);
}
while(1) {
int done = 0;
if(scan_deque.empty()) {
minimum_d = inf_;
std::vector<int> deposit;
std::vector<int>::iterator todo_itr = todo_list.begin();
while(todo_itr != todo_list.end()) {
if(d((*todo_itr)-1) <= minimum_d) {
if(d((*todo_itr)-1) < minimum_d) {
minimum_d = d((*todo_itr)-1);
for(unsigned int ind2=0 ; ind2<scan_deque.size() ; ind2++)
deposit.push_back(scan_deque[ind2]);
scan_deque.clear();
}
scan_deque.push_back((*todo_itr));
todo_itr = todo_list.erase(todo_itr);
}else{
++todo_itr;
}
}
for(unsigned int ind2=0 ; ind2<deposit.size() ; ind2++)
todo_list.push_back(deposit[ind2]);
deposit.clear();
////
for(unsigned int ind2=0 ; ind2<scan_deque.size() ; ind2++) {
if(y_(scan_deque[ind2]-1) == 0) {
Augment(free_row, scan_deque[ind2], ready_deque, minimum_d, d, pred);
done = 1;
}
if(done) break;
}
if(done) break;
}
int scan_col = scan_deque.front();
scan_deque.pop_front();
int prev_row = y_(scan_col-1);
ready_deque.push_back(scan_col);
////
std::vector<int>::iterator todo_itr = todo_list.begin();
while(todo_itr != todo_list.end()) {
int erase = 0;
if(minimum_d + cred(prev_row, *todo_itr) < d((*todo_itr)-1)) {
d((*todo_itr)-1) = minimum_d + cred(prev_row, *todo_itr);
pred((*todo_itr)-1) = prev_row;
if(d((*todo_itr)-1) == minimum_d) {
if(y_((*todo_itr)-1) == 0) {
Augment(free_row, (*todo_itr), ready_deque, minimum_d, d, pred);
done = 1;
} else {
scan_deque.push_back(*todo_itr);
todo_itr = todo_list.erase(todo_itr);
erase = 1;
}
if(done) break;
}
}
if(!erase)
++todo_itr;
}
if(done) break;
}
}
return 0;
}
void Augment(){
if (mxMatch == N) return;
int x, y, root;
int q[MX], wr = 0, rd = 0;
ms(S,false);
ms(T,false);
ms(prev,-1);
for( x=0;x<N;x++ )
if (xy[x] == -1){
q[wr++] = root = x;
prev[x] = -2;
S[x] = true;
break;
}
for( y=0;y<N;y++ ){
slack[y] = lx[root] + ly[y] - cost[root][y];
slackX[y] = root;
}
while( true ){
while (rd < wr){
x = q[rd++];
for( y=0;y<N;y++ ){
if (cost[x][y] == lx[x] + ly[y] && !T[y]){
if (yx[y] == -1) break;
T[y] = true;
q[wr++] = yx[y];
Add_To_Tree(yx[y], x);
}
}
if (y < N) break;
}
if (y < N) break;
Update_Labels();
wr = rd = 0;
for( y=0;y<N;y++ ){
if (!T[y] && slack[y] == 0){
if (yx[y] == -1){
x = slackX[y];
break;
}
else{
T[y] = true;
if (!S[yx[y]]){
q[wr++] = yx[y];
Add_To_Tree(yx[y], slackX[y]);
}
}
}
}
if (y < N) break;
}
if (y < N){
mxMatch++;
for (int cx = x, cy = y, ty; cx != -2; cx = prev[cx], cy = ty){
ty = xy[cx];
yx[cy] = cx;
xy[cx] = cy;
}
Augment();
}
}