From 0f54e5005de0f05d4bc400f23181301eb40f469e Mon Sep 17 00:00:00 2001 From: Francois Fleuret Date: Tue, 21 Aug 2012 16:55:37 -0700 Subject: [PATCH] Update. --- Makefile | 8 +- mtp.cc | 246 +-------------------------------------------------- mtp_graph.cc | 238 +++++++++++++++++++++++++++++++++++++++++++++++++ mtp_graph.h | 46 ++++++++++ tracker.cc | 123 ++++++++++++++++++++++++++ tracker.h | 47 ++++++++++ 6 files changed, 463 insertions(+), 245 deletions(-) create mode 100644 mtp_graph.cc create mode 100644 mtp_graph.h create mode 100644 tracker.cc create mode 100644 tracker.h diff --git a/Makefile b/Makefile index 5f2807e..adcff72 100644 --- a/Makefile +++ b/Makefile @@ -18,9 +18,9 @@ ######################################################################### ifeq ($(STATIC),yes) - LDFLAGS=-static -lm -ljpeg -lpng -lz -lcairo + LDFLAGS=-static -lm else - LDFLAGS=-lm -ljpeg -lpng -lcairo + LDFLAGS=-lm endif ifeq ($(DEBUG),yes) @@ -33,7 +33,7 @@ ifeq ($(PROFILE),yes) PROFILE_FLAG = -pg endif -CXXFLAGS = -Wall -I/usr/include/cairo $(OPTIMIZE_FLAG) $(PROFILE_FLAG) $(CXXGLPK) +CXXFLAGS = -Wall $(OPTIMIZE_FLAG) $(PROFILE_FLAG) all: mtp random-graph @@ -45,6 +45,8 @@ random-graph: \ $(CXX) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) mtp: \ + mtp_graph.o \ + tracker.o \ mtp.o $(CXX) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) diff --git a/mtp.cc b/mtp.cc index 8f37fd6..118e521 100644 --- a/mtp.cc +++ b/mtp.cc @@ -33,254 +33,16 @@ using namespace std; -typedef float scalar_t; +#include "mtp_graph.h" -#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); + MTPGraph graph(nb_vertices, nb_edges, ea, eb, source, sink); + graph.find_best_paths(el, result_edge_occupation); } void dot_print(int nb_vertices, diff --git a/mtp_graph.cc b/mtp_graph.cc new file mode 100644 index 0000000..fe74dd1 --- /dev/null +++ b/mtp_graph.cc @@ -0,0 +1,238 @@ + +/////////////////////////////////////////////////////////////////////////// +// 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 // +/////////////////////////////////////////////////////////////////////////// + +#include "mtp_graph.h" + +#include +#include + +using namespace std; + +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; } + } +}; + +void MTPGraph::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; + } + } +} + +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]; + 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].terminal_vertex = &vertices[to[e]]; + } +} + +MTPGraph::~MTPGraph() { + delete[] vertices; + delete[] edge_heap; +} + +void MTPGraph::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 MTPGraph::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 MTPGraph::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 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]; + } + + 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; + } + } +} diff --git a/mtp_graph.h b/mtp_graph.h new file mode 100644 index 0000000..072eeec --- /dev/null +++ b/mtp_graph.h @@ -0,0 +1,46 @@ + +/////////////////////////////////////////////////////////////////////////// +// 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 // +/////////////////////////////////////////////////////////////////////////// + +#ifndef MTP_GRAPH_H +#define MTP_GRAPH_H + +#include "misc.h" + +class Vertex; +class Edge; + +class MTPGraph { + void initialize_work_lengths(); + void update_work_length(); + void find_shortest_path(Vertex **front, Vertex **new_front); + + int _nb_vertices, _nb_edges; + Edge *edge_heap; + Vertex *vertices; + Vertex *source, *sink; +public: + MTPGraph(int nb_vertices, int nb_edges, int *from, int *to, + int source, int sink); + + ~MTPGraph(); + + void find_best_paths(scalar_t *lengths, int *result_edge_occupation); + void print(); +}; + +#endif diff --git a/tracker.cc b/tracker.cc new file mode 100644 index 0000000..0a76e08 --- /dev/null +++ b/tracker.cc @@ -0,0 +1,123 @@ + +/////////////////////////////////////////////////////////////////////////// +// 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 // +/////////////////////////////////////////////////////////////////////////// + +#include "tracker.h" + +Tracker::Tracker(int nb_locations, int nb_time_steps) { + _nb_locations = nb_locations; + _nb_time_steps = nb_time_steps; + _detection_score = allocate_array(nb_locations, nb_time_steps); + _allowed_motion = allocate_array(nb_locations, nb_locations); + for(int l = 0; l < nb_locations; l++) { + for(int m = 0; m < nb_locations; m++) { + _allowed_motion[l][m] = 0; + } + } +} + +Tracker::~Tracker() { +} + +void Tracker::set_allowed_motion(int from_location, int to_location) { + _allowed_motion[from_location][to_location] = 1; +} + +void Tracker::set_detection_score(int location, int time, scalar_t score) { +} + +void Tracker::track() { + + cout << "Building graph." << endl; + + int nb_motions = 0; + for(int l = 0; l < _nb_locations; l++) { + for(int m = 0; m < _nb_locations; m++) { + if(_allowed_motion[l][m]) nb_motions++; + } + } + + int nb_vertices = 2 + 2 * (_nb_time_steps + 1) * _nb_locations; + int nb_edges = _nb_locations * 2 // From source and to sink + + _nb_time_steps * nb_motions // Motions + + _nb_locations * _nb_time_steps; // Doubling of nodes to force + // one target per location + + int source = 0, sink = nb_vertices - 1; + int *node_from = new int[nb_edges]; + int *node_to = new int[nb_edges]; + scalar_t *edge_length = new scalar_t[nb_edges]; + int e = 0; + + for(int l = 0; l < _nb_locations; l++) { + node_from[e] = source; + node_to[e] = 1 + l + 0 * _nb_locations; + edge_length[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_length[e] = _detection_score[t][l]; + e++; + if(t == _nb_time_steps) { + node_from[e] = 1 + (2 * (t + 0) + 0) * _nb_locations + l; + node_to[e] = sink; + edge_length[e] = 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_length[e] = 0.0; + e++; + } + } + } + } + } +} + +// 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_length[e] = _detection_score[t][l]; + // e++; + // if(t == _nb_time_steps) { + // e++; + // } else { + // e += _nb_locations; + // } + // } + // } +// } + +int Tracker::nb_trajectories() { +} + +int Tracker::trajectory_start_time(int k) { +} + +int Tracker::trajectory_end_time(int k) { +} + +int Tracker::trajectory_location(int k, int time) { +} diff --git a/tracker.h b/tracker.h new file mode 100644 index 0000000..d37e46a --- /dev/null +++ b/tracker.h @@ -0,0 +1,47 @@ + +/////////////////////////////////////////////////////////////////////////// +// 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 // +/////////////////////////////////////////////////////////////////////////// + +#ifndef TRACKER_H +#define TRACKER_H + +#include "misc.h" +#include "mtp_graph.h" + +class Tracker { + int _nb_locations, _nb_time_steps; + MTPGraph *_graph; + scalar_t **_detection_score; + int **_allowed_motion; + +public: + Tracker(int nb_locations, int nb_time_steps); + ~Tracker(); + + void set_allowed_motion(int from_location, int to_location); + void set_detection_score(int location, int time, scalar_t score); + void make_graph(); + + void track(); + + int nb_trajectories(); + int trajectory_start_time(int k); + int trajectory_end_time(int k); + int trajectory_location(int k, int time); +}; + +#endif -- 2.20.1