X-Git-Url: https://fleuret.org/cgi-bin/gitweb/gitweb.cgi?p=mtp.git;a=blobdiff_plain;f=mtp.cc;h=e749b671681b0c0c53a9fefe8817abcd05105a33;hp=307da22ffc06b1c57dac1940129af6c4d8481744;hb=22e800d663bb7a6b03ba6735fef54bf12c6cd2b5;hpb=af5339cd0eb42ef3fc1c09d3662cf9fdc900ed38 diff --git a/mtp.cc b/mtp.cc index 307da22..e749b67 100644 --- a/mtp.cc +++ b/mtp.cc @@ -1,343 +1,217 @@ -/////////////////////////////////////////////////////////////////////////// -// 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 - -// #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 +#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(); \ +#include "mtp_tracker.h" + +#define FILENAME_SIZE 1024 + +struct Global { + char trajectory_filename[FILENAME_SIZE]; + char graph_filename[FILENAME_SIZE]; + int verbose; +} global; + +void usage(ostream *os) { + (*os) << "mtp [-h|--help] [--help-formats] [-v|--verbose] [-t|--trajectory-filename ] [-g|--graph-filename ] []" << endl; + (*os) << endl; + (*os) << "The mtp command processes a file containing the description of a topology" << endl; + (*os) << "and detection scores, and prints the optimal set of trajectories." << endl; + (*os) << endl; + (*os) << "If no filename is provided, it reads the parameters from the standard" << endl; + (*os) << "input. If no trajectory filename is provided, it writes the result to" << endl; + (*os) << "the standard output." << endl; + (*os) << endl; + (*os) << "Written by Francois Fleuret. (C) Idiap Research Institute, 2012." << endl; } -#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; +void print_help_formats() { + cout << "The tracking parameters the command takes as input have the following" << endl; + cout << "format, where L is the number of locations and T is the number of time" << endl; + cout << "steps:" << endl; + cout << endl; + cout << "---------------------------- snip snip -------------------------------" << endl; + cout << " int:L int:T" << endl; + cout << endl; + cout << " bool:allowed_motion_from_1_to_1 ... bool:allowed_motion_from_1_to_L" << endl; + cout << " ..." << endl; + cout << " bool:allowed_motion_from_L_to_1 ... bool:allowed_motion_from_L_to_L" << endl; + cout << endl; + cout << " bool:entrance_1_1 ... bool:entrance_1_L" << endl; + cout << " ..." << endl; + cout << " bool:entrance_T_1 ... bool:entrance_T_L" << endl; + cout << endl; + cout << " bool:exit_1_1 ... bool:exit_1_L" << endl; + cout << " ..." << endl; + cout << " bool:exit_T_1 ... bool:exit_T_L" << endl; + cout << endl; + cout << " float:detection_score_1_1 ... float:detection_score_1_L" << endl; + cout << " ..." << endl; + cout << " float:detection_score_T_1 ... float:detection_score_T_L" << endl; + cout << "---------------------------- snip snip -------------------------------" << endl; + cout << endl; + cout << "As results, the command writes first the number of trajectories," << endl; + cout << "followed by one line per trajectory with the following structure:" << endl; + cout << endl; + cout << "---------------------------- snip snip -------------------------------" << endl; + cout << " int:traj_number int:entrance_time int:duration float:score int:location_1 ... int:location_duration" << endl; + cout << "---------------------------- snip snip -------------------------------" << endl; +} - Edge *root_edge; - scalar_t distance_from_source; +scalar_t diff_in_second(struct timeval *start, struct timeval *end) { + return + scalar_t(end->tv_sec - start->tv_sec) + + scalar_t(end->tv_usec - start->tv_usec)/1000000; +} - Vertex *pred_vertex; - Edge *pred_edge; +void do_tracking(istream *in_tracker) { + timeval start_time, end_time; + MTPTracker *tracker = new MTPTracker(); - Vertex() { root_edge = 0; } + if(global.verbose) { cout << "Reading the tracking parameters." << endl; } + tracker->read(in_tracker); - inline void add_edge(Edge *e) { - e->next = root_edge; - e->pred = 0; - if(root_edge) { root_edge->pred = e; } - root_edge = e; + if(global.verbose) { + cout << "Building the graph ... "; cout.flush(); + gettimeofday(&start_time, 0); } - - 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; } + tracker->build_graph(); + if(global.verbose) { + gettimeofday(&end_time, 0); + cout << "done (" << diff_in_second(&start_time, &end_time) << "s)." << endl; } -}; - -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; - } + if(global.verbose) { + cout << "Tracking ... "; cout.flush(); + gettimeofday(&start_time, 0); + } + tracker->track(); + if(global.verbose) { + gettimeofday(&end_time, 0); + cout << "done (" << diff_in_second(&start_time, &end_time) << "s)." << 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; + if(strcmp(global.trajectory_filename, "")) { + ofstream out_traj(global.trajectory_filename); + tracker->write_trajectories(&out_traj); + if(global.verbose) { cout << "Wrote " << global.trajectory_filename << "." << endl; } + } else { + tracker->write_trajectories(&cout); } - 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]]; + if(strcmp(global.graph_filename, "") != 0) { + ofstream out_dot(global.graph_filename); + tracker->print_graph_dot(&out_dot); + if(global.verbose) { cout << "Wrote " << global.graph_filename << "." << endl; } } -} -Graph::~Graph() { - delete[] vertices; - delete[] edge_heap; + delete tracker; } -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; - } - } -} +enum +{ + OPT_HELP_FORMATS = CHAR_MAX + 1 +}; -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; - } - } -} +static struct option long_options[] = { + { "trajectory-file", 1, 0, 't' }, + { "graph-file", 1, 0, 'g' }, + { "help", no_argument, 0, 'h' }, + { "verbose", no_argument, 0, 'v' }, + { "help-formats", no_argument, 0, OPT_HELP_FORMATS }, + { 0, 0, 0, 0 } +}; -void Graph::find_shortest_path(Vertex **front, Vertex **new_front) { - Vertex **tmp_front; - int tmp_front_size; - Vertex *v, *tv; - scalar_t d; - -#ifdef DEBUG - for(int n = 0; n < nb_vertices; n++) { - for(Edge *e = vertices[n].root_edge; e; e = e->next) { - if(e->work_length < 0) { - cerr << "DEBUG error in find_shortest_path: Edge fixed lengths have to be positive." - << endl; - abort(); - } - } - } -#endif +int main(int argc, char **argv) { + int c; + int error = 0, show_help = 0; - 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; - } + strncpy(global.trajectory_filename, "", FILENAME_SIZE); + strncpy(global.graph_filename, "", FILENAME_SIZE); + global.verbose = 0; - int front_size = 0, new_front_size; - front[front_size++] = source; - source->distance_from_source = 0; - - do { - new_front_size = 0; - 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; - new_front[new_front_size++] = tv; - } - } - } + while ((c = getopt_long(argc, argv, "t:g:hv", long_options, NULL)) != -1) { - tmp_front = new_front; - new_front = front; - front = tmp_front; + switch(c) { - tmp_front_size = new_front_size; - new_front_size = front_size; - front_size = tmp_front_size; - } while(front_size > 0); -} + case 't': + strncpy(global.trajectory_filename, optarg, FILENAME_SIZE); + break; -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); + case 'g': + strncpy(global.graph_filename, optarg, FILENAME_SIZE); + break; - delete[] front; - delete[] new_front; + case 'h': + show_help = 1; + break; - 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; - } - } -} + case OPT_HELP_FORMATS: + print_help_formats(); + exit(EXIT_SUCCESS); + break; -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); -} + case 'v': + global.verbose = 1; + break; -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; + default: + error = 1; + break; } } - cout << "}" << endl; -} - -////////////////////////////////////////////////////////////////////// - -int main(int argc, char **argv) { - if(argc < 2) { - cerr << argv[0] << " " << endl; + if(error) { + usage(&cerr); exit(EXIT_FAILURE); } - ifstream *file = new ifstream(argv[1]); - - int nb_edges, nb_vertices; - int source, sink; - - if(file->good()) { - - (*file) >> nb_vertices >> nb_edges; - (*file) >> source >> sink; - - 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]; + if(show_help) { + usage(&cout); + exit(EXIT_SUCCESS); + } - for(int e = 0; e < nb_edges; e++) { - (*file) >> vertex_from[e] >> vertex_to[e] >> edge_lengths[e]; + if(optind < argc) { + ifstream *file_in_tracker = new ifstream(argv[optind]); + if(file_in_tracker->good()) { + do_tracking(file_in_tracker); + } else { + cerr << "Can not open " << argv[optind] << endl; + exit(EXIT_FAILURE); } - - find_best_paths(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; - delete[] vertex_from; - delete[] vertex_to; - + delete file_in_tracker; } else { - - cerr << "Can not open " << argv[1] << endl; - - delete file; - exit(EXIT_FAILURE); - + do_tracking(&cin); } - delete file; exit(EXIT_SUCCESS); }