Mercurial > hg > nsaunier > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 507:081a9da6f85b

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
first version with undistort implemented in the feature tracking process
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 01 May 2014 17:41:10 -0400
parents 13d4eb96a751 b96ff16b1c81
children 935430b1d408
comparison
equal deleted inserted replaced
506:13d4eb96a751 507:081a9da6f85b
70 70
71 Mat homography = ::loadMat(params.homographyFilename, " "); 71 Mat homography = ::loadMat(params.homographyFilename, " ");
72 Mat invHomography; 72 Mat invHomography;
73 if (params.display && !homography.empty()) 73 if (params.display && !homography.empty())
74 invHomography = homography.inv(); 74 invHomography = homography.inv();
75 Mat intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " ");
76 //cout << intrinsicCameraMatrix << endl;
75 77
76 float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement; 78 float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
77 Size window = Size(params.windowSize, params.windowSize); 79 Size window = Size(params.windowSize, params.windowSize);
78 80
79 // BruteForceMatcher<Hamming> descMatcher; 81 // BruteForceMatcher<Hamming> descMatcher;
91 cout << "Video filename " << params.videoFilename << " could not be opened. Exiting." << endl; 93 cout << "Video filename " << params.videoFilename << " could not be opened. Exiting." << endl;
92 exit(0); 94 exit(0);
93 } 95 }
94 96
95 Size videoSize = capture->getSize(); 97 Size videoSize = capture->getSize();
98 //cout << capture->getSize() << " " << params.undistortedImageMultiplication*videoSize << endl;
96 unsigned int nFrames = capture->getNbFrames(); 99 unsigned int nFrames = capture->getNbFrames();
97 cout << "Video " << params.videoFilename << 100 cout << "Video " << params.videoFilename <<
98 ": width=" << videoSize.width << 101 ": width=" << videoSize.width <<
99 ", height=" << videoSize.height << 102 ", height=" << videoSize.height <<
100 ", nframes=" << nFrames << endl; 103 ", nframes=" << nFrames << endl;
101 104
105 Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone();
106 Size newVideoSize = videoSize;
107 if (params.undistort) {
108 newVideoSize = Size(static_cast<int>(videoSize.width*params.undistortedImageMultiplication), static_cast<int>(videoSize.height*params.undistortedImageMultiplication));
109 newIntrinsicCameraMatrix.at<float>(0,2) = newVideoSize.width/2.;
110 newIntrinsicCameraMatrix.at<float>(1,2) = newVideoSize.height/2.;
111 }
112
113 Mat map1, map2;
114 Mat R = Mat::eye(3,3, CV_32FC1);
115 if (params.undistort)
116 initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, R, newIntrinsicCameraMatrix, newVideoSize, CV_32FC1, map1, map2);
117
118 // todo mask in new size
102 Mat mask = imread(params.maskFilename, 0); 119 Mat mask = imread(params.maskFilename, 0);
103 if (mask.empty()) { 120 if (mask.empty()) {
104 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl; 121 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl;
105 mask = Mat::ones(videoSize, CV_8UC1); 122 mask = Mat::ones(newVideoSize, CV_8UC1);
106 } 123 }
107 124
108 boost::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = boost::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>()); 125 boost::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = boost::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>());
109 //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>(); 126 //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>();
110 trajectoryDB->connect(params.databaseFilename.c_str()); 127 trajectoryDB->connect(params.databaseFilename.c_str());
119 Mat prevDesc, currDesc; 136 Mat prevDesc, currDesc;
120 137
121 std::vector<FeatureTrajectoryPtr> lostFeatures; 138 std::vector<FeatureTrajectoryPtr> lostFeatures;
122 std::vector<FeaturePointMatch> featurePointMatches; 139 std::vector<FeaturePointMatch> featurePointMatches;
123 140
124 //HOGDescriptor hog;
125 //hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector());
126
127 int key = '?'; 141 int key = '?';
128 unsigned int savedFeatureId=0; 142 unsigned int savedFeatureId=0;
129 Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW; 143 Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame;
130 144
131 unsigned int lastFrameNum = nFrames; 145 unsigned int lastFrameNum = nFrames;
132 if (params.nFrames > 0) 146 if (params.nFrames > 0)
133 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); 147 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames);
134 148
135 capture->setFrameNumber(params.frame1); 149 capture->setFrameNumber(params.frame1);
136 for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) { 150 for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) {
137 bool success = capture->getNextFrame(frame); 151 bool success = capture->getNextFrame(frame);
138 152 if (params.undistort) {
139 if (!success || frame.empty() || frame.size() != videoSize) 153 remap(frame, undistortedFrame, map1, map2, INTER_LINEAR, BORDER_CONSTANT, 0.);
140 break; 154 frame = undistortedFrame;
155 }
156 //if (!success || frame.empty() || frame.size() != videoSize)
157 //break;
141 158
142 if (frameNum%50 ==0) 159 if (frameNum%50 ==0)
143 cout << "frame " << frameNum << endl; 160 cout << "frame " << frameNum << endl;
144 161
145 cvtColor(frame, currentFrameBW, CV_RGB2GRAY); 162 cvtColor(frame, currentFrameBW, CV_RGB2GRAY);
146 163
147 if (!prevPts.empty()) { 164 if (!prevPts.empty()) {
148 currPts.clear(); 165 currPts.clear();
149 calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errors, window, params.pyramidLevel, TermCriteria(3 /*static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS)*/, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold); 166 calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errors, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold);
150 /// \todo try calcOpticalFlowFarneback 167 /// \todo try calcOpticalFlowFarneback
151 168
152 std::vector<Point2f> trackedPts; 169 std::vector<Point2f> trackedPts;
153 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); 170 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
154 while (iter != featurePointMatches.end()) { 171 while (iter != featurePointMatches.end()) {
197 Mat featureMask = mask.clone(); 214 Mat featureMask = mask.clone();
198 for (unsigned int n=0;n<currPts.size(); n++) 215 for (unsigned int n=0;n<currPts.size(); n++)
199 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++) 216 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
200 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++) 217 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++)
201 featureMask.at<uchar>(i,j)=0; 218 featureMask.at<uchar>(i,j)=0;
202 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.windowSize, params.useHarrisDetector, params.k); 219 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k);
203 BOOST_FOREACH(Point2f p, newPts) { //for (unsigned int i=0; i<newPts.size(); i++) { 220 BOOST_FOREACH(Point2f p, newPts) { //for (unsigned int i=0; i<newPts.size(); i++) {
204 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography)); 221 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography));
205 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size())); 222 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
206 currPts.push_back(p); 223 currPts.push_back(p);
207 } 224 }