From 598184c893e63ddd5b473aee104a9c2d1af07830 Mon Sep 17 00:00:00 2001 From: Francois Fleuret Date: Wed, 22 Aug 2012 20:57:12 -0700 Subject: [PATCH] Debugged a stupid bug. --- mtp.cc | 14 +++++++++++--- mtp_graph.cc | 54 +++++++++++++++++++++++++++++++++++++++++++++++++--- mtp_graph.h | 1 + tracker.cc | 15 ++++++++++++--- 4 files changed, 75 insertions(+), 9 deletions(-) diff --git a/mtp.cc b/mtp.cc index e096836..35b431b 100644 --- a/mtp.cc +++ b/mtp.cc @@ -23,6 +23,7 @@ // EXAMPLE: ./mtp ./graph2.txt | dot -T pdf -o- | xpdf - #include +#include #include 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 --- 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 --- a/mtp_graph.h +++ b/mtp_graph.h @@ -20,6 +20,7 @@ #define MTP_GRAPH_H #include +#include using namespace std; diff --git a/tracker.cc b/tracker.cc index 3a311cd..47460d3 100644 --- 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() { -- 2.20.1