1 //*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2008-2011, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
27 // * The name of Intel Corporation may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
42 #include "precomp.hpp"
45 #include "opencv2/calib3d/calib3d.hpp"
46 #include "opencv2/contrib/hybridtracker.hpp"
50 CvFeatureTracker::CvFeatureTracker(CvFeatureTrackerParams _params) :
53 switch (params.feature_type)
55 case CvFeatureTrackerParams::SIFT:
56 dd = Algorithm::create<Feature2D>("Feature2D.SIFT");
58 CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without SIFT support");
59 dd->set("nOctaveLayers", 5);
60 dd->set("contrastThreshold", 0.04);
61 dd->set("edgeThreshold", 10.7);
63 case CvFeatureTrackerParams::SURF:
64 dd = Algorithm::create<Feature2D>("Feature2D.SURF");
66 CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without SURF support");
67 dd->set("hessianThreshold", 400);
68 dd->set("nOctaves", 3);
69 dd->set("nOctaveLayers", 4);
72 CV_Error(CV_StsBadArg, "Unknown feature type");
76 matcher = new BFMatcher(NORM_L2);
79 CvFeatureTracker::~CvFeatureTracker()
83 void CvFeatureTracker::newTrackingWindow(Mat image, Rect selection)
85 image.copyTo(prev_image);
86 cvtColor(prev_image, prev_image_bw, CV_BGR2GRAY);
87 prev_trackwindow = selection;
88 prev_center.x = selection.x;
89 prev_center.y = selection.y;
93 Rect CvFeatureTracker::updateTrackingWindow(Mat image)
95 if(params.feature_type == CvFeatureTrackerParams::OPTICAL_FLOW)
96 return updateTrackingWindowWithFlow(image);
98 return updateTrackingWindowWithSIFT(image);
101 Rect CvFeatureTracker::updateTrackingWindowWithSIFT(Mat image)
104 vector<KeyPoint> prev_keypoints, curr_keypoints;
105 vector<Point2f> prev_keys, curr_keys;
106 Mat prev_desc, curr_desc;
108 Rect window = prev_trackwindow;
109 Mat mask = Mat::zeros(image.size(), CV_8UC1);
110 rectangle(mask, Point(window.x, window.y), Point(window.x + window.width,
111 window.y + window.height), Scalar(255), CV_FILLED);
113 dd->operator()(prev_image, mask, prev_keypoints, prev_desc);
115 window.x -= params.window_size;
116 window.y -= params.window_size;
117 window.width += params.window_size;
118 window.height += params.window_size;
119 rectangle(mask, Point(window.x, window.y), Point(window.x + window.width,
120 window.y + window.height), Scalar(255), CV_FILLED);
122 dd->operator()(image, mask, curr_keypoints, curr_desc);
124 if (prev_keypoints.size() > 4 && curr_keypoints.size() > 4)
126 //descriptor->compute(prev_image, prev_keypoints, prev_desc);
127 //descriptor->compute(image, curr_keypoints, curr_desc);
129 matcher->match(prev_desc, curr_desc, matches);
131 for (int i = 0; i < (int)matches.size(); i++)
133 prev_keys.push_back(prev_keypoints[matches[i].queryIdx].pt);
134 curr_keys.push_back(curr_keypoints[matches[i].trainIdx].pt);
137 Mat T = findHomography(prev_keys, curr_keys, CV_LMEDS);
139 prev_trackwindow.x += cvRound(T.at<double> (0, 2));
140 prev_trackwindow.y += cvRound(T.at<double> (1, 2));
143 prev_center.x = prev_trackwindow.x;
144 prev_center.y = prev_trackwindow.y;
146 return prev_trackwindow;
149 Rect CvFeatureTracker::updateTrackingWindowWithFlow(Mat image)
152 Size subPixWinSize(10,10), winSize(31,31);
154 TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03);
155 vector<uchar> status;
158 cvtColor(image, image_bw, CV_BGR2GRAY);
159 cvtColor(prev_image, prev_image_bw, CV_BGR2GRAY);
163 Mat mask = Mat::zeros(image.size(), CV_8UC1);
164 rectangle(mask, Point(prev_trackwindow.x, prev_trackwindow.y), Point(
165 prev_trackwindow.x + prev_trackwindow.width, prev_trackwindow.y
166 + prev_trackwindow.height), Scalar(255), CV_FILLED);
167 goodFeaturesToTrack(image_bw, features[1], 500, 0.01, 20, mask, 3, 0, 0.04);
168 cornerSubPix(image_bw, features[1], subPixWinSize, Size(-1, -1), termcrit);
172 calcOpticalFlowPyrLK(prev_image_bw, image_bw, features[0], features[1],
173 status, err, winSize, 3, termcrit);
175 Point2f feature0_center(0, 0);
176 Point2f feature1_center(0, 0);
178 for (int i = 0; i < (int)features[1].size(); i++)
182 feature0_center.x += features[0][i].x;
183 feature0_center.y += features[0][i].y;
184 feature1_center.x += features[1][i].x;
185 feature1_center.y += features[1][i].y;
190 feature0_center.x /= goodtracks;
191 feature0_center.y /= goodtracks;
192 feature1_center.x /= goodtracks;
193 feature1_center.y /= goodtracks;
195 prev_center.x += (feature1_center.x - feature0_center.x);
196 prev_center.y += (feature1_center.y - feature0_center.y);
198 prev_trackwindow.x = (int)prev_center.x;
199 prev_trackwindow.y = (int)prev_center.y;
202 swap(features[0], features[1]);
203 image.copyTo(prev_image);
204 return prev_trackwindow;
207 void CvFeatureTracker::setTrackingWindow(Rect _window)
209 prev_trackwindow = _window;
212 Rect CvFeatureTracker::getTrackingWindow()
214 return prev_trackwindow;
217 Point2f CvFeatureTracker::getTrackingCenter()
219 Point2f center(0, 0);
220 center.x = (float)(prev_center.x + prev_trackwindow.width/2.0);
221 center.y = (float)(prev_center.y + prev_trackwindow.height/2.0);