Mercurial Hosting > traffic-intelligence
annotate include/Motion.hpp @ 164:76610dcf3b8d
added test code to read trajectories
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 28 Sep 2011 13:27:20 -0400 |
parents | cde87a07eb58 |
children | 50964af05a80 |
rev | line source |
---|---|
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
1 #ifndef FEATURE_HPP |
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
2 #define FEATURE_HPP |
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; |
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
10 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
11 /** Class for feature data |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
12 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
|
13 before saving. */ |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
14 class FeatureTrajectory { |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
15 public: |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
16 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
|
17 |
134
a617d0808bbc
added test on feature length and display control
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
18 unsigned int length(void) const { return positions.size();} |
a617d0808bbc
added test on feature length and display control
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
19 |
133
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
20 void setId(const unsigned int& id) { positions.setId(id);velocities.setId(id);} |
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
21 |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
22 void setLost(void) { lost = true;} |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
23 bool isLost(void) { return lost;} |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
24 |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
25 /// indicates whether the sum of the last nDisplacements displacements has been inferior to minFeatureDisplacement |
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
26 bool smallDisplacement(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
|
27 |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
28 /// indicates whether the last two displacements are smooth (limited acceleration and angle) |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
29 bool motionSmooth(const int& accelerationBound, const int& deviationBound) const; |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
30 |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
31 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
|
32 |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
33 void shorten(void); |
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
34 |
142
a3532db00c28
added code to write velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
139
diff
changeset
|
35 void write(TrajectoryDBAccess<cv::Point2f>& trajectoryDB, const string& positionsTableName, const string& velocitiesTableName) const; |
133
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
36 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
37 #ifdef USE_OPENCV |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
38 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
|
39 #endif |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
40 |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
41 protected: |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
42 bool lost; |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
43 Trajectory<cv::Point2f> positions; |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
44 /** one fewer velocity than position |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
45 v_n = p_n+1 - p_n*/ |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
46 Trajectory<cv::Point2f> velocities; |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
47 |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
48 /// 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
|
49 std::vector<float> displacementDistances; |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
50 |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
51 void computeMotionData(const int& frameNum); |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
52 |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
53 }; |
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
54 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
55 typedef boost::shared_ptr<FeatureTrajectory> FeatureTrajectoryPtr; |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
56 |
135
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
57 // class MovingObject {} |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
58 // roadUserType, group of features |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
59 |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
60 /// Class to group features |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
61 class FeatureGraph { |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
62 public: |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
63 FeatureGraph(float _minDistance, float _maxDistance) : minDistance (_minDistance), maxDistance(_maxDistance) {} |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
64 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
65 protected: |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
66 struct FeatureConnection { |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
67 float minDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
68 float maxDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
69 }; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
70 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
71 struct VertexInformation { |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
72 boost::shared_ptr<FeatureTrajectory> feature; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
73 }; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
74 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
75 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
|
76 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
77 float minDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
78 float maxDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
79 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
80 UndirectedGraph graph; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
81 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
82 std::vector<UndirectedGraph::vertex_descriptor> currentVertices, lostVertices; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
83 }; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
84 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
85 // inlined implementations |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
86 // inline FeatureGraph::FeatureGraph(float _minDistance, float _maxDistance) |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
87 |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
88 #endif |