From ea33e3e5dddbe3796d2361910e1e3ed98a19865c Mon Sep 17 00:00:00 2001 From: Francois Fleuret Date: Tue, 21 Aug 2012 21:58:50 -0700 Subject: [PATCH] Some refactoring to reuse the same graph for several trackings. --- mtp.cc | 39 ++++++++++-------- mtp_graph.cc | 111 ++++++++++++++++++++++++++++----------------------- mtp_graph.h | 14 +++---- tracker.cc | 94 ++++++++++++++++++++++++------------------- tracker.h | 8 +++- 5 files changed, 148 insertions(+), 118 deletions(-) diff --git a/mtp.cc b/mtp.cc index bd6d296..e330ce3 100644 --- a/mtp.cc +++ b/mtp.cc @@ -49,25 +49,30 @@ void find_best_paths(int nb_vertices, ////////////////////////////////////////////////////////////////////// int main(int argc, char **argv) { - int nb_locations = 3; - int nb_time_steps = 4; + int nb_locations = 4; + int nb_time_steps = 3; - Tracker tracker(nb_time_steps, nb_locations); + { + Tracker tracker(nb_time_steps, nb_locations); - for(int l = 0; l < nb_locations; l++) { - for(int k = 0; k < nb_locations; k++) { - if(abs(l - k) <= 1) tracker.set_allowed_motion(l, k); + 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, -1.0); + 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, + (drand48() < 0.95 ? 1.0 : -1.0) + drand48() * 0.1 - 0.05); } - tracker.set_detection_score(t, 0, 1.0); - } - tracker.track(); + tracker.build_graph(); + tracker.track(); + } exit(0); @@ -100,10 +105,10 @@ int main(int argc, char **argv) { source, sink, result_edge_occupation); - dot_print(nb_vertices, nb_edges, - vertex_from, vertex_to, edge_lengths, - source, sink, - result_edge_occupation); + // dot_print(nb_vertices, nb_edges, + // vertex_from, vertex_to, edge_lengths, + // source, sink, + // result_edge_occupation); delete[] result_edge_occupation; delete[] edge_lengths; diff --git a/mtp_graph.cc b/mtp_graph.cc index eb4d3cc..c2a7fa3 100644 --- a/mtp_graph.cc +++ b/mtp_graph.cc @@ -67,33 +67,64 @@ void MTPGraph::print() { } } +void MTPGraph::print_dot() { + cout << "digraph {" << endl; + cout << " node[shape=circle];" << endl; + for(int n = 0; n < _nb_vertices; n++) { + int a = vertices[n].id; + for(Edge *e = vertices[n].root_edge; e; e = e->next) { + int b = e->terminal_vertex->id; + if(e->occupied) { + cout << " " << b << " -> " << a << " [style=bold,color=black,label=\"" << -e->length << "\"];" << endl; + } else { + cout << " " << a << " -> " << b << " [color=gray,label=\"" << e->length << "\"];" << endl; + } + } + } + cout << "}" << endl; +} + + +void dot_print(int nb_vertices, + int nb_edges, int *ea, int *eb, scalar_t *el, + int _source, int _sink, + int *edge_occupation) { + for(int e = 0; e < nb_edges; e++) { + } + cout << "}" << endl; +} MTPGraph::MTPGraph(int nb_vertices, int nb_edges, int *from, int *to, int src, int snk) { _nb_vertices = nb_vertices; _nb_edges = nb_edges; - edge_heap = new Edge[_nb_edges]; + edges = new Edge[_nb_edges]; vertices = new Vertex[_nb_vertices]; - source = &vertices[src]; - sink = &vertices[snk]; + _source = &vertices[src]; + _sink = &vertices[snk]; for(int v = 0; v < _nb_vertices; v++) { vertices[v].id = v; } for(int e = 0; e < nb_edges; e++) { - vertices[from[e]].add_edge(&edge_heap[e]); - edge_heap[e].occupied = 0; - edge_heap[e].id = e; - edge_heap[e].terminal_vertex = &vertices[to[e]]; + vertices[from[e]].add_edge(&edges[e]); + edges[e].occupied = 0; + edges[e].id = e; + edges[e].terminal_vertex = &vertices[to[e]]; } + + _front = new Vertex *[_nb_vertices]; + _new_front = new Vertex *[_nb_vertices]; } MTPGraph::~MTPGraph() { delete[] vertices; - delete[] edge_heap; + delete[] edges; + delete[] _front; + delete[] _new_front; } void MTPGraph::initialize_work_lengths() { @@ -119,7 +150,7 @@ void MTPGraph::update_work_length() { } } -void MTPGraph::find_shortest_path(Vertex **front, Vertex **new_front) { +void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) { Vertex **tmp_front; int tmp_front_size; Vertex *v, *tv; @@ -151,15 +182,15 @@ void MTPGraph::find_shortest_path(Vertex **front, Vertex **new_front) { int iteration = 0; - int front_size = 0, new_front_size; - front[front_size++] = source; - source->distance_from_source = 0; + int _front_size = 0, _new_front_size; + _front[_front_size++] = _source; + _source->distance_from_source = 0; do { - new_front_size = 0; + _new_front_size = 0; iteration++; - for(int f = 0; f < front_size; f++) { - v = front[f]; + for(int f = 0; f < _front_size; f++) { + v = _front[f]; for(Edge *e = v->root_edge; e; e = e->next) { d = v->distance_from_source + e->work_length; tv = e->terminal_vertex; @@ -168,52 +199,49 @@ void MTPGraph::find_shortest_path(Vertex **front, Vertex **new_front) { tv->pred_vertex = v; tv->pred_edge = e; if(tv->iteration < iteration) { - new_front[new_front_size++] = tv; + _new_front[_new_front_size++] = tv; tv->iteration = iteration; } } } } - tmp_front = new_front; - new_front = front; - front = tmp_front; + tmp_front = _new_front; + _new_front = _front; + _front = tmp_front; - tmp_front_size = new_front_size; - new_front_size = front_size; - front_size = tmp_front_size; - } while(front_size > 0); + tmp_front_size = _new_front_size; + _new_front_size = _front_size; + _front_size = tmp_front_size; + } while(_front_size > 0); } void MTPGraph::find_best_paths(scalar_t *lengths, int *result_edge_occupation) { - Vertex **front = new Vertex *[_nb_vertices]; - Vertex **new_front = new Vertex *[_nb_vertices]; - scalar_t total_length; for(int e = 0; e < _nb_edges; e++) { - edge_heap[e].length = lengths[e]; + edges[e].length = lengths[e]; } initialize_work_lengths(); do { total_length = 0.0; - find_shortest_path(front, new_front); + find_shortest_path(_front, _new_front); update_work_length(); - // Do we reach the sink? - if(sink->pred_edge) { + // Do we reach the _sink? + if(_sink->pred_edge) { // If yes, compute the length of the best path - for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) { + for(Vertex *v = _sink; v->pred_edge; v = v->pred_vertex) { total_length += v->pred_edge->length; } // If that length is negative if(total_length < 0.0) { // Invert all the edges along the best path - for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) { + for(Vertex *v = _sink; v->pred_edge; v = v->pred_vertex) { Edge *e = v->pred_edge; e->terminal_vertex = v->pred_vertex; e->occupied = 1 - e->occupied; @@ -226,9 +254,6 @@ void MTPGraph::find_best_paths(scalar_t *lengths, int *result_edge_occupation) { } } while(total_length < 0.0); - delete[] front; - delete[] new_front; - for(int n = 0; n < _nb_vertices; n++) { Vertex *v = &vertices[n]; for(Edge *e = v->root_edge; e; e = e->next) { @@ -236,19 +261,3 @@ void MTPGraph::find_best_paths(scalar_t *lengths, int *result_edge_occupation) { } } } - -void dot_print(int nb_vertices, - int nb_edges, int *ea, int *eb, scalar_t *el, - int source, int sink, - int *edge_occupation) { - cout << "digraph {" << endl; - cout << " node[shape=circle];" << endl; - for(int e = 0; e < nb_edges; e++) { - if(edge_occupation[e]) { - cout << " " << ea[e] << " -> " << eb[e] << " [style=bold,color=black,label=\"" << el[e] << "\"];" << endl; - } else { - cout << " " << ea[e] << " -> " << eb[e] << " [color=gray,label=\"" << el[e] << "\"];" << endl; - } - } - cout << "}" << endl; -} diff --git a/mtp_graph.h b/mtp_graph.h index 9c14ea2..699ee16 100644 --- a/mtp_graph.h +++ b/mtp_graph.h @@ -30,10 +30,12 @@ class MTPGraph { void find_shortest_path(Vertex **front, Vertex **new_front); int _nb_vertices, _nb_edges; - Edge *edge_heap; - Vertex *vertices; - Vertex *source, *sink; + Vertex *_source, *_sink; + Vertex **_front, **_new_front; public: + Edge *edges; + Vertex *vertices; + MTPGraph(int nb_vertices, int nb_edges, int *from, int *to, int source, int sink); @@ -41,11 +43,7 @@ public: void find_best_paths(scalar_t *lengths, int *result_edge_occupation); void print(); + void print_dot(); }; -void dot_print(int nb_vertices, - int nb_edges, int *ea, int *eb, scalar_t *el, - int source, int sink, - int *edge_occupation); - #endif diff --git a/tracker.cc b/tracker.cc index e0b0903..8490d6f 100644 --- a/tracker.cc +++ b/tracker.cc @@ -32,22 +32,32 @@ Tracker::Tracker(int nb_time_steps, int nb_locations) { _allowed_motion[l][m] = 0; } } + + _edge_lengths = 0; + _graph = 0; + _edge_occupation = 0; } Tracker::~Tracker() { + delete[] _edge_lengths; + delete _graph; + delete[] _edge_occupation; + deallocate_array(_detection_score); + deallocate_array(_allowed_motion); } -void Tracker::set_allowed_motion(int from_location, int to_location) { - _allowed_motion[from_location][to_location] = 1; +void Tracker::set_allowed_motion(int from_location, int to_location, int v) { + _allowed_motion[from_location][to_location] = v; } void Tracker::set_detection_score(int time, int location, scalar_t score) { _detection_score[time][location] = score; } -void Tracker::track() { - - cerr << "Building graph." << endl; +void Tracker::build_graph() { + delete[] _edge_lengths; + delete[] _graph; + delete[] _edge_occupation; int nb_motions = 0; for(int l = 0; l < _nb_locations; l++) { @@ -57,41 +67,51 @@ void Tracker::track() { } int nb_vertices = 2 + 2 * _nb_time_steps * _nb_locations; - int nb_edges = _nb_locations * 2 // From source and to sink - + (_nb_time_steps - 1) * nb_motions // Motions - + _nb_locations * _nb_time_steps; // Doubling of nodes to force - // one target per location + int nb_edges = _nb_locations * 2 + + (_nb_time_steps - 1) * nb_motions + + _nb_locations * _nb_time_steps; int source = 0, sink = nb_vertices - 1; int *node_from = new int[nb_edges]; int *node_to = new int[nb_edges]; - scalar_t *edge_lengths = new scalar_t[nb_edges]; int e = 0; + _edge_lengths = new scalar_t[nb_edges]; + _edge_occupation = new int[nb_edges]; + + // We put the in-node edges first, since these are the ones whose + // lengths we will have to change according to the detection score + + for(int t = 0; t < _nb_time_steps; t++) { + for(int l = 0; l < _nb_locations; l++) { + node_from[e] = 1 + (2 * (t + 0) + 0) * _nb_locations + l; + node_to[e] = 1 + (2 * (t + 0) + 1) * _nb_locations + l; + e++; + } + } + + // We put the other edges after + for(int l = 0; l < _nb_locations; l++) { node_from[e] = source; node_to[e] = 1 + l + 0 * _nb_locations; - edge_lengths[e] = 0.0; + _edge_lengths[e] = 0.0; e++; } for(int t = 0; t < _nb_time_steps; t++) { for(int l = 0; l < _nb_locations; l++) { - node_from[e] = 1 + (2 * (t + 0) + 0) * _nb_locations + l; - node_to[e] = 1 + (2 * (t + 0) + 1) * _nb_locations + l; - edge_lengths[e] = - _detection_score[t][l]; - e++; if(t == _nb_time_steps - 1) { node_from[e] = 1 + (2 * (t + 0) + 1) * _nb_locations + l; - node_to[e] = sink; - edge_lengths[e] = 0.0; + node_to[e] = sink; + _edge_lengths[e] = 0.0; e++; } else { for(int k = 0; k < _nb_locations; k++) { if(_allowed_motion[l][k]) { node_from[e] = 1 + (2 * (t + 0) + 1) * _nb_locations + l; node_to[e] = 1 + (2 * (t + 1) + 0) * _nb_locations + k; - edge_lengths[e] = 0.0; + _edge_lengths[e] = 0.0; e++; } } @@ -99,35 +119,29 @@ void Tracker::track() { } } - cerr << "DEBUG e = " << e << " nb_edges = " << nb_edges << endl; + _graph = new MTPGraph(nb_vertices, nb_edges, + node_from, node_to, + source, sink); - _graph = new MTPGraph(nb_vertices, nb_edges, node_from, node_to, source, sink); - - int *edge_occupation = new int[nb_edges]; + delete[] node_from; + delete[] node_to; +} - _graph->find_best_paths(edge_lengths, edge_occupation); +void Tracker::track() { + int e = 0; + for(int t = 0; t < _nb_time_steps; t++) { + for(int l = 0; l < _nb_locations; l++) { + _edge_lengths[e] = - _detection_score[t][l]; + e++; + } + } - dot_print(nb_vertices, nb_edges, node_from, node_to, edge_lengths, - source, sink, - edge_occupation); + _graph->find_best_paths(_edge_lengths, _edge_occupation); - delete[] edge_occupation; - delete _graph; + _graph->print_dot(); } // void Tracker::track() { - // int e = _nb_locations; - // for(int t = 0; t <= _nb_time_steps; t++) { - // for(int l = 0; l < _nb_locations; l++) { - // edge_lengths[e] = _detection_score[t][l]; - // e++; - // if(t == _nb_time_steps) { - // e++; - // } else { - // e += _nb_locations; - // } - // } - // } // } // int Tracker::nb_trajectories() { diff --git a/tracker.h b/tracker.h index 4143cf5..d8cb811 100644 --- a/tracker.h +++ b/tracker.h @@ -24,15 +24,19 @@ class Tracker { int _nb_locations, _nb_time_steps; - MTPGraph *_graph; scalar_t **_detection_score; int **_allowed_motion; + MTPGraph *_graph; + int *_edge_occupation; + scalar_t *_edge_lengths; public: Tracker(int nb_time_steps, int nb_locations); ~Tracker(); - void set_allowed_motion(int from_location, int to_location); + void set_allowed_motion(int from_location, int to_location, int v); + void build_graph(); + void set_detection_score(int time, int location, scalar_t score); void track(); -- 2.39.5