void QPBO<REAL>::Solve()
{
Node* i;
maxflow();
if (stage == 0)
{
if (all_edges_submodular)
{
for (i=nodes[0]; i<node_last[0]; i++)
{
i->label = what_segment(i);
}
return;
}
TransformToSecondStage(true);
maxflow(true);
}
for (i=nodes[0]; i<node_last[0]; i++)
{
i->label = what_segment(i);
if (i->label == what_segment(GetMate0(i))) i->label = -1;
}
}
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>::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;
}
typename QPBO<REAL>::EdgeId QPBO<REAL>::AddPairwiseTerm(NodeId _i, NodeId _j, REAL E00, REAL E01, REAL E10, REAL E11)
{
//printf("%d,%d",_i,node_num);
user_assert(_i >= 0 && _i < node_num);
user_assert(_j >= 0 && _j < node_num);
user_assert(_i != _j);
REAL ci, cj, cij, cji;
if (!first_free)
{
reallocate_arcs(2*(GetMaxEdgeNum() + GetMaxEdgeNum()/2));
}
EdgeId e = (int)(first_free - arcs[IsArc0(first_free) ? 0 : 1])/2;
first_free = first_free->next;
if (stage == 0)
{
Arc *a, *a_rev;
a = &arcs[0][2*e];
a_rev = &arcs[0][2*e+1];
Node* i = nodes[0] + _i;
Node* j = nodes[0] + _j;
if (E01 + E10 >= E00 + E11)
{
ComputeWeights(E00, E01, E10, E11, ci, cj, cij, cji);
SET_TO(a, j);
SET_FROM(a, i);
SET_FROM(a_rev, j);
j->tr_cap += cj;
}
else
{
all_edges_submodular = false;
ComputeWeights(E01, E00, E11, E10, ci, cj, cij, cji);
SET_TO(a, GetMate0(j));
a->next = NULL;
a_rev->next = NULL;
j->tr_cap -= cj;
}
SET_SISTERS(a, a_rev);
SET_TO(a_rev, i);
i->tr_cap += ci;
a->r_cap = cij;
a_rev->r_cap = cji;
}
else
{
Arc *a[2], *a_rev[2];
a[0] = &arcs[0][2*e];
a_rev[0] = &arcs[0][2*e+1];
a[1] = &arcs[1][2*e];
a_rev[1] = &arcs[1][2*e+1];
Node* i[2] = { nodes[0] + _i, nodes[1] + _i };
Node* j[2];
if (E01 + E10 >= E00 + E11)
{
j[0] = nodes[0] + _j; j[1] = nodes[1] + _j;
ComputeWeights(E00, E01, E10, E11, ci, cj, cij, cji);
}
else
{
j[1] = nodes[0] + _j; j[0] = nodes[1] + _j;
ComputeWeights(E01, E00, E11, E10, ci, cj, cij, cji);
}
SET_SISTERS(a[0], a_rev[0]);
SET_SISTERS(a[1], a_rev[1]);
SET_TO(a[0], j[0]);
SET_TO(a_rev[0], i[0]);
SET_TO(a[1], i[1]);
SET_TO(a_rev[1], j[1]);
SET_FROM(a[0], i[0]);
SET_FROM(a_rev[0], j[0]);
SET_FROM(a[1], j[1]);
SET_FROM(a_rev[1], i[1]);
i[0]->tr_cap += ci; i[1]->tr_cap -= ci;
j[0]->tr_cap += cj; j[1]->tr_cap -= cj;
a[0]->r_cap = a[1]->r_cap = cij;
a_rev[0]->r_cap = a_rev[1]->r_cap = cji;
}
zero_energy += E00;
return e;
}
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);
}
}