Mercurial Hosting > traffic-intelligence

annotate include/Motion.hpp @ 180:3a4eef37384f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
method to add features and vertices to graph
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 01 Nov 2011 00:12:33 -0400
parents 4f10e97cb677
children 6c48283a78ca
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
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) {}
180
3a4eef37384f method to add features and vertices to graph
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 179
diff changeset
91 FeatureGraph(float _connectionDistance, float _segmentationDistance, unsigned int _minFeatureTime) : connectionDistance (_connectionDistance), segmentationDistance(_segmentationDistance), minFeatureTime(_minFeatureTime) {}
179
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
d7df8ecf5ccd next steps
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 177
diff changeset
95 // add vertex, includes adding links to current vertices
d7df8ecf5ccd next steps
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 177
diff changeset
96 // find connected components, check if old enough, if so, remove
d7df8ecf5ccd next steps
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 177
diff changeset
97
180
3a4eef37384f method to add features and vertices to graph
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 179
diff changeset
98 std::string informationString(void);
3a4eef37384f method to add features and vertices to graph
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 179
diff changeset
99
163
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
100 protected:
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
101 struct FeatureConnection {
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
102 float minDistance;
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
103 float maxDistance;
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
104 };
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
105
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
106 struct VertexInformation {
179
4f10e97cb677 added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 178
diff changeset
107 FeatureTrajectoryPtr feature;
163
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
108 };
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
109
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
110 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
111
179
4f10e97cb677 added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 178
diff changeset
112 float connectionDistance;
4f10e97cb677 added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 178
diff changeset
113 float segmentationDistance;
180
3a4eef37384f method to add features and vertices to graph
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 179
diff changeset
114 unsigned int minFeatureTime;
179
4f10e97cb677 added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 178
diff changeset
115 // float minDistance;
4f10e97cb677 added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 178
diff changeset
116 // float maxDistance;
163
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
117
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
118 UndirectedGraph graph;
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
119
179
4f10e97cb677 added getting first and last instant for each feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 178
diff changeset
120 //std::vector<UndirectedGraph::vertex_descriptor> currentVertices, lostVertices;
163
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
121 };
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
122
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
123 // inlined implementations
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
124 // inline FeatureGraph::FeatureGraph(float _minDistance, float _maxDistance)
cde87a07eb58 added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 147
diff changeset
125
119
45a426552aaa compilation of very simple feature class with trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
126 #endif