modules/contrib/src/featuretracker.cpp

   1 //*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   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.
   8 //
   9 //
  10 //                                License Agreement
  11 //                       For Open Source Computer Vision Library
  12 //
  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.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  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.
  26 //
  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.
  29 //
  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.
  40 //
  41 //M*/
  42 #include "precomp.hpp"
  43 #include <stdio.h>
  44 #include <iostream>
  45 #include "opencv2/calib3d.hpp"
  46 #include "opencv2/contrib/hybridtracker.hpp"
  47
  48 #ifdef HAVE_OPENCV_NONFREE
  49 #include "opencv2/nonfree/nonfree.hpp"
  50
  51 static bool makeUseOfNonfree = initModule_nonfree();
  52 #endif
  53
  54 using namespace cv;
  55
  56 CvFeatureTracker::CvFeatureTracker(CvFeatureTrackerParams _params) :
  57     params(_params)
  58 {
  59     switch (params.feature_type)
  60     {
  61     case CvFeatureTrackerParams::SIFT:
  62         dd = Algorithm::create<Feature2D>("Feature2D.SIFT");
  63         if( !dd )
  64             CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without SIFT support");
  65         dd->set("nOctaveLayers", 5);
  66         dd->set("contrastThreshold", 0.04);
  67         dd->set("edgeThreshold", 10.7);
  68         break;
  69     case CvFeatureTrackerParams::SURF:
  70         dd = Algorithm::create<Feature2D>("Feature2D.SURF");
  71         if( !dd )
  72             CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without SURF support");
  73         dd->set("hessianThreshold", 400);
  74         dd->set("nOctaves", 3);
  75         dd->set("nOctaveLayers", 4);
  76         break;
  77     default:
  78         CV_Error(CV_StsBadArg, "Unknown feature type");
  79         break;
  80     }
  81
  82     matcher = makePtr<BFMatcher>(int(NORM_L2));
  83 }
  84
  85 CvFeatureTracker::~CvFeatureTracker()
  86 {
  87 }
  88
  89 void CvFeatureTracker::newTrackingWindow(Mat image, Rect selection)
  90 {
  91     image.copyTo(prev_image);
  92     cvtColor(prev_image, prev_image_bw, COLOR_BGR2GRAY);
  93     prev_trackwindow = selection;
  94     prev_center.x = selection.x;
  95     prev_center.y = selection.y;
  96     ittr = 0;
  97 }
  98
  99 Rect CvFeatureTracker::updateTrackingWindow(Mat image)
 100 {
 101     if(params.feature_type == CvFeatureTrackerParams::OPTICAL_FLOW)
 102         return updateTrackingWindowWithFlow(image);
 103     else
 104         return updateTrackingWindowWithSIFT(image);
 105 }
 106
 107 Rect CvFeatureTracker::updateTrackingWindowWithSIFT(Mat image)
 108 {
 109     ittr++;
 110     std::vector<KeyPoint> prev_keypoints, curr_keypoints;
 111     std::vector<Point2f> prev_keys, curr_keys;
 112     Mat prev_desc, curr_desc;
 113
 114     Rect window = prev_trackwindow;
 115     Mat mask = Mat::zeros(image.size(), CV_8UC1);
 116     rectangle(mask, Point(window.x, window.y), Point(window.x + window.width,
 117             window.y + window.height), Scalar(255), CV_FILLED);
 118
 119     dd->operator()(prev_image, mask, prev_keypoints, prev_desc);
 120
 121     window.x -= params.window_size;
 122     window.y -= params.window_size;
 123     window.width += params.window_size;
 124     window.height += params.window_size;
 125     rectangle(mask, Point(window.x, window.y), Point(window.x + window.width,
 126             window.y + window.height), Scalar(255), CV_FILLED);
 127
 128     dd->operator()(image, mask, curr_keypoints, curr_desc);
 129
 130     if (prev_keypoints.size() > 4 && curr_keypoints.size() > 4)
 131     {
 132         //descriptor->compute(prev_image, prev_keypoints, prev_desc);
 133         //descriptor->compute(image, curr_keypoints, curr_desc);
 134
 135         matcher->match(prev_desc, curr_desc, matches);
 136
 137         for (int i = 0; i < (int)matches.size(); i++)
 138         {
 139             prev_keys.push_back(prev_keypoints[matches[i].queryIdx].pt);
 140             curr_keys.push_back(curr_keypoints[matches[i].trainIdx].pt);
 141         }
 142
 143         Mat T = findHomography(prev_keys, curr_keys, LMEDS);
 144
 145         prev_trackwindow.x += cvRound(T.at<double> (0, 2));
 146         prev_trackwindow.y += cvRound(T.at<double> (1, 2));
 147     }
 148
 149     prev_center.x = prev_trackwindow.x;
 150     prev_center.y = prev_trackwindow.y;
 151     prev_image = image;
 152     return prev_trackwindow;
 153 }
 154
 155 Rect CvFeatureTracker::updateTrackingWindowWithFlow(Mat image)
 156 {
 157     ittr++;
 158     Size subPixWinSize(10,10), winSize(31,31);
 159     Mat image_bw;
 160     TermCriteria termcrit(TermCriteria::COUNT | TermCriteria::EPS, 20, 0.03);
 161     std::vector<uchar> status;
 162     std::vector<float> err;
 163
 164     cvtColor(image, image_bw, COLOR_BGR2GRAY);
 165     cvtColor(prev_image, prev_image_bw, COLOR_BGR2GRAY);
 166
 167     if (ittr == 1)
 168     {
 169         Mat mask = Mat::zeros(image.size(), CV_8UC1);
 170         rectangle(mask, Point(prev_trackwindow.x, prev_trackwindow.y), Point(
 171                 prev_trackwindow.x + prev_trackwindow.width, prev_trackwindow.y
 172                         + prev_trackwindow.height), Scalar(255), CV_FILLED);
 173         goodFeaturesToTrack(image_bw, features[1], 500, 0.01, 20, mask, 3, 0, 0.04);
 174         cornerSubPix(image_bw, features[1], subPixWinSize, Size(-1, -1), termcrit);
 175     }
 176     else
 177     {
 178         calcOpticalFlowPyrLK(prev_image_bw, image_bw, features[0], features[1],
 179                 status, err, winSize, 3, termcrit);
 180
 181         Point2f feature0_center(0, 0);
 182         Point2f feature1_center(0, 0);
 183         int goodtracks = 0;
 184         for (int i = 0; i < (int)features[1].size(); i++)
 185         {
 186             if (status[i] == 1)
 187             {
 188                 feature0_center.x += features[0][i].x;
 189                 feature0_center.y += features[0][i].y;
 190                 feature1_center.x += features[1][i].x;
 191                 feature1_center.y += features[1][i].y;
 192                 goodtracks++;
 193             }
 194         }
 195
 196         feature0_center.x /= goodtracks;
 197         feature0_center.y /= goodtracks;
 198         feature1_center.x /= goodtracks;
 199         feature1_center.y /= goodtracks;
 200
 201         prev_center.x += (feature1_center.x - feature0_center.x);
 202         prev_center.y += (feature1_center.y - feature0_center.y);
 203
 204         prev_trackwindow.x = (int)prev_center.x;
 205         prev_trackwindow.y = (int)prev_center.y;
 206     }
 207
 208     swap(features[0], features[1]);
 209     image.copyTo(prev_image);
 210     return prev_trackwindow;
 211 }
 212
 213 void CvFeatureTracker::setTrackingWindow(Rect _window)
 214 {
 215     prev_trackwindow = _window;
 216 }
 217
 218 Rect CvFeatureTracker::getTrackingWindow()
 219 {
 220     return prev_trackwindow;
 221 }
 222
 223 Point2f CvFeatureTracker::getTrackingCenter()
 224 {
 225     Point2f center(0, 0);
 226     center.x = (float)(prev_center.x + prev_trackwindow.width/2.0);
 227     center.y = (float)(prev_center.y + prev_trackwindow.height/2.0);
 228     return center;
 229 }