mtp_graph.cc

   1
   2 ///////////////////////////////////////////////////////////////////////////
   3 // This program is free software: you can redistribute it and/or modify  //
   4 // it under the terms of the version 3 of the GNU General Public License //
   5 // as published by the Free Software Foundation.                         //
   6 //                                                                       //
   7 // This program is distributed in the hope that it will be useful, but   //
   8 // WITHOUT ANY WARRANTY; without even the implied warranty of            //
   9 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      //
  10 // General Public License for more details.                              //
  11 //                                                                       //
  12 // You should have received a copy of the GNU General Public License     //
  13 // along with this program. If not, see <http://www.gnu.org/licenses/>.  //
  14 //                                                                       //
  15 // Written by and Copyright (C) Francois Fleuret                         //
  16 // Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
  17 ///////////////////////////////////////////////////////////////////////////
  18
  19 #include "mtp_graph.h"
  20
  21 #include <iostream>
  22 #include <float.h>
  23
  24 using namespace std;
  25
  26 class Edge {
  27 public:
  28   int id, occupied;
  29   scalar_t length, work_length;
  30   Vertex *origin_vertex, *terminal_vertex;
  31
  32   // These are the links in the origin_vertex leaving edge list
  33   Edge *next, *pred;
  34
  35   inline void revert();
  36 };
  37
  38 class Vertex {
  39 public:
  40   int id, iteration;
  41   Edge *root_edge;
  42   scalar_t distance_from_source;
  43   Edge *best_pred_edge_to_source;
  44
  45   Vertex();
  46   inline void add_edge(Edge *e);
  47   inline void del_edge(Edge *e);
  48 };
  49
  50 //////////////////////////////////////////////////////////////////////
  51
  52 void Edge::revert() {
  53   length = - length;
  54   work_length = 0;
  55   origin_vertex->del_edge(this);
  56   terminal_vertex->add_edge(this);
  57   Vertex *t = terminal_vertex;
  58   terminal_vertex = origin_vertex;
  59   origin_vertex = t;
  60 }
  61
  62 //////////////////////////////////////////////////////////////////////
  63
  64 Vertex::Vertex() {
  65   root_edge = 0;
  66 }
  67
  68 void Vertex::add_edge(Edge *e) {
  69   e->next = root_edge;
  70   e->pred = 0;
  71   if(root_edge) { root_edge->pred = e; }
  72   root_edge = e;
  73 }
  74
  75 void Vertex::del_edge(Edge *e) {
  76   if(e == root_edge) { root_edge = e->next; }
  77   if(e->pred) { e->pred->next = e->next; }
  78   if(e->next) { e->next->pred = e->pred; }
  79 }
  80
  81 //////////////////////////////////////////////////////////////////////
  82
  83 void MTPGraph::print() {
  84   for(int k = 0; k < _nb_edges; k++) {
  85     Edge *e = _edges + k;
  86     cout << e->origin_vertex->id
  87          << " -> "
  88          << e->terminal_vertex->id
  89          << " "
  90          << e->length;
  91     if(e->occupied) {
  92       cout << " *";
  93     }
  94     cout << endl;
  95   }
  96 }
  97
  98 void MTPGraph::print_dot() {
  99   cout << "digraph {" << endl;
 100   cout << "  node[shape=circle];" << endl;
 101   for(int k = 0; k < _nb_edges; k++) {
 102     Edge *e = _edges + k;
 103     if(e->occupied) {
 104       cout << "  " << e->origin_vertex->id << " -> " << e->terminal_vertex->id
 105            << " [style=bold,color=black,label=\"" << -e->length << "\"];" << endl;
 106     } else {
 107       cout << "  " << e->origin_vertex->id << " -> " << e->terminal_vertex->id
 108            << " [color=gray,label=\"" << e->length << "\"];" << endl;
 109     }
 110   }
 111   cout << "}" << endl;
 112 }
 113
 114 MTPGraph::MTPGraph(int nb_vertices, int nb_edges,
 115                    int *from, int *to,
 116                    int source, int sink) {
 117   _nb_vertices = nb_vertices;
 118   _nb_edges = nb_edges;
 119
 120   _edges = new Edge[_nb_edges];
 121   _vertices = new Vertex[_nb_vertices];
 122   _front = new Vertex *[_nb_vertices];
 123   _new_front = new Vertex *[_nb_vertices];
 124
 125   _source = &_vertices[source];
 126   _sink = &_vertices[sink];
 127
 128   for(int v = 0; v < _nb_vertices; v++) {
 129     _vertices[v].id = v;
 130   }
 131
 132   for(int e = 0; e < nb_edges; e++) {
 133     _vertices[from[e]].add_edge(_edges + e);
 134     _edges[e].occupied = 0;
 135     _edges[e].id = e;
 136     _edges[e].origin_vertex = _vertices + from[e];
 137     _edges[e].terminal_vertex = _vertices + to[e];
 138   }
 139
 140 }
 141
 142 MTPGraph::~MTPGraph() {
 143   delete[] _vertices;
 144   delete[] _edges;
 145   delete[] _front;
 146   delete[] _new_front;
 147 }
 148
 149 void MTPGraph::initialize_work_lengths_with_min() {
 150   scalar_t length_min = 0;
 151   for(int n = 0; n < _nb_vertices; n++) {
 152     for(Edge *e = _vertices[n].root_edge; e; e = e->next) {
 153       length_min = min(e->length, length_min);
 154     }
 155   }
 156   for(int n = 0; n < _nb_vertices; n++) {
 157     for(Edge *e = _vertices[n].root_edge; e; e = e->next) {
 158       e->work_length = e->length - length_min;
 159     }
 160   }
 161 }
 162
 163 void MTPGraph::update_work_lengths() {
 164   for(int k = 0; k < _nb_edges; k++) {
 165     Edge *e = _edges + k;
 166     e->work_length += e->terminal_vertex->distance_from_source - e->terminal_vertex->distance_from_source;
 167   }
 168 }
 169
 170 void MTPGraph::force_positive_work_lengths() {
 171 #ifdef VERBOSE
 172   scalar_t residual_error = 0.0;
 173 #endif
 174   for(int n = 0; n < _nb_vertices; n++) {
 175     for(Edge *e = _vertices[n].root_edge; e; e = e->next) {
 176       if(e->work_length < 0) {
 177 #ifdef VERBOSE
 178         residual_error -= e->work_length;
 179 #endif
 180         e->work_length = 0.0;
 181       }
 182     }
 183   }
 184 #ifdef VERBOSE
 185   cerr << "residual_error " << residual_error << endl;
 186 #endif
 187 }
 188
 189 // This method does not change the edge occupation. It update
 190 // distance_from_source and best_pred_edge_to_source.
 191 void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) {
 192   Vertex **tmp_front;
 193   int tmp_front_size;
 194   Vertex *v, *tv;
 195   scalar_t d;
 196
 197   for(int v = 0; v < _nb_vertices; v++) {
 198     _vertices[v].distance_from_source = FLT_MAX;
 199     _vertices[v].best_pred_edge_to_source = 0;
 200     _vertices[v].iteration = 0;
 201   }
 202
 203   int iteration = 0;
 204
 205   int _front_size = 0, _new_front_size;
 206   _front[_front_size++] = _source;
 207   _source->distance_from_source = 0;
 208
 209   do {
 210     _new_front_size = 0;
 211     iteration++;
 212     for(int f = 0; f < _front_size; f++) {
 213       v = _front[f];
 214       for(Edge *e = v->root_edge; e; e = e->next) {
 215         d = v->distance_from_source + e->work_length;
 216         tv = e->terminal_vertex;
 217         if(d < tv->distance_from_source) {
 218           tv->distance_from_source = d;
 219           tv->best_pred_edge_to_source = e;
 220           if(tv->iteration < iteration) {
 221             _new_front[_new_front_size++] = tv;
 222             tv->iteration = iteration;
 223           }
 224         }
 225       }
 226     }
 227
 228     tmp_front = _new_front;
 229     _new_front = _front;
 230     _front = tmp_front;
 231
 232     tmp_front_size = _new_front_size;
 233     _new_front_size = _front_size;
 234     _front_size = tmp_front_size;
 235   } while(_front_size > 0);
 236 }
 237
 238 void MTPGraph::find_best_paths(scalar_t *lengths, int *result_edge_occupation) {
 239   scalar_t total_length;
 240   Vertex *v;
 241   Edge *e;
 242
 243   for(int e = 0; e < _nb_edges; e++) {
 244     _edges[e].length = lengths[e];
 245     _edges[e].work_length = _edges[e].length;
 246   }
 247
 248   // We use one iteration of find_shortest_path simply to propagate
 249   // the distance to make all the edge lengths positive.
 250   find_shortest_path(_front, _new_front);
 251   update_work_lengths();
 252
 253   // #warning
 254   // initialize_work_lengths_with_min();
 255
 256   do {
 257     force_positive_work_lengths();
 258     find_shortest_path(_front, _new_front);
 259     update_work_lengths();
 260
 261     total_length = 0.0;
 262
 263     // Do we reach the _sink?
 264     if(_sink->best_pred_edge_to_source) {
 265       // If yes, compute the length of the best path
 266       v = _sink;
 267       while(v->best_pred_edge_to_source) {
 268         total_length += v->best_pred_edge_to_source->length;
 269         v = v->best_pred_edge_to_source->origin_vertex;
 270       }
 271       // If that length is negative
 272       if(total_length < 0.0) {
 273 #ifdef VERBOSE
 274         cout << "Found a path of length " << total_length << endl;
 275 #endif
 276         // Invert all the edges along the best path
 277         v = _sink;
 278         while(v->best_pred_edge_to_source) {
 279           e = v->best_pred_edge_to_source;
 280           v = e->origin_vertex;
 281           e->revert();
 282           e->occupied = 1 - e->occupied;
 283         }
 284       }
 285     }
 286
 287   } while(total_length < 0.0);
 288
 289   for(int k = 0; k < _nb_edges; k++) {
 290     Edge *e = _edges + k;
 291     if(e->occupied) { e->revert(); }
 292     result_edge_occupation[k] = e->occupied;
 293   }
 294 }