X-Git-Url: https://fleuret.org/cgi-bin/gitweb/gitweb.cgi?a=blobdiff_plain;f=mtp.cc;h=13cfad771b185167893034a0d7bfe39c44a14ec9;hb=4343b59acbf5be40580a688ccb85a9fbccd3f617;hp=8f37fd6f624f526a8e8dc1172320b9b004391b45;hpb=ad2517dfc2d6dd190cfe865c6ac87ff6e3a4bc05;p=mtp.git diff --git a/mtp.cc b/mtp.cc index 8f37fd6..13cfad7 100644 --- a/mtp.cc +++ b/mtp.cc @@ -22,8 +22,6 @@ // EXAMPLE: ./mtp ./graph2.txt | dot -T pdf -o- | xpdf - -#define VERBOSE - #include #include #include @@ -33,275 +31,49 @@ using namespace std; -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 - -class Vertex; - -class Edge { -public: - int id, occupied; - scalar_t length, work_length; - Vertex *terminal_vertex; - Edge *next, *pred; -}; - -class Vertex { -public: - int id, iteration; - Edge *root_edge; - scalar_t distance_from_source; - Vertex *pred_vertex; - Edge *pred_edge; - - Vertex() { root_edge = 0; } - - inline void add_edge(Edge *e) { - e->next = root_edge; - e->pred = 0; - if(root_edge) { root_edge->pred = e; } - root_edge = e; - } - - inline void del_edge(Edge *e) { - if(e == root_edge) { root_edge = e->next; } - if(e->pred) { e->pred->next = e->next; } - if(e->next) { e->next->pred = e->pred; } - } -}; - -class Graph { - void initialize_work_lengths(); - void update_work_length(); - void find_shortest_path(Vertex **front, Vertex **new_front); - - int nb_vertices; - Edge *edge_heap; - Vertex *vertices; - Vertex *source, *sink; -public: - Graph(int nb_vertices, int nb_edges, int *from, int *to, scalar_t *lengths, - int source, int sink); - - ~Graph(); - - void find_best_paths(int *result_edge_occupation); - void print(); -}; - -void Graph::print() { - for(int n = 0; n < nb_vertices; n++) { - for(Edge *e = vertices[n].root_edge; e; e = e->next) { - cout << n << " -> " << e->terminal_vertex->id << " " << e->length; - if(e->occupied) { - cout << " *"; - } - cout << endl; - } - } -} - -Graph::Graph(int nb_vrt, int nb_edges, - int *from, int *to, scalar_t *lengths, - int src, int snk) { - nb_vertices = nb_vrt; - - edge_heap = new Edge[nb_edges]; - vertices = new Vertex[nb_vertices]; - - 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].length = lengths[e]; - edge_heap[e].terminal_vertex = &vertices[to[e]]; - } -} - -Graph::~Graph() { - delete[] vertices; - delete[] edge_heap; -} - -void Graph::initialize_work_lengths() { - scalar_t length_min = 0; - for(int n = 0; n < nb_vertices; n++) { - for(Edge *e = vertices[n].root_edge; e; e = e->next) { - length_min = min(e->length, length_min); - } - } - for(int n = 0; n < nb_vertices; n++) { - for(Edge *e = vertices[n].root_edge; e; e = e->next) { - e->work_length = e->length - length_min; - } - } -} - -void Graph::update_work_length() { - for(int n = 0; n < nb_vertices; n++) { - scalar_t d = vertices[n].distance_from_source; - for(Edge *e = vertices[n].root_edge; e; e = e->next) { - e->work_length += d - e->terminal_vertex->distance_from_source; - } - } -} - -void Graph::find_shortest_path(Vertex **front, Vertex **new_front) { - Vertex **tmp_front; - int tmp_front_size; - Vertex *v, *tv; - scalar_t d; - -#ifdef VERBOSE - scalar_t residual_error = 0.0; -#endif - for(int n = 0; n < nb_vertices; n++) { - for(Edge *e = vertices[n].root_edge; e; e = e->next) { - if(e->work_length < 0) { -#ifdef VERBOSE - residual_error -= e->work_length; -#endif - e->work_length = 0.0; - } - } - } -#ifdef VERBOSE - cerr << "residual_error " << residual_error << endl; -#endif - - for(int v = 0; v < nb_vertices; v++) { - vertices[v].distance_from_source = FLT_MAX; - vertices[v].pred_vertex = 0; - vertices[v].pred_edge = 0; - vertices[v].iteration = 0; - } - - int iteration = 0; - - int front_size = 0, new_front_size; - front[front_size++] = source; - source->distance_from_source = 0; - - do { - new_front_size = 0; - iteration++; - 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; - if(d < tv->distance_from_source) { - tv->distance_from_source = d; - tv->pred_vertex = v; - tv->pred_edge = e; - if(tv->iteration < iteration) { - new_front[new_front_size++] = tv; - tv->iteration = iteration; - } - } - } - } - - 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); -} - -void Graph::find_best_paths(int *result_edge_occupation) { - Vertex **front = new Vertex *[nb_vertices]; - Vertex **new_front = new Vertex *[nb_vertices]; - - scalar_t total_length; - - initialize_work_lengths(); - - do { - total_length = 0.0; - find_shortest_path(front, new_front); - update_work_length(); +#include "mtp_graph.h" +#include "tracker.h" - // 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) { - 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) { - Edge *e = v->pred_edge; - e->terminal_vertex = v->pred_vertex; - e->occupied = 1 - e->occupied; - e->length = - e->length; - e->work_length = - e->work_length; - v->pred_vertex->del_edge(e); - v->add_edge(e); - } - } - } - } 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) { - result_edge_occupation[e->id] = e->occupied; - } - } -} +////////////////////////////////////////////////////////////////////// void find_best_paths(int nb_vertices, int nb_edges, int *ea, int *eb, scalar_t *el, int source, int sink, int *result_edge_occupation) { - Graph graph(nb_vertices, nb_edges, ea, eb, el, source, sink); - graph.find_best_paths(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; + MTPGraph graph(nb_vertices, nb_edges, ea, eb, source, sink); + graph.find_best_paths(el, result_edge_occupation); + graph.print_dot(); } ////////////////////////////////////////////////////////////////////// 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); if(argc < 2) { cerr << argv[0] << " " << endl; @@ -332,10 +104,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;