Mercurial Hosting > traffic-intelligence

annotate trajectorymanagement/src/Trajectory.h @ 1250:77fbd0e2ba7d

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