REAL QPBO<REAL>::ComputeTwiceEnergy(int option)
{
REAL E = 2*zero_energy, E1[2], E2[2][2];
int i, j, e;
int xi, xj;
for (i=0; i<GetNodeNum(); i++)
{
GetTwiceUnaryTerm(i, E1[0], E1[1]);
if (option == 0) xi = (nodes[0][i].label < 0) ? 0 : nodes[0][i].label;
else xi = nodes[0][i].user_label;
code_assert(xi==0 || xi==1);
E += E1[xi] - E1[0];
}
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
{
GetTwicePairwiseTerm(e, i, j, E2[0][0], E2[0][1], E2[1][0], E2[1][1]);
if (option == 0)
{
xi = (nodes[0][i].label < 0) ? 0 : nodes[0][i].label;
xj = (nodes[0][j].label < 0) ? 0 : nodes[0][j].label;
}
else
{
xi = nodes[0][i].user_label;
xj = nodes[0][j].user_label;
}
E += E2[xi][xj] - E2[0][0];
}
return E;
}
void QPBO<REAL>::reallocate_nodes(int node_num_max_new)
{
code_assert(node_num_max_new > node_shift/((int)sizeof(Node)));
Node* nodes_old[2] = { nodes[0], nodes[1] };
int node_num_max = node_num_max_new;
nodes[0] = (Node*) realloc(nodes_old[0], 2*node_num_max*sizeof(Node));
if (!nodes[0]) { if (error_function) (*error_function)("Not enough memory!"); exit(1); }
node_shift = node_num_max*sizeof(Node);
node_last[0] = nodes[0] + node_num;
node_max[0] = nodes[1] = nodes[0] + node_num_max;
node_last[1] = nodes[1] + node_num;
node_max[1] = nodes[1] + node_num_max;
if (stage)
{
memmove(nodes[1], (char*)nodes[0] + ((char*)nodes_old[1] - (char*)nodes_old[0]), node_num*sizeof(Node));
}
Arc* a;
for (a=arcs[0]; a<arc_max[stage]; a++)
{
if (a->sister)
{
int k = (a->head < nodes_old[1]) ? 0 : 1;
a->head = (Node*) ((char*)a->head + (((char*) nodes[k]) - ((char*) nodes_old[k])));
}
}
}
void QPBO<REAL>::reallocate_arcs(int arc_num_max_new)
{
int arc_num_max_old = (int)(arc_max[0] - arcs[0]);
int arc_num_max = arc_num_max_new;
if (arc_num_max & 1) arc_num_max ++;
code_assert(arc_num_max > arc_num_max_old);
Arc* arcs_old[2] = { arcs[0], arcs[1] };
arcs[0] = (Arc*) realloc(arcs_old[0], 2*arc_num_max*sizeof(Arc));
if (!arcs[0]) {
if (error_function) (*error_function)("Not enough memory!");
exit(1);
}
arc_shift = arc_num_max*sizeof(Arc);
arc_max[0] = arcs[1] = arcs[0] + arc_num_max;
arc_max[1] = arcs[1] + arc_num_max;
if (stage)
{
memmove(arcs[1], arcs[0]+arc_num_max_old, arc_num_max_old*sizeof(Arc));
memset(arcs[0]+arc_num_max_old, 0, (arc_num_max-arc_num_max_old)*sizeof(Arc));
memset(arcs[1]+arc_num_max_old, 0, (arc_num_max-arc_num_max_old)*sizeof(Arc));
}
else
{
memset(arcs[0]+arc_num_max_old, 0, (2*arc_num_max-arc_num_max_old)*sizeof(Arc));
}
Node* i;
Arc* a;
for (i=nodes[0]; i<node_last[stage]; i++)
{
if (i==node_last[0]) i = nodes[1];
if (i->first)
{
int k = (i->first < arcs_old[1]) ? 0 : 1;
i->first = (Arc*) ((char*)i->first + (((char*) arcs[k]) - ((char*) arcs_old[k])));
}
}
for (a=arcs[0]; a<arc_max[stage]; a++)
{
if (a->sister)
{
if (a->next)
{
int k = (a->next < arcs_old[1]) ? 0 : 1;
a->next = (Arc*) ((char*)a->next + (((char*) arcs[k]) - ((char*) arcs_old[k])));
}
int k = (a->sister < arcs_old[1]) ? 0 : 1;
a->sister = (Arc*) ((char*)a->sister + (((char*) arcs[k]) - ((char*) arcs_old[k])));
}
}
InitFreeList();
}
void QPBO<REAL>::TestRelaxedSymmetry()
{
Node* i;
Arc* a;
REAL c1, c2;
if (stage == 0) return;
for (i=nodes[0]; i<node_last[0]; i++)
{
if (i->is_removed) continue;
c1 = i->tr_cap;
for (a=i->first; a; a=a->next) c1 += a->sister->r_cap;
c2 = -GetMate0(i)->tr_cap;
for (a=GetMate0(i)->first; a; a=a->next) c2 += a->r_cap;
if (c1 != c2)
{
code_assert(0);
exit(1);
}
}
}
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;
}
void QPBO<REAL>::ComputeWeakPersistencies()
{
if (stage == 0) return;
Node* i;
Node* j;
Node* stack = NULL;
int component;
for (i=nodes[0]; i<node_last[0]; i++)
{
code_assert(i->label>=-1 && i->label<=1);
Node* i1 = GetMate0(i);
if (i->label >= 0)
{
i->dfs_parent = i;
i1->dfs_parent = i1;
i->region = i1->region = 0;
}
else
{
i->dfs_parent = i1->dfs_parent = NULL;
i->region = i1->region = -1;
}
}
// first DFS
for (i=nodes[0]; i<node_last[1]; i++)
{
if (i == node_last[0]) i = nodes[1];
if (i->dfs_parent) continue;
// DFS starting from i
i->dfs_parent = i;
i->dfs_current = i->first;
while ( 1 )
{
if (!i->dfs_current)
{
i->next = stack;
stack = i;
if (i->dfs_parent == i) break;
i = i->dfs_parent;
i->dfs_current = i->dfs_current->next;
continue;
}
j = i->dfs_current->head;
if (!(i->dfs_current->r_cap>0) || j->dfs_parent)
{
i->dfs_current = i->dfs_current->next;
continue;
}
j->dfs_parent = i;
i = j;
i->dfs_current = i->first;
}
}
// second DFS
component = 0;
while ( stack )
{
i = stack;
stack = i->next;
if (i->region > 0) continue;
i->region = ++ component;
i->dfs_parent = i;
i->dfs_current = i->first;
while ( 1 )
{
if (!i->dfs_current)
{
if (i->dfs_parent == i) break;
i = i->dfs_parent;
i->dfs_current = i->dfs_current->next;
continue;
}
j = i->dfs_current->head;
if (!(i->dfs_current->sister->r_cap>0) || j->region>=0)
{
i->dfs_current = i->dfs_current->next;
continue;
}
j->dfs_parent = i;
i = j;
i->dfs_current = i->first;
i->region = component;
}
}
// assigning labels
for (i=nodes[0]; i<node_last[0]; i++)
{
if (i->label < 0)
{
code_assert(i->region > 0);
if (i->region > GetMate0(i)->region) { i->label = 0; i->region = 0; }
else if (i->region < GetMate0(i)->region) { i->label = 1; i->region = 0; }
}
else code_assert(i->region == 0);
}
}
