X-Git-Url: https://fleuret.org/cgi-bin/gitweb/gitweb.cgi?p=mtp.git;a=blobdiff_plain;f=mtp.cc;h=36f09f113bc1ba091751a91590a59c0cafe8b48d;hp=8f37fd6f624f526a8e8dc1172320b9b004391b45;hb=02b0f31c9e2e4ca12f7f33d63ae7a77de2d711c2;hpb=ad2517dfc2d6dd190cfe865c6ac87ff6e3a4bc05 diff --git a/mtp.cc b/mtp.cc index 8f37fd6..36f09f1 100644 --- a/mtp.cc +++ b/mtp.cc @@ -1,356 +1,81 @@ -/////////////////////////////////////////////////////////////////////////// -// This program is free software: you can redistribute it and/or modify // -// it under the terms of the version 3 of the GNU General Public License // -// as published by the Free Software Foundation. // -// // -// This program is distributed in the hope that it will be useful, but // -// WITHOUT ANY WARRANTY; without even the implied warranty of // -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // -// General Public License for more details. // -// // -// You should have received a copy of the GNU General Public License // -// along with this program. If not, see . // -// // -// Written by and Copyright (C) Francois Fleuret // -// Contact for comments & bug reports // -/////////////////////////////////////////////////////////////////////////// - -// Multi-Tracked Path - -// Takes the graph description file as input and produces a dot file. - -// EXAMPLE: ./mtp ./graph2.txt | dot -T pdf -o- | xpdf - - -#define VERBOSE +/* + * mtp is the ``Multi Tracked Paths'', an implementation of the + * k-shortest paths algorithm for multi-target tracking. + * + * Copyright (c) 2012 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of mtp. + * + * mtp is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * mtp is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public + * License for more details. + * + * You should have received a copy of the GNU General Public License + * along with selector. If not, see . + * + */ #include #include -#include -#include -#include -#include 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(); - - // 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; -} - -////////////////////////////////////////////////////////////////////// +#include "mtp_tracker.h" int main(int argc, char **argv) { + time_t start_time, end_time; if(argc < 2) { - cerr << argv[0] << " " << endl; + cerr << argv[0] << " " << endl; exit(EXIT_FAILURE); } - ifstream *file = new ifstream(argv[1]); + ifstream *in_tracker = new ifstream(argv[1]); - int nb_edges, nb_vertices; - int source, sink; + if(in_tracker->good()) { - if(file->good()) { + MTPTracker *tracker = new MTPTracker(); - (*file) >> nb_vertices >> nb_edges; - (*file) >> source >> sink; + cout << "Reading " << argv[1] << "." << endl; + tracker->read(in_tracker); - scalar_t *edge_lengths = new scalar_t[nb_edges]; - int *vertex_from = new int[nb_edges]; - int *vertex_to = new int[nb_edges]; - int *result_edge_occupation = new int[nb_edges]; + cout << "Building the graph ... "; cout.flush(); + start_time = time(0); + tracker->build_graph(); + end_time = time(0); + cout << "done (" << end_time - start_time << "s)." << endl; - for(int e = 0; e < nb_edges; e++) { - (*file) >> vertex_from[e] >> vertex_to[e] >> edge_lengths[e]; - } + cout << "Tracking ... "; cout.flush(); + start_time = time(0); + tracker->track(); + end_time = time(0); + cout << "done (" << end_time - start_time << "s)." << endl; - find_best_paths(nb_vertices, nb_edges, - vertex_from, vertex_to, edge_lengths, - source, sink, - result_edge_occupation); + ofstream out_traj("result.trj"); + tracker->write_trajectories(&out_traj); + cout << "Wrote result.trj" << endl; - dot_print(nb_vertices, nb_edges, - vertex_from, vertex_to, edge_lengths, - source, sink, - result_edge_occupation); + ofstream out_dot("graph.dot"); + tracker->print_graph_dot(&out_dot); + cout << "Wrote graph.dot" << endl; - delete[] result_edge_occupation; - delete[] edge_lengths; - delete[] vertex_from; - delete[] vertex_to; + delete tracker; } else { cerr << "Can not open " << argv[1] << endl; - - delete file; exit(EXIT_FAILURE); } - delete file; + delete in_tracker; + exit(EXIT_SUCCESS); }