TrackerPsa.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 "TrackerPsa.h"

using namespace std;

namespace alvar {
using namespace std;

TrackerPsa::TrackerPsa(int _max_shift) {
        max_shift = _max_shift;
        x_res = 0; y_res = 0;
        hor = 0; horprev = 0;
        ver = 0; verprev = 0;
        framecount = 0;
}

TrackerPsa::~TrackerPsa() {
        if (hor) delete [] hor;
        if (horprev) delete [] horprev;
        if (ver) delete [] ver;
        if (verprev) delete [] verprev;
}

double TrackerPsa::Track(IplImage *img) {
        long best_x_factor=99999999;
        long best_y_factor=99999999;
        long worst_x_factor=0;
        long worst_y_factor=0;
        double quality=0;

        //std::cout<<"Moi1"<<std::endl;
        // Init
        if ((x_res != img->width) || (y_res != img->height)) {
                x_res = img->width;
                y_res = img->height;
                if (hor) delete [] hor;
                if (horprev) delete [] horprev;
                if (ver) delete [] ver;
                if (verprev) delete [] verprev;
                hor = new long [x_res];
                horprev = new long [x_res];
                ver = new long [y_res];
                verprev = new long [y_res];
                framecount=0;
        }
        framecount++;

        // Make shift tables
        memset(hor, 0, sizeof(long)*x_res);
        memset(ver, 0, sizeof(long)*y_res);
        for (int y=0; y<y_res; y++) {
                for (int x=0; x<x_res; x++) {
                        unsigned char c = (unsigned char)cvGet2D(img, y, x).val[0];
                        hor[x] += c;
                        ver[y] += c;
                }
        }

        // If this is first frame -- no motion
        if (framecount == 1) {
                xd=0; yd=0;
        } 

        // Otherwais detect for motion
        else {
                // Sequence for s: 0, 1, -1, 2, -2, ... -(max_shift-1), (max_shift-1)
                for (int s=0; s<max_shift; (s>0 ? s=-s : s=-s+1) ) {
                        long factor=0;
                        long factorfirst=0;
                        int count=0;
                        for (int x=0; x<x_res; x++) {
                                int x2 = x+s;
                                if ((x2 > 0) && (x2<x_res)) {
                                        factor += labs(hor[x2] - horprev[x]);
                                        count++;
                                }
                        }
                        factor /= count;
                        if (factor < best_x_factor) {
                                best_x_factor=factor;
                                xd=s;
                        }
                        if (factor > worst_x_factor) worst_x_factor=factor;
                }
                for (int s=0; s<max_shift; (s>0 ? s=-s : s=-s+1)) {
                        long factor=0;
                        int count=0;
                        for (int y=0; y<y_res; y++) {
                                int y2 = y+s;
                                if ((y2 > 0) && (y2<y_res)) {
                                        factor += labs(ver[y2] - verprev[y]);
                                        count++;
                                }
                        }
                        factor /= count;
                        if (factor < best_y_factor) {
                                best_y_factor=factor;
                                yd=s;
                        }
                        if (factor > worst_y_factor) worst_y_factor=factor;
                }
                // Figure out the quality?
                // We assume the result is poor if the 
                // worst factor is very near the best factor
                int qual_x = (worst_x_factor - best_x_factor)/y_res;
                int qual_y = (worst_y_factor - best_y_factor)/x_res;
                quality = std::min(qual_x, qual_y);
        }

        // Swap memories
        long *tmp;
        tmp=hor; hor=horprev; horprev=tmp;
        tmp=ver; ver=verprev; verprev=tmp;

        // Return confidence (bigger is better)
        // Smaller than 10 is poor, and you almost certainly have 
        // problems with quality values less than 5.
        //printf("%d\n", quality);
        return quality;
}

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

TrackerPsaRot::TrackerPsaRot(int _max_shift) : TrackerPsa(_max_shift) {
        rotd = 0;
        rot=new double [360];
        rotprev=new double[360];
        rot_count=new int[360];
}

TrackerPsaRot::~TrackerPsaRot() {
        if (rot) delete [] rot;
        if (rotprev) delete [] rotprev;
        if (rot_count) delete [] rot_count;
}

double TrackerPsaRot::Track(IplImage *img) {
        long best_rot_factor=99999999;
        double conf1 = TrackerPsa::Track(img);

        if (framecount == 1) {
                rotd=0;
        }
        else {
                memset(rot, 0, sizeof(double)*360);
                memset(rot_count, 0, sizeof(int)*360);
                for (int y=0; y<y_res; y++) {
                        for (int x=0; x<x_res; x++) {
                                double y2 = y-(y_res/2)-(yd);
                                double x2 = x-(x_res/2)-(xd);
                                double r = sqrt((double)y2*y2 + x2*x2);
                                int theta = int(atan2((double)y2, (double)x2)*180/3.14159265);
                                if (theta < 0) theta+=360;
                                if ((y >= 0) && (y < img->height) &&
                                        (x >= 0) && (x < img->width))
                                {
                                        rot[theta] += (unsigned char)cvGet2D(img, y, x).val[0];
                                        rot_count[theta] ++;
                                }
                        }
                }
                for (int i=0; i<360; i++) {
                        rot[i] /= rot_count[i];
                }
                for (int s=0; s<45; (s>0 ? s=-s : s=-s+1)) {
                        long factor=0;
                        for (int theta=0; theta<360; theta++) {
                                int theta2 = theta+s;
                                if (theta2 < 0) theta2+=360;
                                if (theta2 >= 360) theta2-=360;
                                factor += (long)labs(long(rot[theta2] - rotprev[theta]));
                        }
                        if (factor < best_rot_factor) {
                                best_rot_factor=factor;
                                rotd=s;
                        }
                }
        }
        // Remember rotation based on center
        memset(rotprev, 0, sizeof(double)*360);
        memset(rot_count, 0, sizeof(int)*360);
        for (int y=0; y<y_res; y++) {
                for (int x=0; x<x_res; x++) {
                        double y2 = y-(y_res/2);
                        double x2 = x-(x_res/2);
                        double r = sqrt((double)y2*y2 + x2*x2);
                        int theta = int(atan2((double)y2, (double)x2)*180/3.14159265);
                        if (theta < 0) theta+=360;
                        if ((y >= 0) && (y < img->height) &&
                                (x >= 0) && (x < img->width))
                        {
                                rotprev[theta] += (unsigned char)cvGet2D(img, y, x).val[0];
                                rot_count[theta] ++;
                        }
                }
        }
        for (int i=0; i<360; i++) {
                rotprev[i] /= rot_count[i];
        }
        return conf1 + best_rot_factor;
}

void TrackerPsaRot::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