From 818ac2906f7eb845c097ef8362a9c1f231978f44 Mon Sep 17 00:00:00 2001 From: Matthias Kramm Date: Wed, 25 Nov 2009 11:44:46 -0800 Subject: [PATCH] added graphcut library --- lib/graphcut.c | 634 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ lib/graphcut.h | 56 +++++ 2 files changed, 690 insertions(+) create mode 100644 lib/graphcut.c create mode 100644 lib/graphcut.h diff --git a/lib/graphcut.c b/lib/graphcut.c new file mode 100644 index 0000000..fd6da5a --- /dev/null +++ b/lib/graphcut.c @@ -0,0 +1,634 @@ +/* + graphcut- a graphcut implementation based on the Boykov Kolmogorov algorithm + + Part of the swftools package. + + Copyright (c) 2007,2008,2009 Matthias Kramm + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include +#include +#include +#include +#include "graphcut.h" +#include "../mem.h" + +//#define DEBUG + +//#define CHECKS + +#ifdef DEBUG +#define DBG +#include +#else +#define DBG if(0) +#define assert(x) (x) +#endif + +#define ACTIVE 0x10 +#define IN_TREE 0x20 + +#define TWOTREES + +typedef struct _posqueue_entry { + node_t*pos; + struct _posqueue_entry*next; +} posqueue_entry_t; + +typedef struct _posqueue { + posqueue_entry_t*list; +} posqueue_t; + +typedef struct _graphcut_workspace { + unsigned char*flags1; + unsigned char*flags2; + halfedge_t**back; + graph_t*graph; + node_t*pos1; + node_t*pos2; + posqueue_t*queue1; + posqueue_t*queue2; + posqueue_t*tmpqueue; +} graphcut_workspace_t; + +static posqueue_t*posqueue_new() +{ + posqueue_t*m = (posqueue_t*)malloc(sizeof(posqueue_t)); + memset(m, 0, sizeof(posqueue_t)); + return m; +} +static void posqueue_delete(posqueue_t*q) +{ + posqueue_entry_t*l = q->list; + while(l) { + posqueue_entry_t*next = l->next; + free(l); + l = next; + } + free(q); +} +static inline void posqueue_addpos(posqueue_t*queue, node_t*pos) +{ + posqueue_entry_t*old = queue->list; + queue->list = malloc(sizeof(posqueue_entry_t)); + queue->list->pos = pos; + queue->list->next = old; +} +static inline node_t* posqueue_extract(posqueue_t*queue) +{ + posqueue_entry_t*item = queue->list; + node_t*pos; + if(!item) + return 0; + pos = item->pos; + queue->list = queue->list->next; + free(item); + return pos; +} +static inline int posqueue_notempty(posqueue_t*queue) +{ + return (int)queue->list; +} + +#define NR(p) ((p)->nr) + +static void posqueue_print(graphcut_workspace_t*w, posqueue_t*queue) +{ + posqueue_entry_t*e = queue->list; + while(e) { + halfedge_t*back = w->back[NR(e->pos)]; + printf("%d(%d) ", NR(e->pos), back?NR(back->fwd->node):-1); + e = e->next; + } + printf("\n"); +} +static void posqueue_purge(posqueue_t*queue) +{ + posqueue_entry_t*e = queue->list; + while(e) { + posqueue_entry_t*next = e->next; + e->next = 0;free(e); + e = next; + } + queue->list = 0; +} + +graph_t* graph_new(int num_nodes) +{ + graph_t*graph = rfx_calloc(sizeof(graph_t)); + graph->num_nodes = num_nodes; + graph->nodes = rfx_calloc(sizeof(node_t)*num_nodes); + int t; + for(t=0;tnodes[t].nr = t; + } + return graph; +} + +void graph_delete(graph_t*graph) +{ + int t; + for(t=0;tnum_nodes;t++) { + halfedge_t*e = graph->nodes[t].edges; + while(e) { + halfedge_t*next = e->next; + free(e); + e = next; + } + } + free(graph->nodes);graph->nodes=0; + free(graph); +} + +static graphcut_workspace_t*graphcut_workspace_new(graph_t*graph, node_t*pos1, node_t*pos2) +{ + graphcut_workspace_t*workspace = malloc(sizeof(graphcut_workspace_t)); + workspace->flags1 = rfx_calloc(graph->num_nodes); + workspace->flags2 = rfx_calloc(graph->num_nodes); + workspace->back = rfx_calloc(graph->num_nodes*sizeof(halfedge_t*)); + workspace->pos1 = pos1; + workspace->pos2 = pos2; + workspace->graph = graph; + workspace->queue1 = posqueue_new(); + workspace->queue2 = posqueue_new(); + workspace->tmpqueue = posqueue_new(); + return workspace; +} +static void graphcut_workspace_delete(graphcut_workspace_t*w) +{ + posqueue_delete(w->queue1);w->queue1=0; + posqueue_delete(w->queue2);w->queue2=0; + posqueue_delete(w->tmpqueue);w->tmpqueue=0; + if(w->flags1) free(w->flags1);w->flags1=0; + if(w->flags2) free(w->flags2);w->flags2=0; + if(w->back) free(w->back);w->back=0; + free(w); +} + +typedef struct _path { + node_t**pos; + halfedge_t**dir; + unsigned char*firsthalf; + int length; +} path_t; + +static path_t*path_new(int len) +{ + path_t*p = malloc(sizeof(path_t)); + p->pos = malloc(sizeof(node_t*)*len); + p->dir = malloc(sizeof(halfedge_t*)*len); + p->firsthalf = malloc(sizeof(unsigned char)*len); + p->length = len; + return p; +} +static void path_delete(path_t*path) +{ + free(path->pos);path->pos = 0; + free(path->dir);path->dir = 0; + free(path->firsthalf);path->firsthalf = 0; + free(path); +} + +static path_t*extract_path(graphcut_workspace_t*work, unsigned char*mytree, unsigned char*othertree, node_t*pos, node_t*newpos, halfedge_t*dir) +{ + int t; + node_t*p = pos; + node_t*nodes = work->graph->nodes; + int len1 = 0; + /* walk up tree1 */ + DBG printf("walk back up (1) to %d\n", NR(work->pos1)); + while(p != work->pos1) { + halfedge_t*back = work->back[NR(p)]; + DBG printf("walk backward (1): %d %d\n", NR(p), back?NR(back->fwd->node):-1); + node_t*old = p; + p = work->back[NR(p)]->fwd->node; + assert(p!=old); + len1++; + } + p = newpos; + int len2 = 0; + DBG printf("walk back up (2) to %d\n", NR(work->pos2)); + /* walk up tree2 */ + while(p != work->pos2) { + DBG printf("walk backward (2): %d\n", NR(p)); + p = work->back[NR(p)]->fwd->node; + len2++; + } + path_t*path = path_new(len1+len2+2); + + t = len1; + path->pos[t] = p = pos; + path->dir[t] = dir; + path->firsthalf[t] = 1; + while(p != work->pos1) { + assert(mytree[NR(p)]&IN_TREE); + halfedge_t*dir = work->back[NR(p)]; + assert(dir->node == p); + p = dir->fwd->node; + t--; + path->pos[t] = p; + path->dir[t] = dir->fwd; + path->firsthalf[t] = 1; + } + assert(!t); + + t = len1+1; + + p = newpos; + while(p != work->pos2) { + assert(othertree[NR(p)]&IN_TREE); + halfedge_t*dir = work->back[NR(p)]; + path->pos[t] = p; + path->dir[t] = dir; + path->firsthalf[t] = 0; + p = dir->fwd->node; + t++; + } + + /* terminator */ + path->pos[t] = p; + path->dir[t] = 0; // last node + path->firsthalf[t] = 0; + + assert(t == len1+len2+1); + return path; +} + +static void path_print(path_t*path) +{ + int t; + for(t=0;tlength;t++) { + node_t*n = path->pos[t]; + printf("%d (firsthalf: %d)", NR(n), path->firsthalf[t]); + if(tlength-1) { + printf(" -(%d/%d)-> \n", + path->dir[t]->used, + path->dir[t]->fwd->used); + } else { + printf("\n"); + } + } + + for(t=0;tlength-1;t++) { + if(path->firsthalf[t]==path->firsthalf[t+1]) { + assert(( path->firsthalf[t] && path->dir[t]->used) || + (!path->firsthalf[t] && path->dir[t]->fwd->used)); + } + } + printf("\n"); +} + + +static void workspace_print(graphcut_workspace_t*w) +{ + printf("queue1: ");posqueue_print(w, w->queue1); + printf("queue2: ");posqueue_print(w, w->queue2); +} + +static void myassert(graphcut_workspace_t*w, char assertion, const char*file, int line, const char*func) +{ + if(!assertion) { + printf("Assertion %s:%d (%s) failed:\n", file, line, func); + workspace_print(w); + exit(0); + } +} + +#define ASSERT(w,c) {myassert(w,c,__FILE__,__LINE__,__func__);} + +static path_t* expand_pos(graphcut_workspace_t*w, posqueue_t*queue, node_t*pos, char reverse, unsigned char*mytree, unsigned char*othertree) +{ + graph_t*graph = w->graph; + int dir; + if((mytree[NR(pos)]&(IN_TREE|ACTIVE)) != (IN_TREE|ACTIVE)) { + /* this node got deleted or marked inactive in the meantime. ignore it */ + DBG printf("node %d is deleted or inactive\n", NR(pos)); + return 0; + } + + halfedge_t*e = pos->edges; + for(;e;e=e->next) { + node_t*newpos = e->fwd->node; + weight_t weight = reverse?e->fwd->weight:e->weight; + if(mytree[NR(newpos)]) continue; // already known + + if(weight) { + if(othertree[NR(newpos)]) { + DBG printf("found connection: %d connects to %d\n", NR(pos), NR(newpos)); + posqueue_addpos(queue, pos); mytree[NR(pos)] |= ACTIVE; // re-add, this vertex might have other connections + + path_t*path; + if(reverse) { + path = extract_path(w, othertree, mytree, newpos, pos, e->fwd); + } else { + path = extract_path(w, mytree, othertree, pos, newpos, e); + } + return path; + } else { + DBG printf("advance from %d to new pos %d\n", NR(pos), NR(newpos)); + w->back[NR(newpos)] = e->fwd; + e->used = 1; + posqueue_addpos(queue, newpos); mytree[NR(newpos)] |= ACTIVE|IN_TREE; // add + } + } + } + /* if we can't expand this node anymore, it's now an inactive node */ + mytree[NR(pos)] &= ~ACTIVE; + return 0; +} + +static int node_count_edges(node_t*node) +{ + halfedge_t*e = node->edges; + int num = 0; + while(e) { + num++; + e = e->next; + } + return num; +} + +static void bool_op(graphcut_workspace_t*w, unsigned char*flags, node_t*pos, unsigned char and, unsigned char or) +{ + posqueue_t*q = w->tmpqueue; + posqueue_purge(q); + posqueue_addpos(q, pos); + + while(posqueue_notempty(q)) { + node_t*p = posqueue_extract(q); + flags[NR(p)] = (flags[NR(p)]&and)|or; + halfedge_t*e = p->edges; + while(e) { + if(e->used) { + posqueue_addpos(q, e->fwd->node); + } + e = e->next; + } + } +} + +static weight_t decrease_weights(graph_t*map, path_t*path) +{ + int t; + assert(path->length); + + weight_t min = path->dir[0]->weight; + for(t=0;tlength-1;t++) { + int w = path->dir[t]->weight; + DBG printf("%d->%d (%d)\n", NR(path->dir[t]->node), NR(path->dir[t]->fwd->node), w); + if(t==0 || w < min) min = w; + } + assert(min); + if(min<=0) + return; + + for(t=0;tlength-1;t++) { + path->dir[t]->weight-=min; + path->dir[t]->fwd->weight+=min; + } + return min; +} + +static int reconnect(graphcut_workspace_t*w, unsigned char*flags, node_t*pos, char reverse) +{ + graph_t*graph = w->graph; + + halfedge_t*e = pos->edges; + for(;e;e=e->next) { + node_t*newpos = e->fwd->node; + int weight; + if(!reverse) { + weight = e->fwd->weight; + } else { + weight = e->weight; + } + if(weight && (flags[NR(newpos)]&IN_TREE)) { + DBG printf("successfully reconnected node %d to %d (%d->%d) (reverse:%d)\n", + NR(pos), NR(newpos), NR(e->node), NR(e->fwd->node), reverse); + + w->back[NR(pos)] = e; + e->fwd->used = 1; + return 1; + } + } + return 0; +} + +static void clear_node(graphcut_workspace_t*w, node_t*n) +{ + w->flags1[NR(n)] = 0; + w->flags2[NR(n)] = 0; + w->back[NR(n)] = 0; + halfedge_t*e = n->edges; + while(e) {e->used = 0;e=e->next;} +} + +static void destroy_subtree(graphcut_workspace_t*w, unsigned char*flags, node_t*pos, posqueue_t*posqueue) +{ + DBG printf("destroying subtree starting with %d\n", NR(pos)); + + posqueue_t*q = w->tmpqueue; + posqueue_purge(q); + posqueue_addpos(q, pos); + + while(posqueue_notempty(q)) { + node_t*p = posqueue_extract(q); + halfedge_t*e = p->edges; + while(e) { + node_t*newpos = e->fwd->node; + if(e->used) { + posqueue_addpos(q, newpos); + } else if((flags[NR(newpos)]&(ACTIVE|IN_TREE)) == IN_TREE) { + // re-activate all nodes that surround our subtree. + // TODO: we should check the weight of the edge from that other + // node to our node. if it's zero, we don't need to activate that node. + posqueue_addpos(posqueue, newpos); + flags[NR(newpos)]|=ACTIVE; + } + e = e->next; + } + + clear_node(w, p); + DBG printf("removed pos %d\n", NR(p)); + } +} + +static void combust_tree(graphcut_workspace_t*w, posqueue_t*q1, posqueue_t*q2, path_t*path) +{ + graph_t*graph = w->graph; + int t; + for(t=0;tlength-1 && path->firsthalf[t+1];t++) { + node_t*pos = path->pos[t]; + halfedge_t*dir = path->dir[t]; + node_t*newpos = dir->fwd->node; + if(!dir->weight) { + /* disconnect node */ + DBG printf("remove link %d -> %d from tree 1\n", NR(pos), NR(newpos)); + + dir->used = 0; + w->flags1[NR(newpos)] &= ACTIVE; + bool_op(w, w->flags1, newpos, ~IN_TREE, 0); + + /* try to reconnect the path to some other tree part */ + if(reconnect(w, w->flags1, newpos, 0)) { + bool_op(w, w->flags1, newpos, ~0, IN_TREE); + } else { + destroy_subtree(w, w->flags1, newpos, q1); + break; + } + } + } + + for(t=path->length-1;t>0 && !path->firsthalf[t-1];t--) { + node_t*pos = path->pos[t]; + node_t*newpos = path->pos[t-1]; + halfedge_t*dir = path->dir[t-1]->fwd; + node_t*newpos2 = dir->fwd->node; + assert(newpos == newpos2); + if(!dir->fwd->weight) { + /* disconnect node */ + DBG printf("remove link %d->%d from tree 2\n", NR(pos), NR(newpos)); + + dir->used = 0; + w->flags2[NR(newpos)] &= ACTIVE; + bool_op(w, w->flags2, newpos, ~IN_TREE, 0); + + /* try to reconnect the path to some other tree part */ + if(reconnect(w, w->flags2, newpos, 1)) { + bool_op(w, w->flags2, newpos, ~0, IN_TREE); + } else { + destroy_subtree(w, w->flags2, newpos, q2); + break; + } + } + } +} + +static void check_graph(graph_t*g) +{ + int t; + for(t=0;tnum_nodes;t++) { + assert(g->nodes[t].nr==t); + halfedge_t*e = g->nodes[t].edges; + while(e) { + assert(!e->used || !e->fwd->used); + e = e->next; + } + } +} + +weight_t graph_maxflow(graph_t*graph, node_t*pos1, node_t*pos2) +{ + int max_flow = 0; + graphcut_workspace_t* w = graphcut_workspace_new(graph, pos1, pos2); + + check_graph(graph); + + posqueue_addpos(w->queue1, pos1); w->flags1[pos1->nr] |= ACTIVE|IN_TREE; + posqueue_addpos(w->queue2, pos2); w->flags2[pos2->nr] |= ACTIVE|IN_TREE; + DBG workspace_print(w); + + while(1) { + path_t*path; + while(1) { + char done1=0,done2=0; + node_t* p1 = posqueue_extract(w->queue1); + if(!p1) { + graphcut_workspace_delete(w); + return max_flow; + } + DBG printf("extend 1 from %d (%d edges)\n", NR(p1), node_count_edges(p1)); + path = expand_pos(w, w->queue1, p1, 0, w->flags1, w->flags2); + if(path) + break; + DBG workspace_print(w); + +#ifdef TWOTREES + node_t* p2 = posqueue_extract(w->queue2); + if(!p2) { + graphcut_workspace_delete(w); + return max_flow; + } + DBG printf("extend 2 from %d (%d edges)\n", NR(p2), node_count_edges(p2)); + path = expand_pos(w, w->queue2, p2, 1, w->flags2, w->flags1); + if(path) + break; + DBG workspace_print(w); +#endif + + } + DBG printf("found connection between tree1 and tree2\n"); + DBG path_print(path); + + DBG printf("decreasing weights\n"); + max_flow += decrease_weights(graph, path); + DBG workspace_print(w); + + DBG printf("destroying trees\n"); + combust_tree(w, w->queue1, w->queue2, path); + DBG workspace_print(w); + + DBG check_graph(w->graph); + + path_delete(path); + } + graphcut_workspace_delete(w); + return max_flow; +} + +halfedge_t*edge_new(node_t*from, node_t*to, weight_t forward_weight, weight_t backward_weight) +{ + halfedge_t*e1 = (halfedge_t*)rfx_calloc(sizeof(halfedge_t)); + halfedge_t*e2 = (halfedge_t*)rfx_calloc(sizeof(halfedge_t)); + e1->fwd = e2; + e2->fwd = e1; + e1->node = from; + e2->node = to; + e1->weight = forward_weight; + e2->weight = backward_weight; + + e1->next = from->edges; + from->edges = e1; + e2->next = to->edges; + to->edges = e2; + return e1; +} + +#ifdef MAIN +int main() +{ + int t; + int s; + for(s=0;s<10;s++) { + int width = (lrand48()%8)+1; + graph_t*g = graph_new(width*width); + for(t=0;t0) edge_new(&g->nodes[t], &g->nodes[t-1], R, R); + if(xnodes[t], &g->nodes[t+1], R, R); + if(y>0) edge_new(&g->nodes[t], &g->nodes[t-width], R, R); + if(ynodes[t], &g->nodes[t+width], R, R); + } + + int x = graph_maxflow(g, &g->nodes[0], &g->nodes[width*width-1]); + printf("max flow: %d\n", x); + graph_delete(g); + } +} +#endif diff --git a/lib/graphcut.h b/lib/graphcut.h new file mode 100644 index 0000000..ca6d82a --- /dev/null +++ b/lib/graphcut.h @@ -0,0 +1,56 @@ +/* + graphcut- a graphcut implementation based on the Boykov Kolmogorov algorithm + + Part of the swftools package. + + Copyright (c) 2007,2008,2009 Matthias Kramm + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#ifndef __graphcut_h__ +#define __graphcut_h__ + +#include "image.h" + +typedef signed int weight_t; +#define MAX_WEIGHT 0x07ffffff + +typedef struct _halfedge halfedge_t; +typedef struct _node node_t; +typedef struct _graph graph_t; + +struct _halfedge { + node_t*node; + struct _halfedge*fwd; + weight_t weight; + char used; + halfedge_t*next; +}; + +struct _node { + halfedge_t*edges; + int nr; +}; + +struct _graph { + node_t* nodes; + int num_nodes; +}; + +graph_t* graph_new(int num_nodes); +halfedge_t*edge_new(node_t*from, node_t*to, weight_t forward_weight, weight_t backward_weight); +weight_t graph_maxflow(graph_t*graph, node_t*pos1, node_t*pos2); +void graph_delete(graph_t*); + +#endif -- 1.7.10.4