Subversion Repositories OpenCV2-Cookbook

/*------------------------------------------------------------------------------------------*\
   This file contains material supporting chapter 10 of the cookbook:  
   Computer Vision Programming using the OpenCV Library.
   by Robert Laganiere, Packt Publishing, 2011.

   This program is free software; permission is hereby granted to use, copy, modify,
   and distribute this source code, or portions thereof, for any purpose, without fee,
   subject to the restriction that the copyright notice may not be removed
   or altered from any source or altered source distribution.
   The software is released on an as-is basis and without any warranties of any kind.
   In particular, the software is not guaranteed to be fault-tolerant or free from failure.
   The author disclaims all warranties with regard to this software, any use,
   and any consequent failure, is purely the responsibility of the user.
 
   Copyright (C) 2010-2011 Robert Laganiere, www.laganiere.name
\*------------------------------------------------------------------------------------------*/

#if !defined FTRACKER
#define FTRACKER

#include <string>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>

#include "videoprocessor.h"

class FeatureTracker : public FrameProcessor {
       
        cv::Mat gray;                   // current gray-level image
        cv::Mat gray_prev;              // previous gray-level image
        std::vector<cv::Point2f> points[2]; // tracked features from 0->1
        std::vector<cv::Point2f> initial;   // initial position of tracked points
        std::vector<cv::Point2f> features;  // detected features
        int max_count;    // maximum number of features to detect
        double qlevel;    // quality level for feature detection
        double minDist;   // minimum distance between two feature points
        std::vector<uchar> status; // status of tracked features
    std::vector<float> err;    // error in tracking

  public:

        FeatureTracker() : max_count(500), qlevel(0.01), minDist(10.) {}
       
        // processing method
        void process(cv:: Mat &frame, cv:: Mat &output) {

                // convert to gray-level image
                cv::cvtColor(frame, gray, CV_BGR2GRAY);
                frame.copyTo(output);

                // 1. if new feature points must be added
                if(addNewPoints())
                {
                        // detect feature points
                        detectFeaturePoints();
                        // add the detected features to the currently tracked features
                        points[0].insert(points[0].end(),features.begin(),features.end());
                        initial.insert(initial.end(),features.begin(),features.end());
                }
               
                // for first image of the sequence
                if(gray_prev.empty())
           gray.copyTo(gray_prev);
           
                // 2. track features
                cv::calcOpticalFlowPyrLK(gray_prev, gray, // 2 consecutive images
                        points[0], // input point position in first image
                        points[1], // output point postion in the second image
                        status,    // tracking success
                        err);      // tracking error
           
                // 2. loop over the tracked points to reject the undesirables
                int k=0;
                for( int i= 0; i < points[1].size(); i++ ) {

                        // do we keep this point?
                        if (acceptTrackedPoint(i)) {

                                // keep this point in vector
                                initial[k]= initial[i];
                                points[1][k++] = points[1][i];
                        }
                }

                // eliminate unsuccesful points
        points[1].resize(k);
                initial.resize(k);

                // 3. handle the accepted tracked points
                handleTrackedPoints(frame, output);

                // 4. current points and image become previous ones
                std::swap(points[1], points[0]);
        cv::swap(gray_prev, gray);
        }

        // feature point detection
        void detectFeaturePoints() {
                       
                // detect the features
                cv::goodFeaturesToTrack(gray, // the image
                        features,   // the output detected features
                        max_count,  // the maximum number of features
                        qlevel,     // quality level
                        minDist);   // min distance between two features
        }

        // determine if new points should be added
        bool addNewPoints() {

                // if too few points
                return points[0].size()<=10;
        }

        // determine which tracked point should be accepted
        bool acceptTrackedPoint(int i) {

                return status[i] &&
                        // if point has moved
                        (abs(points[0][i].x-points[1][i].x)+
                        (abs(points[0][i].y-points[1][i].y))>2);
        }

        // handle the currently tracked points
        void handleTrackedPoints(cv:: Mat &frame, cv:: Mat &output) {

                // for all tracked points
                for(int i= 0; i < points[1].size(); i++ ) {

                        // draw line and circle
                    cv::line(output, initial[i], points[1][i], cv::Scalar(255,255,255));
                        cv::circle(output, points[1][i], 3, cv::Scalar(255,255,255),-1);
                }
        }
};

#endif