Mercurial Hosting > traffic-intelligence
annotate include/Motion.hpp @ 177:ae2286b1a3fd
added loading FeatureTrajectory from database, printing
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Sun, 30 Oct 2011 11:25:51 -0400 |
parents | 9323427aa0a3 |
children | d7df8ecf5ccd |
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 |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
27 unsigned int length(void) const { return positions->size();} |
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 |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
34 /// 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
|
35 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
|
36 |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
37 /// 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
|
38 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
|
39 |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
40 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
|
41 |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
42 void shorten(void); |
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
43 |
165
50964af05a80
solved issue with header inclusion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
44 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
|
45 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
46 #ifdef USE_OPENCV |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
47 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
|
48 #endif |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
49 |
177
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
50 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
|
51 |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
52 protected: |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
53 bool lost; |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
165
diff
changeset
|
54 TrajectoryPoint2fPtr positions; |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
55 /** one fewer velocity than position |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
56 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
|
57 TrajectoryPoint2fPtr velocities; |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
58 |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
59 /// 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
|
60 std::vector<float> displacementDistances; |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
61 |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
119
diff
changeset
|
62 void computeMotionData(const int& frameNum); |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
63 }; |
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
64 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
65 typedef boost::shared_ptr<FeatureTrajectory> FeatureTrajectoryPtr; |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
66 |
177
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
67 // inlined |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
68 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
|
69 out << *(ft.positions); |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
70 out << "\n"; |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
71 out << *(ft.velocities); |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
72 return out; |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
73 } |
ae2286b1a3fd
added loading FeatureTrajectory from database, printing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
176
diff
changeset
|
74 |
135
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
75 // class MovingObject {} |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
76 // roadUserType, group of features |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
134
diff
changeset
|
77 |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
78 /// Class to group features |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
79 class FeatureGraph { |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
80 public: |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
81 FeatureGraph(float _minDistance, float _maxDistance) : minDistance (_minDistance), maxDistance(_maxDistance) {} |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
82 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
83 protected: |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
84 struct FeatureConnection { |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
85 float minDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
86 float maxDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
87 }; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
88 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
89 struct VertexInformation { |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
90 boost::shared_ptr<FeatureTrajectory> feature; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
91 }; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
92 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
93 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
|
94 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
95 float minDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
96 float maxDistance; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
97 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
98 UndirectedGraph graph; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
99 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
100 std::vector<UndirectedGraph::vertex_descriptor> currentVertices, lostVertices; |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
101 }; |
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 // inlined implementations |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
104 // inline FeatureGraph::FeatureGraph(float _minDistance, float _maxDistance) |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
105 |
119
45a426552aaa
compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
106 #endif |