Mercurial > hg > nsaunier > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 513:dbf4b83afbb9

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
pulled in and merged the new functionalities to deal with camera distortion (eg GoPro cameras)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 04 Jun 2014 10:57:09 -0400
parents 935430b1d408
children 018653d1db3c
comparison
equal deleted inserted replaced
505:35c99776e593 513:dbf4b83afbb9
21 #include <boost/filesystem.hpp> 21 #include <boost/filesystem.hpp>
22 22
23 #include <iostream> 23 #include <iostream>
24 #include <vector> 24 #include <vector>
25 #include <ctime> 25 #include <ctime>
26 #include <cmath>
26 27
27 using namespace std; 28 using namespace std;
28 using namespace cv; 29 using namespace cv;
29 namespace fs = boost::filesystem; 30 namespace fs = boost::filesystem;
30 31
57 FeaturePointMatch(FeatureTrajectoryPtr _feature, const int& _pointNum): 58 FeaturePointMatch(FeatureTrajectoryPtr _feature, const int& _pointNum):
58 feature(_feature), pointNum(_pointNum) {} 59 feature(_feature), pointNum(_pointNum) {}
59 }; 60 };
60 61
61 inline void saveFeatures(vector<FeatureTrajectoryPtr>& features, TrajectoryDBAccess<Point2f>& db, const string& positionsTableName, const string& velocitiesTableName) { 62 inline void saveFeatures(vector<FeatureTrajectoryPtr>& features, TrajectoryDBAccess<Point2f>& db, const string& positionsTableName, const string& velocitiesTableName) {
62 /// \todo smoothing
63 BOOST_FOREACH(FeatureTrajectoryPtr f, features) f->write(db, positionsTableName, velocitiesTableName); 63 BOOST_FOREACH(FeatureTrajectoryPtr f, features) f->write(db, positionsTableName, velocitiesTableName);
64 features.clear(); 64 features.clear();
65 } 65 }
66 66
67 void trackFeatures(const KLTFeatureTrackingParameters& params) { 67 void trackFeatures(const KLTFeatureTrackingParameters& params) {
71 71
72 Mat homography = ::loadMat(params.homographyFilename, " "); 72 Mat homography = ::loadMat(params.homographyFilename, " ");
73 Mat invHomography; 73 Mat invHomography;
74 if (params.display && !homography.empty()) 74 if (params.display && !homography.empty())
75 invHomography = homography.inv(); 75 invHomography = homography.inv();
76 Mat intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " ");
77 //cout << intrinsicCameraMatrix << endl;
76 78
77 float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement; 79 float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
78 Size window = Size(params.windowSize, params.windowSize); 80 Size window = Size(params.windowSize, params.windowSize);
81
82 int interpolationMethod = -1;
83 if (params.interpolationMethod == 0)
84 interpolationMethod = INTER_NEAREST;
85 else if (params.interpolationMethod == 1)
86 interpolationMethod = INTER_LINEAR;
87 else if (params.interpolationMethod == 2)
88 interpolationMethod = INTER_CUBIC;
89 else if (params.interpolationMethod == 3)
90 interpolationMethod = INTER_LANCZOS4;
91 else
92 cout << "Unsupported option " << interpolationMethod << " for interpolation method" << endl;
79 93
80 // BruteForceMatcher<Hamming> descMatcher; 94 // BruteForceMatcher<Hamming> descMatcher;
81 // vector<DMatch> matches; 95 // vector<DMatch> matches;
82 96
83 boost::shared_ptr<InputFrameProviderIface> capture; 97 boost::shared_ptr<InputFrameProviderIface> capture;
97 unsigned int nFrames = capture->getNbFrames(); 111 unsigned int nFrames = capture->getNbFrames();
98 cout << "Video " << params.videoFilename << 112 cout << "Video " << params.videoFilename <<
99 ": width=" << videoSize.width << 113 ": width=" << videoSize.width <<
100 ", height=" << videoSize.height << 114 ", height=" << videoSize.height <<
101 ", nframes=" << nFrames << endl; 115 ", nframes=" << nFrames << endl;
116
117 Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone();
118 Mat map1, map2;
119 if (params.undistort) {
120 videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
121 newIntrinsicCameraMatrix.at<float>(0,2) = videoSize.width/2.;
122 newIntrinsicCameraMatrix.at<float>(1,2) = videoSize.height/2.;
123 initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2);
124
125 cout << "Undistorted width=" << videoSize.width <<
126 ", height=" << videoSize.height << endl;
127 }
102 128
103 Mat mask = imread(params.maskFilename, 0); 129 Mat mask = imread(params.maskFilename, 0);
104 if (mask.empty()) { 130 if (mask.empty()) {
105 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl; 131 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl;
106 mask = Mat::ones(videoSize, CV_8UC1); 132 mask = Mat::ones(videoSize, CV_8UC1);
120 Mat prevDesc, currDesc; 146 Mat prevDesc, currDesc;
121 147
122 std::vector<FeatureTrajectoryPtr> lostFeatures; 148 std::vector<FeatureTrajectoryPtr> lostFeatures;
123 std::vector<FeaturePointMatch> featurePointMatches; 149 std::vector<FeaturePointMatch> featurePointMatches;
124 150
125 //HOGDescriptor hog;
126 //hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector());
127
128 int key = '?'; 151 int key = '?';
129 unsigned int savedFeatureId=0; 152 unsigned int savedFeatureId=0;
130 Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW; 153 Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame;
131 154
132 unsigned int lastFrameNum = nFrames; 155 unsigned int lastFrameNum = nFrames;
133 if (params.nFrames > 0) 156 if (params.nFrames > 0)
134 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); 157 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames);
135 158
136 capture->setFrameNumber(params.frame1); 159 capture->setFrameNumber(params.frame1);
137 for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) { 160 for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) {
138 bool success = capture->getNextFrame(frame); 161 bool success = capture->getNextFrame(frame);
139 162 if (params.undistort) {
140 if (!success || frame.empty() || frame.size() != videoSize) 163 remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
141 break; 164 frame = undistortedFrame;
165 }
166 //if (!success || frame.empty() || frame.size() != videoSize)
167 //break;
142 168
143 if (frameNum%50 ==0) 169 if (frameNum%50 ==0)
144 cout << "frame " << frameNum << endl; 170 cout << "frame " << frameNum << endl;
145 171
146 cvtColor(frame, currentFrameBW, CV_RGB2GRAY); 172 cvtColor(frame, currentFrameBW, CV_RGB2GRAY);