Mercurial Hosting > traffic-intelligence

annotate c/Motion.cpp @ 163:cde87a07eb58

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added graph structures
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 27 Sep 2011 01:38:05 -0400
parents 0089fb29cd26
children 9323427aa0a3
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
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) {
147
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
16 addPoint(frameNum, p, homography);
132
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
17 }
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
18
138
c1b260b48d2a corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 137
diff changeset
19 bool FeatureTrajectory::smallDisplacement(const unsigned int& nDisplacements, const float& minTotalFeatureDisplacement) const {
c1b260b48d2a corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 137
diff changeset
20 bool result = false;
135
32d2722d4028 added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 133
diff changeset
21 unsigned int nPositions = positions.size();
32d2722d4028 added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 133
diff changeset
22 if (nPositions > nDisplacements) {
32d2722d4028 added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 133
diff changeset
23 float disp = 0;
137
445e773c9be3 created the parameter structure to parse parameters (bug remaining)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 136
diff changeset
24 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
25 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
26 result = disp < minTotalFeatureDisplacement;
135
32d2722d4028 added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 133
diff changeset
27 }
32d2722d4028 added constraint on minimum displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 133
diff changeset
28 return result;
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
139
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
31 bool FeatureTrajectory::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
32 bool result = true;
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
33 unsigned int nPositions = positions.size();
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
34 if (nPositions >= 3) {
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
35 float ratio;
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
36 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
37 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
38 else
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
39 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
40
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
41 float cosine = scalarProduct(velocities[nPositions-3],velocities[nPositions-2]) / (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
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
43 result &= (ratio < accelerationBound) & (cosine > deviationBound);
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 return result;
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
147
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
48 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
49 Point2f pp = p;
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
50 if (!homography.empty())
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
51 pp = project(p, homography);
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
52 positions.add(frameNum, pp);
129
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
53 computeMotionData(frameNum);
139
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
54 assert(positions.size() == displacementDistances.size()+1);
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
55 assert(positions.size() == velocities.size()+1);
129
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
56 }
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
57
138
c1b260b48d2a corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 137
diff changeset
58 void FeatureTrajectory::shorten(void) {
c1b260b48d2a corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 137
diff changeset
59 positions.pop_back();
c1b260b48d2a corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 137
diff changeset
60 velocities.pop_back();
c1b260b48d2a corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 137
diff changeset
61 displacementDistances.pop_back();
c1b260b48d2a corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 137
diff changeset
62 }
c1b260b48d2a corrected initialization bugs and feature shortening before saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 137
diff changeset
63
142
a3532db00c28 added code to write velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 139
diff changeset
64 void FeatureTrajectory::write(TrajectoryDBAccess<Point2f>& trajectoryDB, const string& positionsTableName, const string& velocitiesTableName) const {
a3532db00c28 added code to write velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 139
diff changeset
65 trajectoryDB.write(positions, positionsTableName);
a3532db00c28 added code to write velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 139
diff changeset
66 trajectoryDB.write(velocities, velocitiesTableName);
133
63dd4355b6d1 saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 132
diff changeset
67 }
63dd4355b6d1 saving of feature positions in sqlite database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 132
diff changeset
68
132
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
69 #ifdef USE_OPENCV
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
70 /// \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
71 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
72 Point2f p1, p2;
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
73 if (!homography.empty())
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
74 p1 = project(positions[0], homography);
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
75 else
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
76 p1 = positions[0];
132
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
77 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
78 if (!homography.empty())
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
79 p2 = project(positions[i], homography);
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
80 else
0089fb29cd26 added projection of points and reprojection for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 142
diff changeset
81 p2 = positions[i];
132
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
82 line(img, p1, p2, color, 1);
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
83 p1 = p2;
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
84 }
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
85 }
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
86 #endif
45c64e68053c added drawing function for features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 129
diff changeset
87
129
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
88 // protected
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
89
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
90 void FeatureTrajectory::computeMotionData(const int& frameNum) {
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
91 unsigned int nPositions = positions.size();
139
47329bd16cc0 cleaned code, added condition on smooth displacement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 138
diff changeset
92 if (nPositions >= 2) {
129
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
93 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
94 //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
95 //velocities.add(frameNum-1, displacement);
129
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
96 velocities.add(frameNum, displacement);
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
97 float dist = norm(displacement);
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
98 displacementDistances.push_back(dist);
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
99 }
4742b2b6d851 created basic feature saving code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
100 }
163
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 /******************** FeatureGraph ********************/