mtp_graph.cc

   1
   2 /*
   3  *  mtp is the ``Multi Tracked Paths'', an implementation of the
   4  *  k-shortest paths algorithm for multi-target tracking.
   5  *
   6  *  Copyright (c) 2012 Idiap Research Institute, http://www.idiap.ch/
   7  *  Written by Francois Fleuret <francois.fleuret@idiap.ch>
   8  *
   9  *  This file is part of mtp.
  10  *
  11  *  mtp is free software: you can redistribute it and/or modify it
  12  *  under the terms of the GNU General Public License version 3 as
  13  *  published by the Free Software Foundation.
  14  *
  15  *  mtp is distributed in the hope that it will be useful, but WITHOUT
  16  *  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  17  *  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
  18  *  License for more details.
  19  *
  20  *  You should have received a copy of the GNU General Public License
  21  *  along with selector.  If not, see <http://www.gnu.org/licenses/>.
  22  *
  23  */
  24
  25 #include "mtp_graph.h"
  26
  27 #include <cmath>
  28 #include <float.h>
  29
  30 using namespace std;
  31
  32 class Edge {
  33 public:
  34   int occupied;
  35   scalar_t length, positivized_length;
  36   Vertex *origin_vertex, *terminal_vertex;
  37
  38   // These fields are used for the linked list of a vertex's leaving
  39   // edge list. We have to do insertions / deletions.
  40   Edge *next_leaving_edge, *pred_leaving_edge;
  41
  42   inline void invert();
  43 };
  44
  45 class Vertex {
  46 public:
  47   scalar_t distance_from_source;
  48   Edge *pred_edge_toward_source;
  49
  50   Edge *leaving_edge_list_root;
  51   Vertex **heap_slot;
  52
  53   Vertex();
  54
  55   inline void add_leaving_edge(Edge *e);
  56   inline void del_leaving_edge(Edge *e);
  57   inline void decrease_distance_in_heap(Vertex **heap);
  58   inline void increase_distance_in_heap(Vertex **heap, Vertex **heap_bottom);
  59 };
  60
  61 //////////////////////////////////////////////////////////////////////
  62
  63 void Edge::invert() {
  64   length = - length;
  65   positivized_length = - positivized_length;
  66   origin_vertex->del_leaving_edge(this);
  67   terminal_vertex->add_leaving_edge(this);
  68   swap(terminal_vertex, origin_vertex);
  69 }
  70
  71 //////////////////////////////////////////////////////////////////////
  72
  73 Vertex::Vertex() {
  74   leaving_edge_list_root = 0;
  75 }
  76
  77 void Vertex::add_leaving_edge(Edge *e) {
  78   e->next_leaving_edge = leaving_edge_list_root;
  79   e->pred_leaving_edge = 0;
  80   if(leaving_edge_list_root) {
  81     leaving_edge_list_root->pred_leaving_edge = e;
  82   }
  83   leaving_edge_list_root = e;
  84 }
  85
  86 void Vertex::del_leaving_edge(Edge *e) {
  87   if(e == leaving_edge_list_root) {
  88     leaving_edge_list_root = e->next_leaving_edge;
  89   }
  90   if(e->pred_leaving_edge) {
  91     e->pred_leaving_edge->next_leaving_edge = e->next_leaving_edge;
  92   }
  93   if(e->next_leaving_edge) {
  94     e->next_leaving_edge->pred_leaving_edge = e->pred_leaving_edge;
  95   }
  96 }
  97
  98 void Vertex::decrease_distance_in_heap(Vertex **heap) {
  99   Vertex **p, **h;
 100   // There is some beauty in that
 101   h = heap_slot;
 102   while(h > heap &&
 103         (p = heap + (h - heap + 1) / 2 - 1,
 104          (*p)->distance_from_source > (*h)->distance_from_source)) {
 105     swap(*p, *h);
 106     swap((*p)->heap_slot, (*h)->heap_slot);
 107     h = p;
 108   }
 109 }
 110
 111 void Vertex::increase_distance_in_heap(Vertex **heap, Vertex **heap_bottom) {
 112   Vertex **c1, **c2, **h;
 113   // omg, that's beautiful
 114   h = heap_slot;
 115   while(c1 = heap + 2 * (h - heap) + 1,
 116         c1 < heap_bottom &&
 117         (c2 = c1 + 1,
 118          (*c1)->distance_from_source < (*h)->distance_from_source
 119          ||
 120          (c2 < heap_bottom && (*c2)->distance_from_source < (*h)->distance_from_source)
 121          )) {
 122     if(c2 < heap_bottom && (*c2)->distance_from_source <= (*c1)->distance_from_source) {
 123       swap(*c2, *h);
 124       swap((*c2)->heap_slot, (*h)->heap_slot);
 125       h = c2;
 126     } else {
 127       swap(*c1, *h);
 128       swap((*c1)->heap_slot, (*h)->heap_slot);
 129       h = c1;
 130     }
 131   }
 132 }
 133
 134 //////////////////////////////////////////////////////////////////////
 135
 136 MTPGraph::MTPGraph(int nb_vertices, int nb_edges,
 137                    int *vertex_from, int *vertex_to,
 138                    int source, int sink) {
 139   _nb_vertices = nb_vertices;
 140   _nb_edges = nb_edges;
 141
 142   _edges = new Edge[_nb_edges];
 143   _vertices = new Vertex[_nb_vertices];
 144   _heap = new Vertex *[_nb_vertices];
 145   _dp_order = new Vertex *[_nb_vertices];
 146
 147   _source = &_vertices[source];
 148   _sink = &_vertices[sink];
 149
 150   for(int e = 0; e < nb_edges; e++) {
 151     _vertices[vertex_from[e]].add_leaving_edge(&_edges[e]);
 152     _edges[e].occupied = 0;
 153     _edges[e].origin_vertex = &_vertices[vertex_from[e]];
 154     _edges[e].terminal_vertex = &_vertices[vertex_to[e]];
 155   }
 156
 157   for(int v = 0; v < _nb_vertices; v++) {
 158     _heap[v] = &_vertices[v];
 159     _vertices[v].heap_slot = &_heap[v];
 160   }
 161
 162   paths = 0;
 163   nb_paths = 0;
 164
 165   compute_dp_ordering();
 166 }
 167
 168 MTPGraph::~MTPGraph() {
 169   delete[] _vertices;
 170   delete[] _dp_order;
 171   delete[] _heap;
 172   delete[] _edges;
 173   for(int p = 0; p < nb_paths; p++) delete paths[p];
 174   delete[] paths;
 175 }
 176
 177 //////////////////////////////////////////////////////////////////////
 178
 179 void MTPGraph::print(ostream *os) {
 180   for(int k = 0; k < _nb_edges; k++) {
 181     Edge *e = &_edges[k];
 182     (*os) << e->origin_vertex - _vertices
 183           << " -> "
 184           << e->terminal_vertex - _vertices
 185           << " (" << e->length << ")";
 186     if(e->occupied) { (*os) << " *"; }
 187     (*os) << endl;
 188   }
 189 }
 190
 191 void MTPGraph::print_dot(ostream *os) {
 192   (*os) << "digraph {" << endl;
 193   (*os) << "        rankdir=\"LR\";" << endl;
 194   (*os) << "        node [shape=circle,width=0.75,fixedsize=true];" << endl;
 195   (*os) << "        edge [color=gray,arrowhead=open]" << endl;
 196   (*os) << "        " << _source - _vertices << " [peripheries=2];" << endl;
 197   (*os) << "        " << _sink - _vertices << " [peripheries=2];" << endl;
 198   for(int k = 0; k < _nb_edges; k++) {
 199     Edge *e = &_edges[k];
 200     (*os) << "        "
 201           << e->origin_vertex - _vertices
 202           << " -> "
 203           << e->terminal_vertex - _vertices
 204           << " [";
 205     if(e->occupied) {
 206       (*os) << "style=bold,color=black,";
 207     }
 208     (*os) << "label=\"" << e->length << "\"];" << endl;
 209   }
 210   (*os) << "}" << endl;
 211 }
 212
 213 //////////////////////////////////////////////////////////////////////
 214
 215 void MTPGraph::update_positivized_lengths() {
 216   for(int k = 0; k < _nb_edges; k++) {
 217     Edge *e = &_edges[k];
 218     e->positivized_length +=
 219       e->origin_vertex->distance_from_source - e->terminal_vertex->distance_from_source;
 220   }
 221 }
 222
 223 void MTPGraph::force_positivized_lengths() {
 224 #ifdef VERBOSE
 225   scalar_t residual_error = 0.0;
 226   scalar_t max_error = 0.0;
 227 #endif
 228   for(int k = 0; k < _nb_edges; k++) {
 229     Edge *e = &_edges[k];
 230
 231     if(e->positivized_length < 0) {
 232 #ifdef VERBOSE
 233       residual_error -= e->positivized_length;
 234       max_error = max(max_error, - e->positivized_length);
 235 #endif
 236       e->positivized_length = 0.0;
 237     }
 238   }
 239 #ifdef VERBOSE
 240   cerr << __FILE__ << ": residual_error " << residual_error << " max_error " << residual_error << endl;
 241 #endif
 242 }
 243
 244 void MTPGraph::dp_compute_distances() {
 245   Vertex *v, *tv;
 246   Edge *e;
 247   scalar_t d;
 248
 249   for(int k = 0; k < _nb_vertices; k++) {
 250     _vertices[k].distance_from_source = FLT_MAX;
 251     _vertices[k].pred_edge_toward_source = 0;
 252   }
 253
 254   _source->distance_from_source = 0;
 255
 256   for(int k = 0; k < _nb_vertices; k++) {
 257     v = _dp_order[k];
 258     for(e = v->leaving_edge_list_root; e; e = e->next_leaving_edge) {
 259       d = v->distance_from_source + e->positivized_length;
 260       tv = e->terminal_vertex;
 261       if(d < tv->distance_from_source) {
 262         tv->distance_from_source = d;
 263         tv->pred_edge_toward_source = e;
 264       }
 265     }
 266   }
 267 }
 268
 269 // This method does not change the edge occupation. It only sets
 270 // properly, for every vertex, the fields distance_from_source and
 271 // pred_edge_toward_source.
 272
 273 void MTPGraph::find_shortest_path() {
 274   Vertex *v, *tv, **last_slot;
 275   Edge *e;
 276   scalar_t d;
 277
 278   for(int k = 0; k < _nb_vertices; k++) {
 279     _vertices[k].distance_from_source = FLT_MAX;
 280     _vertices[k].pred_edge_toward_source = 0;
 281   }
 282
 283   _heap_size = _nb_vertices;
 284   _source->distance_from_source = 0;
 285   _source->decrease_distance_in_heap(_heap);
 286
 287   do {
 288     // Get the closest to the source
 289     v = _heap[0];
 290
 291     // Remove it from the heap (swap it with the last_slot in the heap, and
 292     // update the distance of that one)
 293     _heap_size--;
 294     last_slot = _heap + _heap_size;
 295     swap(*_heap, *last_slot); swap((*_heap)->heap_slot, (*last_slot)->heap_slot);
 296     _heap[0]->increase_distance_in_heap(_heap, _heap + _heap_size);
 297
 298     // Now update the neighbors of the node currently closest to the
 299     // source
 300     for(e = v->leaving_edge_list_root; e; e = e->next_leaving_edge) {
 301       d = v->distance_from_source + e->positivized_length;
 302       tv = e->terminal_vertex;
 303       if(d < tv->distance_from_source) {
 304         ASSERT(tv->heap_slot - _heap < _heap_size);
 305         tv->distance_from_source = d;
 306         tv->pred_edge_toward_source = e;
 307         tv->decrease_distance_in_heap(_heap);
 308       }
 309     }
 310   } while(_heap_size > 0);
 311 }
 312
 313 void MTPGraph::find_best_paths(scalar_t *lengths) {
 314   scalar_t shortest_path_length;
 315   Vertex *v;
 316   Edge *e;
 317
 318   for(int e = 0; e < _nb_edges; e++) {
 319     _edges[e].length = lengths[e];
 320     _edges[e].occupied = 0;
 321     _edges[e].positivized_length = _edges[e].length;
 322   }
 323
 324   // Compute the distance of all the nodes from the source by just
 325   // visiting them in the proper DAG ordering we computed when
 326   // building the graph
 327   dp_compute_distances();
 328
 329   do {
 330     // Use the current distance from the source to make all edge
 331     // lengths positive
 332     update_positivized_lengths();
 333     // Fix numerical errors
 334     force_positivized_lengths();
 335
 336     find_shortest_path();
 337
 338     shortest_path_length = 0.0;
 339
 340     // Do we reach the sink?
 341     if(_sink->pred_edge_toward_source) {
 342       // If yes, compute the length of the best path according to the
 343       // original edge lengths
 344       v = _sink;
 345       while(v->pred_edge_toward_source) {
 346         shortest_path_length += v->pred_edge_toward_source->length;
 347         v = v->pred_edge_toward_source->origin_vertex;
 348       }
 349       // If that length is negative
 350       if(shortest_path_length < 0.0) {
 351 #ifdef VERBOSE
 352         cerr << __FILE__ << ": Found a path of length " << shortest_path_length << endl;
 353 #endif
 354         // Invert all the edges along the best path
 355         v = _sink;
 356         while(v->pred_edge_toward_source) {
 357           e = v->pred_edge_toward_source;
 358           v = e->origin_vertex;
 359           e->invert();
 360           // This is the only place where we change the occupations of
 361           // edges
 362           e->occupied = 1 - e->occupied;
 363         }
 364       }
 365     }
 366
 367   } while(shortest_path_length < 0.0);
 368
 369   // Put back the graph in its original state (i.e. invert edges which
 370   // have been inverted in the process)
 371   for(int k = 0; k < _nb_edges; k++) {
 372     e = &_edges[k];
 373     if(e->occupied) { e->invert(); }
 374   }
 375 }
 376
 377 int MTPGraph::retrieve_one_path(Edge *e, Path *path) {
 378   Edge *f, *next = 0;
 379   int l = 0, nb_occupied_next;
 380
 381   if(path) {
 382     path->nodes[l++] = int(e->origin_vertex - _vertices);
 383     path->length = e->length;
 384   } else l++;
 385
 386   while(e->terminal_vertex != _sink) {
 387     if(path) {
 388       path->nodes[l++] = int(e->terminal_vertex - _vertices);
 389       path->length += e->length;
 390     } else l++;
 391
 392     nb_occupied_next = 0;
 393     for(f = e->terminal_vertex->leaving_edge_list_root; f; f = f->next_leaving_edge) {
 394       if(f->occupied) { nb_occupied_next++; next = f; }
 395     }
 396
 397 #ifdef DEBUG
 398     if(nb_occupied_next == 0) {
 399       cerr << __FILE__ << ": retrieve_one_path: Non-sink end point." << endl;
 400       abort();
 401     }
 402
 403     else if(nb_occupied_next > 1) {
 404       cerr << __FILE__ << ": retrieve_one_path: Non node-disjoint paths." << endl;
 405       abort();
 406     }
 407 #endif
 408
 409     e = next;
 410   }
 411
 412   if(path) {
 413     path->nodes[l++] = int(e->terminal_vertex - _vertices);
 414     path->length += e->length;
 415   } else l++;
 416
 417   return l;
 418 }
 419
 420 //////////////////////////////////////////////////////////////////////
 421
 422 static int compare_vertices_on_distance(const void *v1, const void *v2) {
 423   scalar_t delta =
 424     (*((Vertex **) v1))->distance_from_source -
 425     (*((Vertex **) v2))->distance_from_source;
 426   if(delta < 0) return -1;
 427   else if(delta > 0) return 1;
 428   else return 0;
 429 }
 430
 431 void MTPGraph::compute_dp_ordering() {
 432   Vertex *v;
 433   Edge *e;
 434   int ntv;
 435
 436   // This method computes for each node the length of the longest link
 437   // from the source, and orders the node in _dp_order according to
 438   // it. It aborts if the graph is not a DAG.
 439
 440   int *nb_predecessors = new int[_nb_vertices];
 441
 442   Vertex **already_processed = _dp_order, **front = _dp_order, **new_front = _dp_order;
 443
 444   for(int k = 0; k < _nb_vertices; k++) {
 445     nb_predecessors[k] = 0;
 446   }
 447
 448   for(int k = 0; k < _nb_vertices; k++) {
 449     v = &_vertices[k];
 450     for(e = v->leaving_edge_list_root; e; e = e->next_leaving_edge) {
 451       ntv = int(e->terminal_vertex - _vertices);
 452       nb_predecessors[ntv]++;
 453     }
 454   }
 455
 456   for(int k = 0; k < _nb_vertices; k++) {
 457     if(nb_predecessors[k] == 0) {
 458       *(front++) = _vertices + k;
 459     }
 460   }
 461
 462   scalar_t rank = 1;
 463   while(already_processed < front) {
 464     new_front = front;
 465     while(already_processed < front) {
 466       v = *(already_processed++);
 467       v->distance_from_source = rank;
 468       for(e = v->leaving_edge_list_root; e; e = e->next_leaving_edge) {
 469         ntv = int(e->terminal_vertex - _vertices);
 470         nb_predecessors[ntv]--;
 471         ASSERT(nb_predecessors[ntv] >= 0);
 472         if(nb_predecessors[ntv] == 0) {
 473           *(new_front++) = e->terminal_vertex;
 474         }
 475       }
 476     }
 477     front = new_front;
 478     rank++;
 479   }
 480
 481   if(already_processed < _dp_order + _nb_vertices) {
 482     cerr << __FILE__ << ": The graph is not a DAG." << endl;
 483     abort();
 484   }
 485
 486   delete[] nb_predecessors;
 487
 488   for(int v = 0; v < _nb_vertices; v++) { _dp_order[v] = &_vertices[v]; }
 489   qsort(_dp_order, _nb_vertices, sizeof(Vertex *), compare_vertices_on_distance);
 490 }
 491
 492 //////////////////////////////////////////////////////////////////////
 493
 494 void MTPGraph::retrieve_disjoint_paths() {
 495   Edge *e;
 496   int p, l;
 497
 498   for(int p = 0; p < nb_paths; p++) delete paths[p];
 499   delete[] paths;
 500
 501   nb_paths = 0;
 502   for(e = _source->leaving_edge_list_root; e; e = e->next_leaving_edge) {
 503     if(e->occupied) { nb_paths++; }
 504   }
 505
 506   paths = new Path *[nb_paths];
 507
 508   p = 0;
 509   for(e = _source->leaving_edge_list_root; e; e = e->next_leaving_edge) {
 510     if(e->occupied) {
 511       l = retrieve_one_path(e, 0);
 512       paths[p] = new Path(l);
 513       retrieve_one_path(e, paths[p]);
 514       p++;
 515     }
 516   }
 517 }