void QPBO<REAL>::MergeParallelEdges()
{
if (stage == 0) TransformToSecondStage(false);
Node* i;
Node* j;
Arc* a;
Arc* a_next;
for (i=nodes[0]; i<node_last[0]; i++)
{
for (a=i->first; a; a=a->next)
{
j = a->head;
if (!IsNode0(j)) j = GetMate1(j);
j->parent = a;
}
for (a=i->first; a; a=a_next)
{
a_next = a->next;
j = a->head;
if (!IsNode0(j)) j = GetMate1(j);
if (j->parent == a) continue;
if (MergeParallelEdges(j->parent, a)==0)
{
j->parent = a;
a_next = a->next;
}
}
}
}
void QPBO<REAL>::Stitch()
{
if (stage == 0) return;
Node* i;
Node* i_mate;
Node* j;
Arc* a;
Arc* a_mate;
for (a=arcs[0], a_mate=arcs[1]; a<arc_max[0]; a++, a_mate++)
if (a->sister)
{
a->r_cap = a_mate->r_cap = a->r_cap + a_mate->r_cap;
i = a->sister->head;
j = a->head;
if (i->region==0 || i->region != j->region) continue;
if (IsNode0(i))
{
if (i->user_label != 0) continue;
}
else
{
if (GetMate1(i)->user_label != 1) continue;
}
if (IsNode0(j))
{
if (j->user_label != 1) continue;
}
else
{
if (GetMate1(j)->user_label != 0) continue;
}
a->r_cap = a_mate->r_cap = 0;
}
for (i=nodes[0], i_mate=nodes[1]; i<node_last[0]; i++, i_mate++)
{
i->tr_cap = i->tr_cap - i_mate->tr_cap;
i_mate->tr_cap = -i->tr_cap;
}
ComputeWeakPersistencies();
}
inline void QPBO<REAL>::add_to_changed_list(Node *i)
{
if (keep_changed_list)
{
if (!IsNode0(i)) i = GetMate1(i);
if (!i->is_in_changed_list)
{
Node** ptr = changed_list->New();
*ptr = i;;
i->is_in_changed_list = true;
}
}
}
void QPBO<REAL>::test_consistency(Node* current_node)
{
Node *i;
Arc *a;
int r;
int num1 = 0, num2 = 0;
// test whether all nodes i with i->next!=NULL are indeed in the queue
for (i=nodes[0]; i<node_last[stage]; i++)
{
if (i==node_last[0]) i = nodes[1];
if ((IsNode0(i) && i->is_removed) || (!IsNode0(i) && GetMate1(i)->is_removed))
{
code_assert(i->first == NULL);
continue;
}
if (i->next || i==current_node) num1 ++;
}
for (r=0; r<3; r++)
{
i = (r == 2) ? current_node : queue_first[r];
if (i)
for ( ; ; i=i->next)
{
code_assert((IsNode0(i) && !i->is_removed) || (!IsNode0(i) && !GetMate1(i)->is_removed));
num2 ++;
if (i->next == i)
{
if (r<2) code_assert(i == queue_last[r]);
else code_assert(i == current_node);
break;
}
}
}
code_assert(num1 == num2);
for (i=nodes[0]; i<node_last[stage]; i++)
{
if (i==node_last[0]) i = nodes[1];
if ((IsNode0(i) && i->is_removed) || (!IsNode0(i) && GetMate1(i)->is_removed)) continue;
// test whether all edges in seach trees are non-saturated
if (i->parent == NULL) {}
else if (i->parent == QPBO_MAXFLOW_ORPHAN) {}
else if (i->parent == QPBO_MAXFLOW_TERMINAL)
{
if (!i->is_sink) code_assert(i->tr_cap > 0);
else code_assert(i->tr_cap < 0);
}
else
{
if (!i->is_sink) code_assert (i->parent->sister->r_cap > 0);
else code_assert (i->parent->r_cap > 0);
}
// test whether passive nodes in search trees have neighbors in
// a different tree through non-saturated edges
if (i->parent && !i->next)
{
if (!i->is_sink)
{
code_assert(i->tr_cap >= 0);
for (a=i->first; a; a=a->next)
{
if (a->r_cap > 0) code_assert(a->head->parent && !a->head->is_sink);
}
}
else
{
code_assert(i->tr_cap <= 0);
for (a=i->first; a; a=a->next)
{
if (a->sister->r_cap > 0) code_assert(a->head->parent && a->head->is_sink);
}
}
}
// test marking invariants
if (i->parent && i->parent!=QPBO_MAXFLOW_ORPHAN && i->parent!=QPBO_MAXFLOW_TERMINAL)
{
code_assert(i->TS <= i->parent->head->TS);
if (i->TS == i->parent->head->TS) code_assert(i->DIST > i->parent->head->DIST);
}
}
}
void QPBO<REAL>::maxflow_reuse_trees_init()
{
Node* i;
Node* j;
Node* queue = queue_first[1];
Arc* a;
nodeptr* np;
queue_first[0] = queue_last[0] = NULL;
queue_first[1] = queue_last[1] = NULL;
orphan_first = orphan_last = NULL;
TIME ++;
while ((i=queue))
{
queue = i->next;
if (queue == i) queue = NULL;
if (IsNode0(i))
{
if (i->is_removed) continue;
}
else
{
if (GetMate1(i)->is_removed) continue;
}
i->next = NULL;
i->is_marked = 0;
set_active(i);
if (i->tr_cap == 0)
{
if (i->parent) set_orphan_rear(i);
continue;
}
if (i->tr_cap > 0)
{
if (!i->parent || i->is_sink)
{
i->is_sink = 0;
for (a=i->first; a; a=a->next)
{
j = a->head;
if (!j->is_marked)
{
if (j->parent == a->sister) set_orphan_rear(j);
if (j->parent && j->is_sink && a->r_cap > 0) set_active(j);
}
}
add_to_changed_list(i);
}
}
else
{
if (!i->parent || !i->is_sink)
{
i->is_sink = 1;
for (a=i->first; a; a=a->next)
{
j = a->head;
if (!j->is_marked)
{
if (j->parent == a->sister) set_orphan_rear(j);
if (j->parent && !j->is_sink && a->sister->r_cap > 0) set_active(j);
}
}
add_to_changed_list(i);
}
}
i->parent = QPBO_MAXFLOW_TERMINAL;
i -> TS = TIME;
i -> DIST = 1;
}
code_assert(stage == 1);
//test_consistency();
/* adoption */
while ((np=orphan_first))
{
orphan_first = np -> next;
i = np -> ptr;
nodeptr_block -> Delete(np);
if (!orphan_first) orphan_last = NULL;
if (i->is_sink) process_sink_orphan(i);
else process_source_orphan(i);
}
/* adoption end */
//test_consistency();
}
void QPBO<REAL>::TransformToSecondStage(bool copy_trees)
{
// add non-submodular edges
Node* i[2];
Node* j[2];
Arc* a[2];
memset(nodes[1], 0, node_num*sizeof(Node));
node_last[1] = nodes[1] + node_num;
if (!copy_trees)
{
for (i[0]=nodes[0], i[1]=nodes[1]; i[0]<node_last[0]; i[0]++, i[1]++)
{
i[1]->first = NULL;
i[1]->tr_cap = -i[0]->tr_cap;
}
for (a[0]=arcs[0], a[1]=arcs[1]; a[0]<arc_max[0]; a[0]+=2, a[1]+=2)
{
if (!a[0]->sister) continue;
code_assert(IsNode0(a[0]->sister->head));
SET_SISTERS(a[1], a[1]+1);
if (IsNode0(a[0]->head))
{
i[1] = GetMate0(a[0]->sister->head);
j[1] = GetMate0(a[0]->head);
SET_FROM(a[1], j[1]);
SET_FROM(a[1]->sister, i[1]);
SET_TO(a[1], i[1]);
SET_TO(a[1]->sister, j[1]);
}
else
{
i[0] = a[0]->sister->head;
i[1] = GetMate0(i[0]);
j[1] = a[0]->head;
j[0] = GetMate1(j[1]);
SET_FROM(a[0], i[0]);
SET_FROM(a[0]->sister, j[1]);
SET_FROM(a[1], j[0]);
SET_FROM(a[1]->sister, i[1]);
SET_TO(a[1], i[1]);
SET_TO(a[1]->sister, j[0]);
}
a[1]->r_cap = a[0]->r_cap;
a[1]->sister->r_cap = a[0]->sister->r_cap;
}
}
else
{
for (i[0]=nodes[0], i[1]=nodes[1]; i[0]<node_last[0]; i[0]++, i[1]++)
{
i[1]->first = NULL;
i[1]->tr_cap = -i[0]->tr_cap;
i[1]->is_sink = i[0]->is_sink ^ 1;
i[1]->DIST = i[0]->DIST;
i[1]->TS = i[0]->TS;
if (i[0]->parent == NULL || i[0]->parent == QPBO_MAXFLOW_TERMINAL) i[1]->parent = i[0]->parent;
else i[1]->parent = GetMate0(i[0]->parent->sister);
}
for (a[0]=arcs[0], a[1]=arcs[1]; a[0]<arc_max[0]; a[0]+=2, a[1]+=2)
{
if (!a[0]->sister) continue;
code_assert(IsNode0(a[0]->sister->head));
SET_SISTERS(a[1], a[1]+1);
if (IsNode0(a[0]->head))
{
i[1] = GetMate0(a[0]->sister->head);
j[1] = GetMate0(a[0]->head);
SET_FROM(a[1], j[1]);
SET_FROM(a[1]->sister, i[1]);
SET_TO(a[1], i[1]);
SET_TO(a[1]->sister, j[1]);
}
else
{
i[0] = a[0]->sister->head;
i[1] = GetMate0(i[0]);
j[1] = a[0]->head;
j[0] = GetMate1(j[1]);
SET_FROM(a[0], i[0]);
SET_FROM(a[0]->sister, j[1]);
SET_FROM(a[1], j[0]);
SET_FROM(a[1]->sister, i[1]);
SET_TO(a[1], i[1]);
SET_TO(a[1]->sister, j[0]);
mark_node(i[0]);
mark_node(i[1]);
mark_node(j[0]);
mark_node(j[1]);
}
a[1]->r_cap = a[0]->r_cap;
a[1]->sister->r_cap = a[0]->sister->r_cap;
}
}
stage = 1;
}
void QPBO<REAL>::AddPairwiseTerm(EdgeId e, NodeId _i, NodeId _j, REAL E00, REAL E01, REAL E10, REAL E11)
{
user_assert(e >= 0 && arcs[0][2*e].sister);
user_assert(arcs[0][2*e].head==&nodes[0][_i] || arcs[0][2*e].head==&nodes[1][_i] || arcs[0][2*e].head==&nodes[0][_j] || arcs[0][2*e].head==&nodes[1][_j]);
user_assert(arcs[0][2*e+1].head==&nodes[0][_i] || arcs[0][2*e+1].head==&nodes[1][_i] || arcs[0][2*e+1].head==&nodes[0][_j] || arcs[0][2*e+1].head==&nodes[1][_j]);
user_assert(_i != _j);
REAL delta, ci, cj, cij, cji;
if (stage == 0)
{
Arc* a = &arcs[0][2*e];
Arc* a_rev = &arcs[0][2*e+1];
code_assert(a->sister==a_rev && a->sister==a_rev);
Node* i = a_rev->head;
Node* j = a->head;
code_assert(IsNode0(i));
if (i != &nodes[0][_i]) { delta = E01; E01 = E10; E10 = delta; }
if (IsNode0(j))
{
ComputeWeights(E00, E01, E10, E11, ci, cj, cij, cji);
i->tr_cap += ci;
j->tr_cap += cj;
a->r_cap += cij;
a_rev->r_cap += cji;
if (a->r_cap < 0)
{
delta = a->r_cap;
a->r_cap = 0;
a_rev->r_cap += delta;
i->tr_cap -= delta;
j->tr_cap += delta;
}
if (a_rev->r_cap < 0)
{
delta = a_rev->r_cap;
a_rev->r_cap = 0;
a->r_cap += delta;
j->tr_cap -= delta;
i->tr_cap += delta;
}
if (a->r_cap < 0)
{
all_edges_submodular = false;
REMOVE_FROM(a, i);
REMOVE_FROM(a_rev, j);
SET_TO(a, GetMate0(j));
delta = a->r_cap;
i->tr_cap -= delta;
a->r_cap = -delta;
}
}
else
{
j = GetMate1(j);
ComputeWeights(E01, E00, E11, E10, ci, cj, cij, cji);
i->tr_cap += ci;
j->tr_cap -= cj;
a->r_cap += cij;
a_rev->r_cap += cji;
if (a->r_cap < 0)
{
delta = a->r_cap;
a->r_cap = 0;
a_rev->r_cap += delta;
i->tr_cap -= delta;
j->tr_cap -= delta;
}
if (a_rev->r_cap < 0)
{
delta = a_rev->r_cap;
a_rev->r_cap = 0;
a->r_cap += delta;
j->tr_cap += delta;
i->tr_cap += delta;
}
if (a->r_cap < 0)
{
SET_FROM(a, i);
SET_FROM(a_rev, j);
SET_TO(a, j);
delta = a->r_cap;
i->tr_cap -= delta;
a->r_cap = -delta;
}
}
}
else
{
Arc* a[2] = { &arcs[0][2*e], &arcs[1][2*e] };
Arc* a_rev[2] = { &arcs[0][2*e+1], &arcs[1][2*e+1] };
code_assert(a[0]->sister==a_rev[0] && a[1]->sister==a_rev[1] && a[0]==a_rev[0]->sister && a[1]==a_rev[1]->sister);
Node* i[2] = { a_rev[0]->head, a[1]->head };
Node* j[2] = { a[0]->head, a_rev[1]->head };
int k = IsNode0(i[0]) ? 0 : 1;
if (i[k] != &nodes[0][_i]) { delta = E01; E01 = E10; E10 = delta; }
if (IsNode0(j[k]))
{
ComputeWeights(E00, E01, E10, E11, ci, cj, cij, cji);
}
else
{
ComputeWeights(E01, E00, E11, E10, ci, cj, cij, cji);
};
// make sure that a[0]->r_cap == a[1]->r_cap and a_rev[0]->r_cap == a_rev[1]->r_cap by pushing flow
delta = a[1]->r_cap - a[0]->r_cap;
//a[1]->r_cap -= delta; // don't do the subtraction - later we'll set explicitly a[1]->r_cap = a[0]->r_cap
//a[1]->sister->r_cap += delta;
a_rev[1]->head->tr_cap -= delta;
a[1]->head->tr_cap += delta;
i[0]->tr_cap += ci; i[1]->tr_cap -= ci;
j[0]->tr_cap += cj; j[1]->tr_cap -= cj;
a[0]->r_cap += cij;
a_rev[0]->r_cap += cji;
if (a[0]->r_cap < 0)
{
delta = a[0]->r_cap;
a[0]->r_cap = 0;
a_rev[0]->r_cap += delta;
i[0]->tr_cap -= delta; i[1]->tr_cap += delta;
j[0]->tr_cap += delta; j[1]->tr_cap -= delta;
}
if (a_rev[0]->r_cap < 0)
{
delta = a_rev[0]->r_cap;
a_rev[0]->r_cap = 0;
a[0]->r_cap += delta;
j[0]->tr_cap -= delta; j[1]->tr_cap += delta;
i[0]->tr_cap += delta; i[1]->tr_cap -= delta;
}
if (a[0]->r_cap < 0)
{
// need to swap submodular <-> supermodular
SET_TO(a[0], j[1]);
SET_TO(a_rev[1], j[0]);
REMOVE_FROM(a_rev[0], j[0]);
SET_FROM(a_rev[0], j[1]);
REMOVE_FROM(a[1], j[1]);
SET_FROM(a[1], j[0]);
delta = a[0]->r_cap;
i[0]->tr_cap -= delta; i[1]->tr_cap += delta;
a[0]->r_cap = -delta;
}
a[1]->r_cap = a[0]->r_cap;
a_rev[1]->r_cap = a_rev[0]->r_cap;
}
zero_energy += E00;
}