From: Francois Fleuret <francois@fleuret.org>
Date: Wed, 22 Aug 2012 16:54:28 +0000 (-0700)
Subject: Update.
X-Git-Url: https://fleuret.org/cgi-bin/gitweb/gitweb.cgi?p=mtp.git;a=commitdiff_plain;h=b33d04fc473e7de0f9d404b014fb694b7ee6c661

Update.
---

diff --git a/misc.h b/misc.h
new file mode 100644
index 0000000..f9bc8e5
--- /dev/null
+++ b/misc.h
@@ -0,0 +1,46 @@
+
+////////////////////////////////////////////////////////////////////
+// START_IP_HEADER                                                //
+//                                                                //
+// Written by Francois Fleuret                                    //
+// Contact <francois.fleuret@idiap.ch> for comments & bug reports //
+//                                                                //
+// END_IP_HEADER                                                  //
+////////////////////////////////////////////////////////////////////
+
+#ifndef MISC_H
+#define MISC_H
+
+#define VERBOSE
+
+typedef float scalar_t;
+
+#ifdef DEBUG
+#define ASSERT(x) if(!(x)) {                                            \
+    std::cerr << "ASSERT FAILED IN " << __FILE__ << ":" << __LINE__ << endl; \
+    abort();                                                            \
+  }
+#else
+#define ASSERT(x)
+#endif
+
+template<class T>
+T **allocate_array(int a, int b) {
+  T *whole = new T[a * b];
+  T **array = new T *[a];
+  for(int k = 0; k < a; k++) {
+    array[k] = whole;
+    whole += b;
+  }
+  return array;
+}
+
+template<class T>
+void deallocate_array(T **array) {
+  if(array) {
+    delete[] array[0];
+    delete[] array;
+  }
+}
+
+#endif
diff --git a/mtp.cc b/mtp.cc
index 044fefa..ce9e56b 100644
--- a/mtp.cc
+++ b/mtp.cc
@@ -22,8 +22,6 @@
 
 // EXAMPLE: ./mtp ./graph2.txt  | dot -T pdf -o- | xpdf -
 
-#define VERBOSE
-
 #include <iostream>
 #include <fstream>
 #include <cmath>
@@ -49,32 +47,32 @@ void find_best_paths(int nb_vertices,
 //////////////////////////////////////////////////////////////////////
 
 int main(int argc, char **argv) {
-  int nb_locations = 6;
-  int nb_time_steps = 5;
-
-  {
-    Tracker tracker(nb_time_steps, nb_locations);
-
-    for(int l = 0; l < nb_locations; l++) {
-      for(int k = 0; k < nb_locations; k++) {
-        tracker.set_allowed_motion(l, k, abs(l - k) <= 1);
-      }
-    }
-
-    for(int t = 0; t < nb_time_steps; t++) {
-      for(int l = 0; l < nb_locations; l++) {
-        tracker.set_detection_score(t, l,
-                                    (drand48() < 0.9 ? -1.0 : 1.0) + drand48() * 0.1 - 0.05);
-      }
-      tracker.set_detection_score(t, 0,
-                                  (drand48() < 0.9 ? 1.0 : -1.0) + drand48() * 0.1 - 0.05);
-    }
-
-    tracker.build_graph();
-    tracker.track();
-  }
-
-  exit(0);
+  // int nb_locations = 6;
+  // int nb_time_steps = 5;
+
+  // {
+    // Tracker tracker(nb_time_steps, nb_locations);
+
+    // for(int l = 0; l < nb_locations; l++) {
+      // for(int k = 0; k < nb_locations; k++) {
+        // tracker.set_allowed_motion(l, k, abs(l - k) <= 1);
+      // }
+    // }
+
+    // for(int t = 0; t < nb_time_steps; t++) {
+      // for(int l = 0; l < nb_locations; l++) {
+        // tracker.set_detection_score(t, l,
+                                    // (drand48() < 0.9 ? -1.0 : 1.0) + drand48() * 0.1 - 0.05);
+      // }
+      // tracker.set_detection_score(t, 0,
+                                  // (drand48() < 0.9 ? 1.0 : -1.0) + drand48() * 0.1 - 0.05);
+    // }
+
+    // tracker.build_graph();
+    // tracker.track();
+  // }
+
+  // exit(0);
 
   if(argc < 2) {
     cerr << argv[0] << " <graph file>" << endl;
diff --git a/mtp_graph.cc b/mtp_graph.cc
index c7e6b98..04b84aa 100644
--- a/mtp_graph.cc
+++ b/mtp_graph.cc
@@ -141,12 +141,7 @@ void MTPGraph::update_work_lengths() {
   }
 }
 
-void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) {
-  Vertex **tmp_front;
-  int tmp_front_size;
-  Vertex *v, *tv;
-  scalar_t d;
-
+void MTPGraph::force_positive_work_lengths() {
 #ifdef VERBOSE
   scalar_t residual_error = 0.0;
 #endif
@@ -163,6 +158,13 @@ void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) {
 #ifdef VERBOSE
   cerr << "residual_error " << residual_error << endl;
 #endif
+}
+
+void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) {
+  Vertex **tmp_front;
+  int tmp_front_size;
+  Vertex *v, *tv;
+  scalar_t d;
 
   for(int v = 0; v < _nb_vertices; v++) {
     vertices[v].distance_from_source = FLT_MAX;
@@ -212,15 +214,21 @@ void MTPGraph::find_best_paths(scalar_t *lengths, int *result_edge_occupation) {
 
   for(int e = 0; e < _nb_edges; e++) {
     edges[e].length = lengths[e];
+    edges[e].work_length = edges[e].length;
   }
 
-  initialize_work_lengths();
+  find_shortest_path(_front, _new_front);
+  update_work_lengths();
+
+  // initialize_work_lengths();
 
   do {
-    total_length = 0.0;
+    force_positive_work_lengths();
     find_shortest_path(_front, _new_front);
     update_work_lengths();
 
+    total_length = 0.0;
+
     // Do we reach the _sink?
     if(_sink->pred_edge) {
 
@@ -243,8 +251,21 @@ void MTPGraph::find_best_paths(scalar_t *lengths, int *result_edge_occupation) {
         }
       }
     }
+
   } while(total_length < 0.0);
 
+  for(Edge *e = _sink->root_edge; e; e = e->next) {
+    if(e->occupied) {
+      Edge *f = e;
+      cout << "PATH " << _sink->id;
+      while(f) {
+        cout << " " << f->terminal_vertex->id;
+        for(f = f->terminal_vertex->root_edge; f && !f->occupied; f = f->next);
+      }
+      cout << endl;
+    }
+  }
+
   for(int n = 0; n < _nb_vertices; n++) {
     Vertex *v = &vertices[n];
     for(Edge *e = v->root_edge; e; e = e->next) {
diff --git a/mtp_graph.h b/mtp_graph.h
index 75e48ee..f12262e 100644
--- a/mtp_graph.h
+++ b/mtp_graph.h
@@ -27,6 +27,7 @@ class Edge;
 class MTPGraph {
   void initialize_work_lengths();
   void update_work_lengths();
+  void force_positive_work_lengths();
   void find_shortest_path(Vertex **front, Vertex **new_front);
 
   int _nb_vertices, _nb_edges;