SampleTrack.cpp

Go to the documentation of this file.

#include "CvTestbed.h"
#include "Camera.h"
#include "TrackerPsa.h"
#include "TrackerStat.h"
#include "TrackerFeatures.h"
#include "Shared.h"
using namespace alvar;
using namespace std;

int tracker = 0;
bool reset = true;
CvFont font;
std::stringstream calibrationFilename;

void track_none(IplImage *image, IplImage *img_gray) {
}

void track_psa(IplImage *image, IplImage *img_gray) {
    static TrackerPsa tracker_psa;
    static double x, y;
    if (reset) {
        reset = false;
        x = img_gray->width / 2;
        y = img_gray->height / 2;
        tracker_psa.Track(img_gray); // To reset tracker call it twice
    }
    tracker_psa.Track(img_gray);
    tracker_psa.Compensate(&x, &y);
    cvCircle(image, cvPoint(int(x), int(y)), 10, CV_RGB(255,0,0));
}

void track_psa_rot(IplImage *image, IplImage *img_gray) {
    static TrackerPsaRot tracker_psa_rot;
    static double x, y, r;
    if (reset) {
        reset = false;
        x = img_gray->width / 2;
        y = img_gray->height / 2;
        r = 0;
        tracker_psa_rot.Track(img_gray); // To reset tracker call it twice
    }
    tracker_psa_rot.Track(img_gray);
    tracker_psa_rot.Compensate(&x, &y);
    r += tracker_psa_rot.rotd;
    cvCircle(image, cvPoint(int(x), int(y)), 15, CV_RGB(255,0,0));
    double r_rad = r*3.1415926535/180;
    cvLine(image, cvPoint(int(x), int(y)), cvPoint(int(x-sin(r_rad)*15), int(y+cos(r_rad)*15)), CV_RGB(255,0,0));
}

void track_stat(IplImage *image, IplImage *img_gray) {
    static TrackerStat tracker_stat;
    static double x, y;
    if (reset) {
        reset = false;
        x = img_gray->width / 2;
        y = img_gray->height / 2;
        tracker_stat.Track(img_gray); // To reset tracker call it twice
    }
    tracker_stat.Track(img_gray);
    tracker_stat.Compensate(&x, &y);
    cvCircle(image, cvPoint(int(x), int(y)), 10, CV_RGB(0,255,0));
}

void track_stat_rot(IplImage *image, IplImage *img_gray) {
    static TrackerStatRot tracker_stat_rot;
    static double x, y, r;
    if (reset) {
        reset = false;
        x = img_gray->width / 2;
        y = img_gray->height / 2;
        r = 0;
        tracker_stat_rot.Track(img_gray); // To reset tracker call it twice
    }
    tracker_stat_rot.Track(img_gray);
    tracker_stat_rot.Compensate(&x, &y);
    r += tracker_stat_rot.rotd;
    cvCircle(image, cvPoint(int(x), int(y)), 15, CV_RGB(0,255,0));
    double r_rad = r*3.1415926535/180;
    cvLine(image, cvPoint(int(x), int(y)), cvPoint(int(x-sin(r_rad)*15), int(y+cos(r_rad)*15)), CV_RGB(0,255,0));
}

void track_features(IplImage *image, IplImage *img_gray) {
    static TrackerFeatures tracker_features(200, 190, 0.01, 0, 4, 6);
    if (reset) {
        reset = false;
        tracker_features.Reset();
    }
    tracker_features.Purge();
    tracker_features.Track(img_gray);
    for (int i=0; i<tracker_features.feature_count; i++) {
        cvCircle(image, 
            cvPoint(int(tracker_features.features[i].x), int(tracker_features.features[i].y)), 2, 
            CV_RGB(tracker_features.ids[i]%255,(tracker_features.ids[i]*7)%255,(tracker_features.ids[i]*11)%255));
    }
}

const int nof_trackers = 6;
void (*(trackers[nof_trackers]))(IplImage *image, IplImage *img_gray) = {
    track_none,
    track_psa,
    track_psa_rot,
    track_stat,
    track_stat_rot,
    track_features,
};
char tracker_names[nof_trackers][64]={
    "No tracker - Press any key to change",
    "TrackerPsa",
    "TrackerPsaRot",
    "TrackerStat",
    "TrackerStatRot",
    "TrackerFeatures",
};

void videocallback(IplImage *image)
{
    assert(image);
    static Camera cam;
    static IplImage *img_gray=NULL;
    static bool init=true;

    if (init) {
        init = false;
        img_gray = CvTestbed::Instance().CreateImageWithProto("img_gray", image, 0, 1);
        cout<<"Loading calibration: "<<calibrationFilename.str();
        if (cam.SetCalib(calibrationFilename.str().c_str(), image->width, image->height)) {
            cout<<" [Ok]"<<endl;
        } 
        else {
            cam.SetRes(image->width, image->height);
            cout<<" [Fail]"<<endl;
        }
    }
    if (image->nChannels == 1) cvCopy(image, img_gray);
    else cvCvtColor(image, img_gray, CV_RGB2GRAY);

    trackers[tracker](image, img_gray);
    cvPutText(image, tracker_names[tracker], cvPoint(3, image->height - 20), &font, CV_RGB(255, 255, 255));
}

