Debugged a stupid bug.

diff --git a/mtp.cc b/mtp.cc
index e096836..35b431b 100644 (file)
--- a/mtp.cc
+++ b/mtp.cc
@@ -23,6 +23,7 @@
 // EXAMPLE: ./mtp ./graph2.txt  | dot -T pdf -o- | xpdf -
 
 #include <iostream>
+#include <fstream>
 #include <stdlib.h>
 
 using namespace std;
@@ -32,8 +33,8 @@ using namespace std;
 //////////////////////////////////////////////////////////////////////
 
 int main(int argc, char **argv) {
-  int nb_locations = 20;
-  int nb_time_steps = 50;
+  int nb_locations = 5;
+  int nb_time_steps = 20;
   int motion_amplitude = 2;
 
   Tracker *tracker = new Tracker(nb_time_steps, nb_locations);
@@ -42,18 +43,25 @@ int main(int argc, char **argv) {
     for(int k = 0; k < nb_locations; k++) {
       tracker->set_allowed_motion(l, k, abs(l - k) <= motion_amplitude);
     }
+    tracker->set_as_exit(0, 1);
+    tracker->set_as_entrance(0, 1);
   }
 
   tracker->build_graph();
+  // {
+    // ofstream out("graph.dot");
+    // tracker->print_dot_graph(&out);
+  // }
 
   for(int r = 0; r < 10; r++) {
     cout << "* ROUND " << r << endl;
+
     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.95 ? -1.0 : 1.0) + drand48() * 0.1 - 0.05);
       }
-      tracker->set_detection_score(t, 0,
+      tracker->set_detection_score(t, nb_locations/2,
                                   (drand48() < 0.95 ? 1.0 : -1.0) + drand48() * 0.1 - 0.05);
     }
 

