Mercurial Hosting > traffic-intelligence
comparison trajectorymanagement/src/Trajectory.h @ 1159:e1e7acef8eab
moved trajectory management library into Traffic Intelligence
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 22 Feb 2021 22:09:35 -0500 |
parents | |
children |
comparison
equal
deleted
inserted
replaced
1158:7eb972942f22 | 1159:e1e7acef8eab |
---|---|
1 #ifndef TRAJECTORY_H_ | |
2 #define TRAJECTORY_H_ | |
3 | |
4 #include "TrajectoryElement.h" | |
5 #include "TrajectoryExceptions.h" | |
6 | |
7 #include "opencv2/core/core.hpp" | |
8 | |
9 #include <ostream> | |
10 #include <vector> | |
11 #include <cassert> | |
12 #include <algorithm> | |
13 | |
14 /** | |
15 * Trajectory class. | |
16 * | |
17 * The Trajectory class is a container to keep information about a trajectory. | |
18 * defined as a sequence of not necessarily consecutive trajectory elements (instand + point coordinates) | |
19 * | |
20 * \todo check (and remove) stuff about ascending frame numbers | |
21 */ | |
22 template<typename Type> | |
23 class Trajectory | |
24 { | |
25 public: | |
26 /// Constructor. | |
27 Trajectory() : id(0), checkAscFrameNumber(false) {} | |
28 | |
29 /** | |
30 * Constructor. | |
31 * | |
32 * @param trajectory trajectory | |
33 */ | |
34 Trajectory(const Trajectory &itrajectory) { | |
35 setId(itrajectory.getId()); | |
36 for (unsigned int i = 0; i < itrajectory.size(); ++i) | |
37 trajectory.push_back(itrajectory.getTrajectoryElement(i)); // justified because we directly copy the first and last instants | |
38 | |
39 setCheckAscFrameNumber(itrajectory.getCheckAscFrameNumber()); | |
40 } | |
41 | |
42 /** | |
43 * Constructor. | |
44 * | |
45 * @param trajectory trajectory | |
46 */ | |
47 Trajectory(const int& id, const std::vector<TrajectoryElement<Type> >& trajectoryElements): | |
48 id(id), checkAscFrameNumber(false) { | |
49 for (typename std::vector<TrajectoryElement<Type> >::const_iterator iter = trajectoryElements.begin(); | |
50 iter != trajectoryElements.end(); iter++) | |
51 add(*iter); | |
52 } | |
53 | |
54 /** | |
55 * Get id of the trajectory. | |
56 * | |
57 * @return id | |
58 */ | |
59 unsigned int getId(void) const { return id;} | |
60 | |
61 /** | |
62 * Set id of the trajectory. | |
63 * | |
64 * @param[in] id id | |
65 */ | |
66 void setId(const unsigned int& iid) { id = iid;} | |
67 | |
68 /** | |
69 * Get element of the trajectory. | |
70 * | |
71 * @param[in] position index of the element of the trajectory | |
72 * @return element of the trajectory#include "OpenCVPointTypeSupport.h" | |
73 */ | |
74 const TrajectoryElement<Type> getTrajectoryElement(unsigned int position) const | |
75 { | |
76 return trajectory[position]; | |
77 } | |
78 | |
79 /** | |
80 * Add an element to the trajectory. | |
81 * | |
82 * @param[in] frameNumber number of the frame of the new element of the trajectory | |
83 * @param[in] point position of the new element of the trajectory | |
84 */ | |
85 void add(unsigned int frameNumber, const Type &point) | |
86 { | |
87 TrajectoryElement<Type> trajectoryElement = TrajectoryElement<Type> (frameNumber, point); | |
88 | |
89 if (getCheckAscFrameNumber()) | |
90 { | |
91 ascFrameNumberAddCheck(frameNumber); | |
92 } | |
93 trajectory.push_back(trajectoryElement); | |
94 } | |
95 | |
96 /** | |
97 * Add an element to the trajectory. | |
98 * | |
99 * @param[in] point position of the new element of the trajectory | |
100 */ | |
101 void add(const Type &point) | |
102 { | |
103 unsigned int frameNumber = 1; | |
104 add(frameNumber, point); | |
105 } | |
106 | |
107 void add(const TrajectoryElement<Type> &trajectoryElement) | |
108 { | |
109 int frameNumber = trajectoryElement.getFrameNumber(); | |
110 if (getCheckAscFrameNumber()) | |
111 { | |
112 ascFrameNumberAddCheck(frameNumber); | |
113 } | |
114 trajectory.push_back(trajectoryElement); | |
115 } | |
116 | |
117 /** | |
118 * Get number of the frame of the element of the trajectory. | |
119 * | |
120 * @param[in] position index of the elesize_t ment of the trajectory | |
121 * @return number of the frame of the element of the trajectory. | |
122 */ | |
123 unsigned int getFrameNumber(unsigned int position) const | |
124 { | |
125 return trajectory[position].getFrameNumber(); | |
126 } | |
127 | |
128 /** | |
129 * Get position of the element of the trajectory. | |
130 * | |
131 * @param[in] position index of the element of the trajectory | |
132 * @return position of the element of the trajectory | |
133 */ | |
134 const Type &getPoint(unsigned int position) const | |
135 { | |
136 return trajectory[position].getPoint(); | |
137 } | |
138 | |
139 /** Get position of the element of the trajectory. */ | |
140 const Type& getPointAtInstant(const unsigned int& t) const | |
141 { | |
142 typename std::vector<TrajectoryElement<Type> >::const_iterator iter = trajectory.begin(); | |
143 while (iter != trajectory.end() && iter->getFrameNumber() != t) | |
144 iter++; | |
145 if (iter != trajectory.end()) | |
146 return iter->getPoint(); | |
147 else { | |
148 throw TrajectoryOutOfRangeErrorException(t); | |
149 } | |
150 } | |
151 | |
152 bool getCheckAscFrameNumber() const | |
153 { | |
154 return checkAscFrameNumber; | |
155 } | |
156 | |
157 void setCheckAscFrameNumber(bool icheckAscFrameNumber) | |
158 { | |
159 if (icheckAscFrameNumber) | |
160 { | |
161 ascFrameNumberCheck(); | |
162 } | |
163 | |
164 checkAscFrameNumber = icheckAscFrameNumber; | |
165 } | |
166 | |
167 /** | |
168 * Get number of elements in the trajectory. | |
169 * | |
170 * @return number of elements in the trajectory | |
171 */ | |
172 unsigned int size() const | |
173 { | |
174 return trajectory.size(); | |
175 } | |
176 | |
177 bool empty() const | |
178 { | |
179 return trajectory.empty(); | |
180 } | |
181 | |
182 void insert(unsigned int position, unsigned int frameNumber, const Type &point) | |
183 { | |
184 trajectoryRangeLeEqCheck(position); | |
185 | |
186 if (getCheckAscFrameNumber()) | |
187 { | |
188 ascFrameNumberInsertCheck(position, frameNumber); | |
189 } | |
190 | |
191 trajectory.insert(trajectory.begin() + position, TrajectoryElement<Type> (frameNumber, point)); | |
192 } | |
193 | |
194 /** | |
195 * Erase an element from the trajectory. | |
196 *TrajectoryFrameNumberErrorException | |
197 * @param[in] position index of the trajectory to be removed | |
198 */ | |
199 void erase(unsigned int position) | |
200 { | |
201 trajectoryRangeLeCheck(position); | |
202 trajectory.erase(trajectory.begin() + position); | |
203 } | |
204 | |
205 /** | |
206 * Erase the last element from the trajectory. | |
207 */ | |
208 void pop_back() | |
209 { | |
210 trajectoryNotEmptyCheck(); | |
211 trajectory.pop_back(); | |
212 } | |
213 | |
214 /** | |
215 * Clear the trajectory. | |
216 */ | |
217 void clear() | |
218 { | |
219 trajectory.clear(); | |
220 } | |
221 | |
222 /** | |
223 * Get position of the element of the trajectory. | |
224 * | |
225 * @param[in] i index of the element of the trajectory | |
226 * @return position of the element of the trajectory | |
227 */ | |
228 Type operator [](unsigned int i) const | |
229 { | |
230 return trajectory[i].getPoint(); | |
231 } | |
232 | |
233 /** | |
234 * Get position of the element of the trajectory. | |
235 * | |
236 * @param[in] i index of the element of the trajectory | |
237 * @return position of the element of the trajectory | |
238 */ | |
239 const Type& at(unsigned int i) const | |
240 { | |
241 trajectoryRangeLeCheck(i); | |
242 return trajectory[i].getPoint(); | |
243 } | |
244 | |
245 /** | |
246 * Shift each element of the trajectory. | |
247 * | |
248 * @param[in] t vector by which the position of each element of the trajectory should be shifted. | |
249 */ | |
250 void shift(const Type& t) | |
251 { | |
252 for (unsigned int i = 0; i < trajectory.size(); ++i) | |
253 { | |
254 trajectory[i].shift(t); | |
255 } | |
256 } | |
257 | |
258 /** Computes the first and last instants */ | |
259 void computeInstants(unsigned int& firstInstant, unsigned int& lastInstant) { | |
260 if (trajectory.empty()) { | |
261 firstInstant = 0; | |
262 lastInstant = 0; | |
263 } else { | |
264 typename std::vector<TrajectoryElement<Type> >::iterator iter = trajectory.begin(); | |
265 firstInstant = iter->getFrameNumber(); | |
266 lastInstant = iter->getFrameNumber(); | |
267 iter++; | |
268 while (iter != trajectory.end()) { | |
269 unsigned int frameNumber = iter->getFrameNumber(); | |
270 if (frameNumber < firstInstant) | |
271 firstInstant = frameNumber; | |
272 else if (frameNumber > lastInstant) | |
273 lastInstant = frameNumber; | |
274 iter++; | |
275 } | |
276 } | |
277 } | |
278 | |
279 /** Smoothes the trajectory positions (parameter is the half-window used for moving average) */ | |
280 void movingAverage(const unsigned int& nFramesSmoothing) { | |
281 if (!trajectory.empty() && nFramesSmoothing >= 1) { | |
282 // todo check the frames are in increasing order | |
283 std::vector<TrajectoryElement<Type> > smoothedTrajectory; | |
284 unsigned int nPositions = trajectory.size(); | |
285 for(unsigned int i=0; i<nPositions; ++i) { | |
286 Type p(0,0); | |
287 unsigned int delta = std::min(nFramesSmoothing, std::min(i, nPositions-1-i)); | |
288 for (unsigned int j=i-delta; j<=i+delta; ++j) | |
289 p = p+getPoint(j); | |
290 smoothedTrajectory.push_back(TrajectoryElement<Type>(trajectory[i].getFrameNumber(), p*(1./(1.+2.*static_cast<float>(delta))))); | |
291 } | |
292 trajectory = smoothedTrajectory; | |
293 } | |
294 } | |
295 | |
296 protected: | |
297 /** | |
298 * Check the length of the trajectories. | |
299 * | |
300 * @param[in] a the size of the trajectory | |
301 * @param[in] b the size of the trajectory | |
302 */ | |
303 void trajectoryNotEmptyCheck() const | |
304 { | |
305 if (trajectory.size() == 0) | |
306 { | |
307 throw TrajectoryLengthErrorException(); | |
308 } | |
309 } | |
310 | |
311 /** | |
312 * Check the range of the trajectory. | |
313 * | |
314 * @param[in] n index of the element of the trajectory | |
315 */ | |
316 void trajectoryRangeLeEqCheck(unsigned int n) const | |
317 { | |
318 if (n > trajectory.size()) | |
319 { | |
320 throw TrajectoryOutOfRangeErrorException(); | |
321 } | |
322 } | |
323 | |
324 /** | |
325 * Check the range of the trajectory. | |
326 * | |
327 * @param[in] n index of the element of the trajectory | |
328 */ | |
329 void trajectoryRangeLeCheck(unsigned int n) const | |
330 { | |
331 if (n >= trajectory.size()) | |
332 { | |
333 throw TrajectoryOutOfRangeErrorException(); | |
334 } | |
335 } | |
336 | |
337 void ascFrameNumberCheck(unsigned int prevFrameNumber, unsigned int currFrameNumber) const | |
338 { | |
339 if (prevFrameNumber >= currFrameNumber) | |
340 { | |
341 throw TrajectoryFrameNumberErrorException(); | |
342 } | |
343 } | |
344 | |
345 void ascFrameNumberCheck() const | |
346 { | |
347 for (unsigned int i = 1; i < trajectory.size(); ++i) | |
348 { | |
349 ascFrameNumberCheck(trajectory[i - 1].getFrameNumber(), trajectory[i].getFrameNumber()); | |
350 } | |
351 } | |
352 | |
353 void ascFrameNumberAddCheck(unsigned int frameNumber) const | |
354 { | |
355 if (!trajectory.empty()) | |
356 { | |
357 ascFrameNumberCheck(trajectory.back().getFrameNumber(), frameNumber); | |
358 } | |
359 } | |
360 | |
361 void ascFrameNumberInsertCheck(unsigned int position, unsigned int frameNumber) const | |
362 { | |
363 if (position > 0) | |
364 { | |
365 ascFrameNumberCheck(trajectory[position - 1].getFrameNumber(), frameNumber); | |
366 } | |
367 | |
368 if (position < trajectory.size()) | |
369 { | |
370 ascFrameNumberCheck(frameNumber, trajectory[position].getFrameNumber()); | |
371 } | |
372 } | |
373 | |
374 private: | |
375 | |
376 /// Id of the trajectory. | |
377 unsigned int id; | |
378 | |
379 /** | |
380 * Trajectory. | |
381 */ | |
382 std::vector<TrajectoryElement<Type> > trajectory; | |
383 | |
384 bool checkAscFrameNumber; | |
385 }; | |
386 | |
387 typedef Trajectory<cv::Point_<int> > TrajectoryPoint2i; | |
388 typedef TrajectoryPoint2i TrajectoryPoint; | |
389 typedef Trajectory<cv::Point_<float> > TrajectoryPoint2f; | |
390 typedef Trajectory<cv::Point_<double> > TrajectoryPoint2d; | |
391 typedef Trajectory<cv::Point3_<int> > TrajectoryPoint3i; | |
392 typedef Trajectory<cv::Point3_<float> > TrajectoryPoint3f; | |
393 typedef Trajectory<cv::Point3_<double> > TrajectoryPoint3d; | |
394 | |
395 /** | |
396 * Compare two trajectories. | |
397 * | |
398 * @param[in] t trajectory trajectory | |
399 * @return information whether trajectories are equal or notPoint_<_Tp> | |
400 */ | |
401 template<typename T> | |
402 static inline bool operator ==(const Trajectory<T>& a, const Trajectory<T>& b) | |
403 { | |
404 if (a.getId() != b.getId() || a.getCheckAscFrameNumber() != b.getCheckAscFrameNumber() || a.size() != b.size()) | |
405 { | |
406 return false; | |
407 } | |
408 | |
409 for (unsigned int i = 0; i < a.size(); ++i) | |
410 { | |
411 if (a.getTrajectoryElement(i) != b.getTrajectoryElement(i)) | |
412 { | |
413 return false; | |
414 } | |
415 } | |
416 | |
417 return true; | |
418 } | |
419 | |
420 template<typename T> | |
421 static inline Trajectory<T>& operator +=(Trajectory<T>& a, const T& b) | |
422 { | |
423 a.add(b); | |
424 return a; | |
425 } | |
426 | |
427 template<typename T> | |
428 static inline Trajectory<T>& operator +=(Trajectory<T>& a, const TrajectoryElement<T>& b) | |
429 { | |
430 a.add(b); | |
431 return a; | |
432 } | |
433 | |
434 template<typename Ti, typename Tr> | |
435 std::basic_istream<Ti>& operator>>(std::basic_istream<Ti>& in, Trajectory<Tr>& trajectory) | |
436 { | |
437 unsigned int id; | |
438 if (in >> id) | |
439 { | |
440 trajectory.setId(id); | |
441 TrajectoryElement<Tr> trajectoryElement; | |
442 while (in >> trajectoryElement) | |
443 { | |
444 trajectory.add(trajectoryElement); | |
445 } | |
446 } | |
447 return in; | |
448 } | |
449 | |
450 template<typename Tr> | |
451 std::ostream& operator<<(std::ostream& out, const Trajectory<Tr>& trajectory) | |
452 { | |
453 out << trajectory.getId(); | |
454 for (unsigned int i = 0; i < trajectory.size(); ++i) | |
455 { | |
456 out << " "; | |
457 out << trajectory.getTrajectoryElement(i); | |
458 } | |
459 return out; | |
460 } | |
461 | |
462 /** | |
463 * Compute the minimum and the maximum position of the trajectory. | |
464 * | |
465 * @param[out] _min minimum position of the trajectory | |
466 * @param[out] _max maximum position of the trajectory | |
467 */ | |
468 template<typename T> | |
469 static inline T min(const Trajectory<T>& trajectory) | |
470 { | |
471 if (trajectory.empty()) | |
472 { | |
473 throw TrajectoryLengthErrorException(); | |
474 } | |
475 | |
476 T point = trajectory.getPoint(0); | |
477 | |
478 for (unsigned int i = 1; i < trajectory.size(); ++i) | |
479 { | |
480 point = min(point, trajectory.getPoint(i)); | |
481 } | |
482 | |
483 return point; | |
484 } | |
485 | |
486 template<typename T> | |
487 static inline T max(const Trajectory<T>& trajectory) | |
488 { | |
489 if (trajectory.empty()) | |
490 { | |
491 throw TrajectoryLengthErrorException(); | |
492 } | |
493 | |
494 T point = trajectory.getPoint(0); | |
495 | |
496 for (unsigned int i = 1; i < trajectory.size(); ++i) | |
497 { | |
498 point = max(point, trajectory.getPoint(i)); | |
499 } | |
500 | |
501 return point; | |
502 } | |
503 | |
504 #endif /* TRAJECTORY_H_ */ |