Mercurial Hosting > traffic-intelligence
annotate c/Motion.cpp @ 176:9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 27 Oct 2011 13:56:46 -0400 |
parents | cde87a07eb58 |
children | ae2286b1a3fd |
rev | line source |
---|---|
136
0f790de9437e
renamed Feature to Motion files and added code to test blob db
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
135
diff
changeset
|
1 #include "Motion.hpp" |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
2 #include "cvutils.hpp" |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
3 |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
4 #include "opencv2/core/core.hpp" |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
5 #include "opencv2/highgui/highgui.hpp" |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
6 |
133
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
7 #include "src/TrajectoryDBAccessList.h" |
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
8 |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
9 using namespace std; |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
10 using namespace cv; |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
11 |
163
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
12 /******************** FeatureTrajectory ********************/ |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
13 |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
14 FeatureTrajectory::FeatureTrajectory(const int& frameNum, const cv::Point2f& p, const Mat& homography) |
cde87a07eb58
added graph structures
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
147
diff
changeset
|
15 : lost(false) { |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
16 positions = TrajectoryPoint2fPtr(new TrajectoryPoint2f()); |
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
17 velocities = TrajectoryPoint2fPtr(new TrajectoryPoint2f()); |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
18 addPoint(frameNum, p, homography); |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
19 } |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
20 |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
21 bool FeatureTrajectory::isDisplacementSmall(const unsigned int& nDisplacements, const float& minTotalFeatureDisplacement) const { |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
137
diff
changeset
|
22 bool result = false; |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
23 unsigned int nPositions = positions->size(); |
135
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
24 if (nPositions > nDisplacements) { |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
25 float disp = 0; |
137
445e773c9be3
created the parameter structure to parse parameters (bug remaining)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
136
diff
changeset
|
26 for (unsigned int i=0; i<nDisplacements; i++) |
135
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
27 disp += displacementDistances[nPositions-2-i]; |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
137
diff
changeset
|
28 result = disp < minTotalFeatureDisplacement; |
135
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
29 } |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
30 return result; |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
31 } |
32d2722d4028
added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
133
diff
changeset
|
32 |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
33 bool FeatureTrajectory::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
|
34 bool result = true; |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
35 unsigned int nPositions = positions->size(); |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
36 if (nPositions >= 3) { |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
37 float ratio; |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
38 if (displacementDistances[nPositions-2] > displacementDistances[nPositions-3]) |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
39 ratio = displacementDistances[nPositions-2] / displacementDistances[nPositions-3]; |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
40 else |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
41 ratio = displacementDistances[nPositions-3] / displacementDistances[nPositions-2]; |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
42 |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
43 float cosine = scalarProduct((*velocities)[nPositions-3],(*velocities)[nPositions-2]) / (displacementDistances[nPositions-3] * displacementDistances[nPositions-2]); |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
44 |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
45 result &= (ratio < accelerationBound) & (cosine > deviationBound); |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
46 } |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
47 return result; |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
48 } |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
49 |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
50 void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p, const Mat& homography) { |
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
51 Point2f pp = p; |
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
52 if (!homography.empty()) |
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
53 pp = project(p, homography); |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
54 positions->add(frameNum, pp); |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
55 computeMotionData(frameNum); |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
56 assert(positions.size() == displacementDistances.size()+1); |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
57 assert(positions.size() == velocities.size()+1); |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
58 } |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
59 |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
137
diff
changeset
|
60 void FeatureTrajectory::shorten(void) { |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
61 positions->pop_back(); |
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
62 velocities->pop_back(); |
138
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
137
diff
changeset
|
63 displacementDistances.pop_back(); |
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
137
diff
changeset
|
64 } |
c1b260b48d2a
corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
137
diff
changeset
|
65 |
142
a3532db00c28
added code to write velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
139
diff
changeset
|
66 void FeatureTrajectory::write(TrajectoryDBAccess<Point2f>& trajectoryDB, const string& positionsTableName, const string& velocitiesTableName) const { |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
67 trajectoryDB.write(*positions, positionsTableName); |
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
68 trajectoryDB.write(*velocities, velocitiesTableName); |
133
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
69 } |
63dd4355b6d1
saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
132
diff
changeset
|
70 |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
71 #ifdef USE_OPENCV |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
72 /// \todo add option for anti-aliased drawing, thickness |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
73 void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Scalar& color) const { |
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
74 Point2f p1, p2; |
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
75 if (!homography.empty()) |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
76 p1 = project((*positions)[0], homography); |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
77 else |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
78 p1 = (*positions)[0]; |
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
79 for (unsigned int i=1; i<positions->size(); i++) { |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
80 if (!homography.empty()) |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
81 p2 = project((*positions)[i], homography); |
147
0089fb29cd26
added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
142
diff
changeset
|
82 else |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
83 p2 = (*positions)[i]; |
132
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
84 line(img, p1, p2, color, 1); |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
85 p1 = p2; |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
86 } |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
87 } |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
88 #endif |
45c64e68053c
added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
129
diff
changeset
|
89 |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
90 // protected |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
91 |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
92 void FeatureTrajectory::computeMotionData(const int& frameNum) { |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
93 unsigned int nPositions = positions->size(); |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
94 if (nPositions >= 2) { |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
95 Point2f displacement = (*positions)[nPositions-1] - (*positions)[nPositions-2]; |
139
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
96 //if (nPositions == 2) // duplicate first displacement so that positions and velocities have the same length |
47329bd16cc0
cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
138
diff
changeset
|
97 //velocities.add(frameNum-1, displacement); |
176
9323427aa0a3
changed positions and velocities to shared pointers and renamed methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
163
diff
changeset
|
98 velocities->add(frameNum, displacement); |
129
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
99 float dist = norm(displacement); |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
100 displacementDistances.push_back(dist); |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
101 } |
4742b2b6d851
created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
102 } |
163
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 /******************** FeatureGraph ********************/ |