diff --git a/mtp_graph.cc b/mtp_graph.cc
index c213a07..cc816c9 100644 (file)
--- a/mtp_graph.cc
+++ b/mtp_graph.cc
@@ -155,6 +155,9 @@ void MTPGraph::print_dot(ostream *os) {
   (*os) << "  node[shape=circle];" << endl;
   for(int k = 0; k < _nb_edges; k++) {
     Edge *e = _edges + k;
+    // (*os) << "  " << e->origin_vertex->id << " -> " << e->terminal_vertex->id
+          // << ";"
+          // << endl;
     if(e->occupied) {
       (*os) << "  " << e->origin_vertex->id << " -> " << e->terminal_vertex->id
            << " [style=bold,color=black,label=\"" << e->length << "\"];" << endl;
@@ -185,26 +188,29 @@ void MTPGraph::initialize_positivized_lengths_with_min() {
 void MTPGraph::update_positivized_lengths() {
   for(int k = 0; k < _nb_edges; k++) {
     Edge *e = _edges + k;
-    e->positivized_length += e->terminal_vertex->distance_from_source - e->terminal_vertex->distance_from_source;
+    e->positivized_length +=
+      e->origin_vertex->distance_from_source - e->terminal_vertex->distance_from_source;
   }
 }
 
 void MTPGraph::force_positivized_lengths() {
 #ifdef VERBOSE
   scalar_t residual_error = 0.0;
+  scalar_t max_error = 0.0;
 #endif
   for(int n = 0; n < _nb_vertices; n++) {
     for(Edge *e = _vertices[n].leaving_edges; e; e = e->next_leaving_edge) {
       if(e->positivized_length < 0) {
 #ifdef VERBOSE
         residual_error -= e->positivized_length;
+        max_error = max(max_error, fabs(e->positivized_length));
 #endif
         e->positivized_length = 0.0;
       }
     }
   }
 #ifdef VERBOSE
-  cerr << "residual_error " << residual_error << endl;
+  cerr << "residual_error " << residual_error << " max_error " << residual_error << endl;
 #endif
 }
 
@@ -232,6 +238,32 @@ void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) {
   do {
     _new_front_size = 0;
     iteration++;
+
+    // for(int k = 0; k < _nb_edges; k++) {
+      // Edge *e = _edges + k;
+      // d = e->origin_vertex->distance_from_source + e->positivized_length;
+      // if(d < e->terminal_vertex->distance_from_source) {
+        // e->terminal_vertex->distance_from_source = d;
+        // _new_front_size++;
+      // }
+    // }
+
+    // for(int n = 0; n < _nb_vertices; n++) {
+      // v = &_vertices[n];
+      // for(e = v->leaving_edges; e; e = e->next_leaving_edge) {
+        // d = v->distance_from_source + e->positivized_length;
+        // tv = e->terminal_vertex;
+        // if(d < tv->distance_from_source) {
+          // tv->distance_from_source = d;
+          // tv->best_pred_edge_to_source = e;
+          // if(tv->iteration < iteration) {
+            // _new_front[_new_front_size++] = tv;
+            // tv->iteration = iteration;
+          // }
+        // }
+      // }
+    // }
+
     for(int f = 0; f < _front_size; f++) {
       v = _front[f];
       for(e = v->leaving_edges; e; e = e->next_leaving_edge) {
@@ -256,6 +288,19 @@ void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) {
     _new_front_size = _front_size;
     _front_size = tmp_front_size;
   } while(_front_size > 0);
+
+#ifdef DEBUG
+  scalar_t min_delta = 0, delta;
+  for(int k = 0; k < _nb_edges; k++) {
+    Edge *e = _edges + k;
+    // d = e->origin_vertex->distance_from_source + e->positivized_length;
+    // if(d > e->terminal_vertex->distance_from_source) { abort(); }
+    delta = e->positivized_length +
+               (e->origin_vertex->distance_from_source - e->terminal_vertex->distance_from_source);
+    min_delta = min(delta, min_delta);
+  }
+  cout << "min_delta = " << delta << endl;
+#endif
 }
 
 void MTPGraph::find_best_paths(scalar_t *lengths) {
@@ -269,6 +314,9 @@ void MTPGraph::find_best_paths(scalar_t *lengths) {
     _edges[e].positivized_length = _edges[e].length;
   }
 
+  cout << "********************************************************" << endl;
+  // print_dot(&cout);
+
   // We use one iteration of find_shortest_path simply to propagate
   // the distance to make all the edge lengths positive.
   find_shortest_path(_front, _new_front);
@@ -319,7 +367,7 @@ void MTPGraph::find_best_paths(scalar_t *lengths) {
 }
 
 int MTPGraph::retrieve_one_path(Edge *e, int *nodes) {
-  Edge *f, *next;
+  Edge *f, *next = 0;
   int l = 0;
 
   if(nodes) { nodes[l++] = e->origin_vertex->id; }

diff --git a/mtp_graph.h b/mtp_graph.h
index ddf7162..0fad09d 100644 (file)
--- a/mtp_graph.h
+++ b/mtp_graph.h
@@ -20,6 +20,7 @@
 #define MTP_GRAPH_H
 
 #include <iostream>
+#include <cmath>
 
 using namespace std;
 

diff --git a/tracker.cc b/tracker.cc
index 3a311cd..47460d3 100644 (file)
--- a/tracker.cc
+++ b/tracker.cc
@@ -86,7 +86,7 @@ void Tracker::build_graph() {
 
   int nb_edges =
     _nb_locations * 2 +
-    _nb_time_steps * (nb_exits + nb_entrances) +
+    (_nb_time_steps - 2) * (nb_exits + nb_entrances) +
     (_nb_time_steps - 1) * nb_motions +
     _nb_locations * _nb_time_steps;
 
@@ -137,7 +137,7 @@ void Tracker::build_graph() {
     }
   }
 
-  for(int t = 0; t < _nb_time_steps; t++) {
+  for(int t = 1; t < _nb_time_steps-1; t++) {
     for(int l = 0; l < _nb_locations; l++) {
       if(_entrances[l]) {
         node_from[e] = source;
@@ -147,7 +147,7 @@ void Tracker::build_graph() {
       }
       if(_exits[l]) {
         node_from[e] =   1 + (2 * (t + 0) + 1) * _nb_locations + l;
-        node_from[e] = sink;
+        node_to[e] = sink;
         _edge_lengths[e] = 0.0;
         e++;
       }
@@ -176,6 +176,15 @@ void Tracker::track() {
 
   _graph->find_best_paths(_edge_lengths);
   _graph->retrieve_disjoint_paths();
+
+  // for(int p = 0; p < _graph->nb_paths; p++) {
+    // Path *path = _graph->paths[p];
+    // cout << "PATH " << p << " [length " << path->length << "] " << path->nodes[0];
+    // for(int n = 1; n < path->length; n++) {
+      // cout << " -> " << path->nodes[n];
+    // }
+    // cout << endl;
+  // }
 }
 
 int Tracker::nb_trajectories() {