Mercurial Hosting > traffic-intelligence
annotate include/Motion.hpp @ 179:4f10e97cb677
added getting first and last instant for each feature
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 31 Oct 2011 19:17:42 -0400 |
parents | d7df8ecf5ccd |
children | 3a4eef37384f |
rev | line source |
---|---|
177
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
1 #ifndef MOTION_HPP |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
2 #define MOTION_HPP |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
3 |
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
4 #include "src/Trajectory.h" |
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
5 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
6 #include <boost/shared_ptr.hpp> |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
7 #include <boost/graph/adjacency_list.hpp> |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
8 |
133
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
9 template<typename T> class TrajectoryDBAccess; |
177
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
10 template<typename T> class TrajectoryDBAccessList; |
133
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
11 |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
12 typedef boost::shared_ptr<TrajectoryPoint2f> TrajectoryPoint2fPtr; |
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
13 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
14 /** Class for feature data |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
15 positions, velocities and other statistics to evaluate their quality |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
16 before saving. */ |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
17 class FeatureTrajectory { |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
18 public: |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
19 FeatureTrajectory(const int& frameNum, const cv::Point2f& p, const cv::Mat& homography); |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
20 |
177
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
21 FeatureTrajectory(TrajectoryPoint2fPtr& _positions, TrajectoryPoint2fPtr& _velocities); |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
22 |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
23 /** loads from database |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
24 can be made generic for different list and blob */ |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
25 FeatureTrajectory(const int& id, TrajectoryDBAccessList<cv::Point2f>& trajectoryDB, const std::string& positionsTableName, const std::string& velocitiesTableName); |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
26 |
179
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
27 unsigned int length(void) const { return positions->size();} // cautious if not continuous: max-min+1 |
134
a617d0808bbc
added test on feature length and display control
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
28 |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
29 void setId(const unsigned int& id) { positions->setId(id);velocities->setId(id);} |
133
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
30 |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
31 void setLost(void) { lost = true;} |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
32 bool isLost(void) { return lost;} |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
33 |
179
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
34 unsigned int getFirstInstant(void) {return firstInstant;} |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
35 unsigned int getLastInstant(void) {return lastInstant;} |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
36 |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
37 /// indicates whether the sum of the last nDisplacements displacements has been inferior to minFeatureDisplacement |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
38 bool isDisplacementSmall(const unsigned int& nDisplacements, const float& minTotalFeatureDisplacement) const; |
135
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
39 |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
40 /// indicates whether the last two displacements are smooth (limited acceleration and angle) |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
41 bool isMotionSmooth(const int& accelerationBound, const int& deviationBound) const; |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
42 |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
43 void addPoint(const int& frameNum, const cv::Point2f& p, const cv::Mat& homography); |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
44 |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
45 void shorten(void); |
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
46 |
165
50964af05a80
solved issue with header inclusion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
47 void write(TrajectoryDBAccess<cv::Point2f>& trajectoryDB, const std::string& positionsTableName, const std::string& velocitiesTableName) const; |
133
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
48 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
49 #ifdef USE_OPENCV |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
50 void draw(cv::Mat& img, const cv::Mat& homography, const cv::Scalar& color) const; |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
51 #endif |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
52 |
177
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
53 friend std::stringstream& operator<<(std::stringstream& out, const FeatureTrajectory& ft); |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
54 |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
55 protected: |
179
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
56 bool lost; /// \todo remove |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
57 /// first frame number |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
58 unsigned int firstInstant; |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
59 /// last frame number |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
60 unsigned int lastInstant; |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
61 |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
62 TrajectoryPoint2fPtr positions; |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
63 /** one fewer velocity than position |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
64 v_n = p_n+1 - p_n*/ |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
65 TrajectoryPoint2fPtr velocities; |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
66 |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
67 /// norms of velocities for feature constraints, one fewer positions than positions |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
68 std::vector<float> displacementDistances; |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
69 |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
70 void computeMotionData(const int& frameNum); |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
71 }; |
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
72 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
73 typedef boost::shared_ptr<FeatureTrajectory> FeatureTrajectoryPtr; |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
74 |
177
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
75 // inlined |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
76 inline std::stringstream& operator<<(std::stringstream& out, const FeatureTrajectory& ft) { |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
77 out << *(ft.positions); |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
78 out << "\n"; |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
79 out << *(ft.velocities); |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
80 return out; |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
81 } |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
82 |
135
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
83 // class MovingObject {} |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
84 // roadUserType, group of features |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
85 |
179
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
86 /** Class to group features: Beymer et al. 99/Saunier and Sayed 06 |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
87 \todo create various graph types with different parameters, that accept different feature distances or ways to connect and segment features */ |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
88 class FeatureGraph { |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
89 public: |
179
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
90 //FeatureGraph(float _minDistance, float _maxDistance) : minDistance (_minDistance), maxDistance(_maxDistance) {} |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
91 FeatureGraph(float _connectionDistance, float _segmentationDistance) : connectionDistance (_connectionDistance), segmentationDistance(_segmentationDistance) {} |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
92 |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
93 void addFeature(const FeatureTrajectoryPtr& ft); |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
94 |
178 | 95 // add vertex, includes adding links to current vertices |
96 // find connected components, check if old enough, if so, remove | |
97 | |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
98 protected: |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
99 struct FeatureConnection { |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
100 float minDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
101 float maxDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
102 }; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
103 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
104 struct VertexInformation { |
179
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
105 FeatureTrajectoryPtr feature; |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
106 }; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
107 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
108 typedef boost::adjacency_list <boost::listS, boost::listS, boost::undirectedS, VertexInformation, FeatureConnection> UndirectedGraph; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
109 |
179
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
110 float connectionDistance; |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
111 float segmentationDistance; |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
112 // float minDistance; |
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
113 // float maxDistance; |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
114 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
115 UndirectedGraph graph; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
116 |
179
4f10e97cb677
added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
178
diff
changeset
|
117 //std::vector<UndirectedGraph::vertex_descriptor> currentVertices, lostVertices; |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
118 }; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
119 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
120 // inlined implementations |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
121 // inline FeatureGraph::FeatureGraph(float _minDistance, float _maxDistance) |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
122 |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
123 #endif |