int keycallback(int key) {
    if ((key == 'r') || (key == 't')) {
        reset = true;
        return 0;
    }
    else if ((key == 'n') || (key == ' ')){
        tracker = ((tracker+1)%nof_trackers);
        reset = true;
        return 0;
    }
    return key;
}

int main(int argc, char *argv[])
{
    try {
        // Output usage message
        std::string filename(argv[0]);
        filename = filename.substr(filename.find_last_of('\\') + 1);
        std::cout << "SampleTrack" << std::endl;
        std::cout << "===========" << std::endl;
        std::cout << std::endl;
        std::cout << "Description:" << std::endl;
        std::cout << "  This is an example of how to use the 'TrackerPsa', 'TrackerPsaRot'," << std::endl;
        std::cout << "  'TrackerFeatures', 'TrackerStat' and 'TrackerStatRot' classes to" << std::endl;
        std::cout << "  track the optical flow of the video." << std::endl;
        std::cout << std::endl;
        std::cout << "Usage:" << std::endl;
        std::cout << "  " << filename << " [device]" << std::endl;
        std::cout << std::endl;
        std::cout << "    device    integer selecting device from enumeration list (default 0)" << std::endl;
        std::cout << "              highgui capture devices are prefered" << std::endl;
        std::cout << std::endl;
        std::cout << "Keyboard Shortcuts:" << std::endl;
        std::cout << "  r,t: reset tracker" << std::endl;
        std::cout << "  n,space: cycle through tracking algorithms" << std::endl;
        std::cout << "  q: quit" << std::endl;
        std::cout << std::endl;

        // Initialize font
        cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0);

        // Initialise CvTestbed
        CvTestbed::Instance().SetVideoCallback(videocallback);
        CvTestbed::Instance().SetKeyCallback(keycallback);

        // Enumerate possible capture plugins
        CaptureFactory::CapturePluginVector plugins = CaptureFactory::instance()->enumeratePlugins();
        if (plugins.size() < 1) {
            std::cout << "Could not find any capture plugins." << std::endl;
            return 0;
        }

        // Display capture plugins
        std::cout << "Available Plugins: ";
        outputEnumeratedPlugins(plugins);
        std::cout << std::endl;

        // Enumerate possible capture devices
        CaptureFactory::CaptureDeviceVector devices = CaptureFactory::instance()->enumerateDevices();
        if (devices.size() < 1) {
            std::cout << "Could not find any capture devices." << std::endl;
            return 0;
        }

        // Check command line argument for which device to use
        int selectedDevice = defaultDevice(devices);
        if (argc > 1) {
            selectedDevice = atoi(argv[1]);
        }
        if (selectedDevice >= (int)devices.size()) {
            selectedDevice = defaultDevice(devices);
        }
        
        // Display capture devices
        std::cout << "Enumerated Capture Devices:" << std::endl;
        outputEnumeratedDevices(devices, selectedDevice);
        std::cout << std::endl;
        
        // Create capture object from camera
        Capture *cap = CaptureFactory::instance()->createCapture(devices[selectedDevice]);
        std::string uniqueName = devices[selectedDevice].uniqueName();

        // Handle capture lifecycle and start video capture
        // Note that loadSettings/saveSettings are not supported by all plugins
        if (cap) {
            std::stringstream settingsFilename;
            settingsFilename << "camera_settings_" << uniqueName << ".xml";
            calibrationFilename << "camera_calibration_" << uniqueName << ".xml";
            
            cap->start();
            cap->setResolution(640, 480);
            
            if (cap->loadSettings(settingsFilename.str())) {
                std::cout << "Loading settings: " << settingsFilename.str() << std::endl;
            }

            std::stringstream title;
            title << "SampleTrack (" << cap->captureDevice().captureType() << ")";

            CvTestbed::Instance().StartVideo(cap, title.str().c_str());

            if (cap->saveSettings(settingsFilename.str())) {
                std::cout << "Saving settings: " << settingsFilename.str() << std::endl;
            }

            cap->stop();
            delete cap;
        }
        else if (CvTestbed::Instance().StartVideo(0, argv[0])) {
        }
        else {
            std::cout << "Could not initialize the selected capture backend." << std::endl;
        }

        return 0;
    }
    catch (const std::exception &e) {
        std::cout << "Exception: " << e.what() << endl;
    }
    catch (...) {
        std::cout << "Exception: unknown" << std::endl;
    }
}