e0b0903af548d6427a6966fe37f20791a867fbc7
[mtp.git] / tracker.cc
1
2 ///////////////////////////////////////////////////////////////////////////
3 // This program is free software: you can redistribute it and/or modify  //
4 // it under the terms of the version 3 of the GNU General Public License //
5 // as published by the Free Software Foundation.                         //
6 //                                                                       //
7 // This program is distributed in the hope that it will be useful, but   //
8 // WITHOUT ANY WARRANTY; without even the implied warranty of            //
9 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      //
10 // General Public License for more details.                              //
11 //                                                                       //
12 // You should have received a copy of the GNU General Public License     //
13 // along with this program. If not, see <http://www.gnu.org/licenses/>.  //
14 //                                                                       //
15 // Written by and Copyright (C) Francois Fleuret                         //
16 // Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
17 ///////////////////////////////////////////////////////////////////////////
18
19 #include "tracker.h"
20
21 #include <iostream>
22
23 using namespace std;
24
25 Tracker::Tracker(int nb_time_steps, int nb_locations) {
26   _nb_locations = nb_locations;
27   _nb_time_steps = nb_time_steps;
28   _detection_score = allocate_array<scalar_t>(nb_time_steps, nb_locations);
29   _allowed_motion = allocate_array<int>(nb_locations, nb_locations);
30   for(int l = 0; l < nb_locations; l++) {
31     for(int m = 0; m < nb_locations; m++) {
32       _allowed_motion[l][m] = 0;
33     }
34   }
35 }
36
37 Tracker::~Tracker() {
38 }
39
40 void Tracker::set_allowed_motion(int from_location, int to_location) {
41   _allowed_motion[from_location][to_location] = 1;
42 }
43
44 void Tracker::set_detection_score(int time, int location, scalar_t score) {
45   _detection_score[time][location] = score;
46 }
47
48 void Tracker::track() {
49
50   cerr << "Building graph." << endl;
51
52   int nb_motions = 0;
53   for(int l = 0; l < _nb_locations; l++) {
54     for(int m = 0; m < _nb_locations; m++) {
55       if(_allowed_motion[l][m]) nb_motions++;
56     }
57   }
58
59   int nb_vertices = 2 + 2 * _nb_time_steps * _nb_locations;
60   int nb_edges = _nb_locations * 2    // From source and to sink
61     + (_nb_time_steps - 1) * nb_motions     // Motions
62     + _nb_locations * _nb_time_steps; // Doubling of nodes to force
63                                       // one target per location
64
65   int source = 0, sink = nb_vertices - 1;
66   int *node_from = new int[nb_edges];
67   int *node_to = new int[nb_edges];
68   scalar_t *edge_lengths = new scalar_t[nb_edges];
69   int e = 0;
70
71   for(int l = 0; l < _nb_locations; l++) {
72     node_from[e] = source;
73     node_to[e] = 1 + l + 0 * _nb_locations;
74     edge_lengths[e] = 0.0;
75     e++;
76   }
77
78   for(int t = 0; t < _nb_time_steps; t++) {
79     for(int l = 0; l < _nb_locations; l++) {
80       node_from[e] = 1 + (2 * (t + 0) + 0) * _nb_locations + l;
81       node_to[e] =   1 + (2 * (t + 0) + 1) * _nb_locations + l;
82       edge_lengths[e] = - _detection_score[t][l];
83       e++;
84       if(t == _nb_time_steps - 1) {
85         node_from[e] = 1 + (2 * (t + 0) + 1) * _nb_locations + l;
86         node_to[e] =   sink;
87         edge_lengths[e] = 0.0;
88         e++;
89       } else {
90         for(int k = 0; k < _nb_locations; k++) {
91           if(_allowed_motion[l][k]) {
92             node_from[e] = 1 + (2 * (t + 0) + 1) * _nb_locations + l;
93             node_to[e] =   1 + (2 * (t + 1) + 0) * _nb_locations + k;
94             edge_lengths[e] = 0.0;
95             e++;
96           }
97         }
98       }
99     }
100   }
101
102   cerr << "DEBUG e = " << e << " nb_edges = " << nb_edges << endl;
103
104   _graph = new MTPGraph(nb_vertices, nb_edges, node_from, node_to, source, sink);
105
106   int *edge_occupation = new int[nb_edges];
107
108   _graph->find_best_paths(edge_lengths, edge_occupation);
109
110   dot_print(nb_vertices, nb_edges, node_from, node_to, edge_lengths,
111             source, sink,
112             edge_occupation);
113
114   delete[] edge_occupation;
115   delete _graph;
116 }
117
118 // void Tracker::track() {
119   // int e = _nb_locations;
120   // for(int t = 0; t <= _nb_time_steps; t++) {
121     // for(int l = 0; l < _nb_locations; l++) {
122       // edge_lengths[e] = _detection_score[t][l];
123       // e++;
124       // if(t == _nb_time_steps) {
125         // e++;
126       // } else {
127         // e += _nb_locations;
128       // }
129     // }
130   // }
131 // }
132
133 // int Tracker::nb_trajectories() {
134 // }
135
136 // int Tracker::trajectory_start_time(int k) {
137 // }
138
139 // int Tracker::trajectory_end_time(int k) {
140 // }
141
142 // int Tracker::trajectory_location(int k, int time) {
143 // }