TrackerStat.cpp

Go to the documentation of this file.

/*
 * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
 *
 * Copyright 2007-2012 VTT Technical Research Centre of Finland
 *
 * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
 *          <http://www.vtt.fi/multimedia/alvar.html>
 *
 * ALVAR is free software; you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with ALVAR; if not, see
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
 */

#include "TrackerStat.h"

using namespace std;

namespace alvar {
using namespace std;

TrackerStat::TrackerStat(int binsize) : f(100,90) {
        hist.AddDimension(binsize); // x
        hist.AddDimension(binsize); // y
}

void TrackerStat::Reset() {
        f.Reset();
}

double TrackerStat::Track(IplImage *img) 
{
        if (img == NULL) return -1;
        f.Track(img);
        hist.Clear();
        for (int p=0; p<f.prev_feature_count ; p++) {
                for (int c=0; c<f.feature_count; c++) {
                        if (f.prev_ids[p] != f.ids[c]) continue;
                        float x = f.features[c].x - f.prev_features[p].x;
                        float y = f.features[c].y - f.prev_features[p].y;
                        hist.Inc(x, y);
                }
        }
        xd = 0; yd = 0;
        return hist.GetMax(&xd, &yd);
}

void TrackerStat::Compensate(double *x, double *y) { 
        *x += xd; *y += yd; 
}

TrackerStatRot::TrackerStatRot(int binsize /*=8*/, int binsize_rot/*=3*/) : TrackerStat(binsize) {
        hist_rot.AddDimension(binsize_rot);
}

double TrackerStatRot::Track(IplImage *img)
{
        if (img == NULL) return -1;
        f.Track(img);
        // Translation
        hist.Clear();
        for (int p=0; p<f.prev_feature_count ; p++) {
                for (int c=0; c<f.feature_count; c++) {
                        if (f.prev_ids[p] != f.ids[c]) continue;
                        float x = f.features[c].x - f.prev_features[p].x;
                        float y = f.features[c].y - f.prev_features[p].y;
                        hist.Inc(x, y);
                }
        }
        xd = 0; yd = 0;
        double ret = hist.GetMax(&xd, &yd);
        // Rotation
        x_res = img->width;
        y_res = img->height;
        hist_rot.Clear();
        for (int p=0; p<f.prev_feature_count ; p++) {
                for (int c=0; c<f.feature_count; c++) {
                        if (f.prev_ids[p] != f.ids[c]) continue;
                        double x_pred = f.prev_features[p].x + xd;
                        double y_pred = f.prev_features[p].y + yd;
                        double x_curr = f.features[c].x;
                        double y_curr = f.features[c].y;
                        double x = x_curr - x_pred;
                        double y = y_curr - y_pred;
                        double theta_pred = atan2((double)y_pred-(y_res/2), (double)x_pred-(x_res/2))*180.0/3.1415926535;
                        double theta_curr = atan2((double)y_curr-(y_res/2), (double)x_curr-(x_res/2))*180.0/3.1415926535;
                        hist_rot.Inc(theta_curr-theta_pred);
                }
        }
        rotd=0;
        hist_rot.GetMax(&rotd);
        return ret;
}

void TrackerStatRot::Compensate(double *x, double *y)
{
        double xx = *x - (x_res/2);
        double yy = *y - (y_res/2);
        double kosini = cos(rotd*3.1415926535/180);
        double sini = sin(rotd*3.1415926535/180);
        *x = ((kosini * xx) - (sini * yy)) + xd + (x_res/2);
        *y = ((sini * xx) + (kosini * yy)) + yd + (y_res/2);
}

} // namespace alvar