Cosmetics.
[mtp.git] / mtp_tracker.cc
1
2 /*
3  *  mtp is the ``Multi Tracked Paths'', an implementation of the
4  *  k-shortest paths algorithm for multi-target tracking.
5  *
6  *  Copyright (c) 2012 Idiap Research Institute, http://www.idiap.ch/
7  *  Written by Francois Fleuret <francois.fleuret@idiap.ch>
8  *
9  *  This file is part of mtp.
10  *
11  *  mtp is free software: you can redistribute it and/or modify it
12  *  under the terms of the GNU General Public License version 3 as
13  *  published by the Free Software Foundation.
14  *
15  *  mtp is distributed in the hope that it will be useful, but WITHOUT
16  *  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
17  *  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
18  *  License for more details.
19  *
20  *  You should have received a copy of the GNU General Public License
21  *  along with selector.  If not, see <http://www.gnu.org/licenses/>.
22  *
23  */
24
25 #include "mtp_tracker.h"
26
27 #include <iostream>
28
29 using namespace std;
30
31 void MTPTracker::free() {
32   delete[] _edge_lengths;
33   delete _graph;
34   deallocate_array<scalar_t>(detection_scores);
35   deallocate_array<int>(allowed_motions);
36   deallocate_array<int>(exits);
37   deallocate_array<int>(entrances);
38 }
39
40 void MTPTracker::allocate(int t, int l) {
41   free();
42
43   nb_locations = l;
44   nb_time_steps = t;
45
46   detection_scores = allocate_array<scalar_t>(nb_time_steps, nb_locations);
47   allowed_motions = allocate_array<int>(nb_locations, nb_locations);
48
49   entrances = allocate_array<int>(nb_time_steps, nb_locations);
50   exits = allocate_array<int>(nb_time_steps, nb_locations);
51
52   for(int l = 0; l < nb_locations; l++) {
53     for(int m = 0; m < nb_locations; m++) {
54       allowed_motions[l][m] = 0;
55     }
56   }
57
58   for(int t = 0; t < nb_time_steps; t++) {
59     for(int l = 0; l < nb_locations; l++) {
60       detection_scores[t][l] = 0.0;
61       entrances[t][l] = 0;
62       exits[t][l] = 0;
63     }
64   }
65
66   _edge_lengths = 0;
67   _graph = 0;
68 }
69
70 void MTPTracker::write(ostream *os) {
71   (*os) << nb_locations << " " << nb_time_steps << endl;
72
73   (*os) << endl;
74
75   for(int l = 0; l < nb_locations; l++) {
76     for(int m = 0; m < nb_locations; m++) {
77       (*os) << allowed_motions[l][m];
78       if(m < nb_locations - 1) (*os) << " "; else (*os) << endl;
79     }
80   }
81
82   (*os) << endl;
83
84   for(int t = 0; t < nb_time_steps; t++) {
85     for(int l = 0; l < nb_locations; l++) {
86       (*os) << entrances[t][l];
87       if(l < nb_locations - 1) (*os) << " "; else (*os) << endl;
88     }
89   }
90
91   (*os) << endl;
92
93   for(int t = 0; t < nb_time_steps; t++) {
94     for(int l = 0; l < nb_locations; l++) {
95       (*os) << exits[t][l];
96       if(l < nb_locations - 1) (*os) << " "; else (*os) << endl;
97     }
98   }
99
100   (*os) << endl;
101
102   for(int t = 0; t < nb_time_steps; t++) {
103     for(int l = 0; l < nb_locations; l++) {
104       (*os) << detection_scores[t][l];
105       if(l < nb_locations - 1) (*os) << " "; else (*os) << endl;
106     }
107   }
108 }
109
110 void MTPTracker::read(istream *is) {
111   int l = 0, t = 0;
112
113   (*is) >> l >> t;
114
115   allocate(t, l);
116
117   for(int l = 0; l < nb_locations; l++) {
118     for(int m = 0; m < nb_locations; m++) {
119       (*is) >> allowed_motions[l][m];
120     }
121   }
122
123   for(int t = 0; t < nb_time_steps; t++) {
124     for(int l = 0; l < nb_locations; l++) {
125       (*is) >> entrances[t][l];
126     }
127   }
128
129   for(int t = 0; t < nb_time_steps; t++) {
130     for(int l = 0; l < nb_locations; l++) {
131       (*is) >> exits[t][l];
132     }
133   }
134
135   for(int t = 0; t < nb_time_steps; t++) {
136     for(int l = 0; l < nb_locations; l++) {
137       (*is) >> detection_scores[t][l];
138     }
139   }
140 }
141
142 void MTPTracker::write_trajectories(ostream *os) {
143   (*os) << nb_trajectories() << endl;
144   for(int t = 0; t < nb_trajectories(); t++) {
145     (*os) << t
146          << " " << trajectory_entrance_time(t)
147          << " " << trajectory_duration(t)
148          << " " << trajectory_score(t);
149     for(int u = 0; u < trajectory_duration(t); u++) {
150       (*os) << " " << trajectory_location(t, u);
151     }
152     (*os) << endl;
153   }
154 }
155
156 MTPTracker::MTPTracker() {
157   nb_locations = 0;
158   nb_time_steps = 0;
159
160   detection_scores = 0;
161   allowed_motions = 0;
162
163   entrances = 0;
164   exits = 0;
165
166   _edge_lengths = 0;
167   _graph = 0;
168 }
169
170 MTPTracker::~MTPTracker() {
171   delete[] _edge_lengths;
172   delete _graph;
173   deallocate_array<scalar_t>(detection_scores);
174   deallocate_array<int>(allowed_motions);
175   deallocate_array<int>(entrances);
176   deallocate_array<int>(exits);
177 }
178
179 int MTPTracker::early_pair_node(int t, int l) {
180   return 1 + (2 * t + 0) * nb_locations + l;
181 }
182
183 int MTPTracker::late_pair_node(int t, int l) {
184   return 1 + (2 * t + 1) * nb_locations + l;
185 }
186
187 void MTPTracker::build_graph() {
188   // Delete the existing graph if there was one
189   delete[] _edge_lengths;
190   delete _graph;
191
192   int nb_motions = 0, nb_exits = 0, nb_entrances = 0;
193
194   for(int l = 0; l < nb_locations; l++) {
195     for(int t = 0; t < nb_time_steps; t++) {
196       if(exits[t][l]) nb_exits++;
197       if(entrances[t][l]) nb_entrances++;
198     }
199     for(int m = 0; m < nb_locations; m++) {
200       if(allowed_motions[l][m]) nb_motions++;
201     }
202   }
203
204   int nb_vertices = 2 + 2 * nb_time_steps * nb_locations;
205
206   int nb_edges =
207     // The edges from the source to the entrances and from the exits
208     // to the sink
209     nb_exits + nb_entrances +
210     // The edges for the motions, between every successive frames
211     (nb_time_steps - 1) * nb_motions +
212     // The edges inside the duplicated nodes
213     nb_locations * nb_time_steps;
214
215   int *node_from = new int[nb_edges];
216   int *node_to = new int[nb_edges];
217
218   int source = 0, sink = nb_vertices - 1;
219   int e = 0;
220
221   _edge_lengths = new scalar_t[nb_edges];
222
223   // We put the in-node edges first, since these are the ones whose
224   // lengths we will have to change before tracking, according to the
225   // detection scores
226
227   for(int t = 0; t < nb_time_steps; t++) {
228     for(int l = 0; l < nb_locations; l++) {
229       node_from[e] = early_pair_node(t, l);
230       node_to[e] = late_pair_node(t, l);
231       e++;
232     }
233   }
234
235   // The edges between frames, corresponding to allowed motions
236
237   for(int t = 0; t < nb_time_steps - 1; t++) {
238     for(int l = 0; l < nb_locations; l++) {
239       for(int k = 0; k < nb_locations; k++) {
240         if(allowed_motions[l][k]) {
241           node_from[e] = late_pair_node(t, l);
242           node_to[e] = early_pair_node(t+1, k);
243           _edge_lengths[e] = 0.0;
244           e++;
245         }
246       }
247     }
248   }
249
250   // The edges from the source to the entrances, and from the exits to
251   // the sink
252
253   for(int t = 0; t < nb_time_steps; t++) {
254     for(int l = 0; l < nb_locations; l++) {
255       if(entrances[t][l]) {
256         node_from[e] = source;
257         node_to[e] = early_pair_node(t, l);
258         _edge_lengths[e] = 0.0;
259         e++;
260       }
261       if(exits[t][l]) {
262         node_from[e] = late_pair_node(t, l);
263         node_to[e] = sink;
264         _edge_lengths[e] = 0.0;
265         e++;
266       }
267     }
268   }
269
270   // We are done, build the graph
271
272   _graph = new MTPGraph(nb_vertices, nb_edges,
273                         node_from, node_to,
274                         source, sink);
275
276   delete[] node_from;
277   delete[] node_to;
278 }
279
280 void MTPTracker::print_graph_dot(ostream *os) {
281   int e = 0;
282   for(int t = 0; t < nb_time_steps; t++) {
283     for(int l = 0; l < nb_locations; l++) {
284       _edge_lengths[e++] = - detection_scores[t][l];
285     }
286   }
287   _graph->print_dot(os);
288 }
289
290 void MTPTracker::track() {
291   ASSERT(_graph);
292
293   int e = 0;
294   for(int t = 0; t < nb_time_steps; t++) {
295     for(int l = 0; l < nb_locations; l++) {
296       _edge_lengths[e++] = - detection_scores[t][l];
297     }
298   }
299
300   _graph->find_best_paths(_edge_lengths);
301   _graph->retrieve_disjoint_paths();
302
303 #ifdef VERBOSE
304   for(int p = 0; p < _graph->nb_paths; p++) {
305     Path *path = _graph->paths[p];
306     cout << "PATH " << p << " [length " << path->nb_nodes << "] " << path->nodes[0];
307     for(int n = 1; n < path->nb_nodes; n++) {
308       cout << " -> " << path->nodes[n];
309     }
310     cout << endl;
311   }
312 #endif
313 }
314
315 int MTPTracker::nb_trajectories() {
316   return _graph->nb_paths;
317 }
318
319 scalar_t MTPTracker::trajectory_score(int k) {
320   return -_graph->paths[k]->length;
321 }
322
323 int MTPTracker::trajectory_entrance_time(int k) {
324   return (_graph->paths[k]->nodes[1] - 1) / (2 * nb_locations);
325 }
326
327 int MTPTracker::trajectory_duration(int k) {
328   return (_graph->paths[k]->nb_nodes - 2) / 2;
329 }
330
331 int MTPTracker::trajectory_location(int k, int time_from_entry) {
332   return (_graph->paths[k]->nodes[2 * time_from_entry + 1] - 1) % nb_locations;
333 }