TrackerPsa.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "TrackerPsa.h"
00025 
00026 using namespace std;
00027 
00028 namespace alvar {
00029 using namespace std;
00030 
00031 TrackerPsa::TrackerPsa(int _max_shift) {
00032         max_shift = _max_shift;
00033         x_res = 0; y_res = 0;
00034         hor = 0; horprev = 0;
00035         ver = 0; verprev = 0;
00036         framecount = 0;
00037 }
00038 
00039 TrackerPsa::~TrackerPsa() {
00040         if (hor) delete [] hor;
00041         if (horprev) delete [] horprev;
00042         if (ver) delete [] ver;
00043         if (verprev) delete [] verprev;
00044 }
00045 
00046 double TrackerPsa::Track(IplImage *img) {
00047         long best_x_factor=99999999;
00048         long best_y_factor=99999999;
00049         long worst_x_factor=0;
00050         long worst_y_factor=0;
00051         double quality=0;
00052 
00053         //std::cout<<"Moi1"<<std::endl;
00054         // Init
00055         if ((x_res != img->width) || (y_res != img->height)) {
00056                 x_res = img->width;
00057                 y_res = img->height;
00058                 if (hor) delete [] hor;
00059                 if (horprev) delete [] horprev;
00060                 if (ver) delete [] ver;
00061                 if (verprev) delete [] verprev;
00062                 hor = new long [x_res];
00063                 horprev = new long [x_res];
00064                 ver = new long [y_res];
00065                 verprev = new long [y_res];
00066                 framecount=0;
00067         }
00068         framecount++;
00069 
00070         // Make shift tables
00071         memset(hor, 0, sizeof(long)*x_res);
00072         memset(ver, 0, sizeof(long)*y_res);
00073         for (int y=0; y<y_res; y++) {
00074                 for (int x=0; x<x_res; x++) {
00075                         unsigned char c = (unsigned char)cvGet2D(img, y, x).val[0];
00076                         hor[x] += c;
00077                         ver[y] += c;
00078                 }
00079         }
00080 
00081         // If this is first frame -- no motion
00082         if (framecount == 1) {
00083                 xd=0; yd=0;
00084         } 
00085 
00086         // Otherwais detect for motion
00087         else {
00088                 // Sequence for s: 0, 1, -1, 2, -2, ... -(max_shift-1), (max_shift-1)
00089                 for (int s=0; s<max_shift; (s>0 ? s=-s : s=-s+1) ) {
00090                         long factor=0;
00091                         long factorfirst=0;
00092                         int count=0;
00093                         for (int x=0; x<x_res; x++) {
00094                                 int x2 = x+s;
00095                                 if ((x2 > 0) && (x2<x_res)) {
00096                                         factor += labs(hor[x2] - horprev[x]);
00097                                         count++;
00098                                 }
00099                         }
00100                         factor /= count;
00101                         if (factor < best_x_factor) {
00102                                 best_x_factor=factor;
00103                                 xd=s;
00104                         }
00105                         if (factor > worst_x_factor) worst_x_factor=factor;
00106                 }
00107                 for (int s=0; s<max_shift; (s>0 ? s=-s : s=-s+1)) {
00108                         long factor=0;
00109                         int count=0;
00110                         for (int y=0; y<y_res; y++) {
00111                                 int y2 = y+s;
00112                                 if ((y2 > 0) && (y2<y_res)) {
00113                                         factor += labs(ver[y2] - verprev[y]);
00114                                         count++;
00115                                 }
00116                         }
00117                         factor /= count;
00118                         if (factor < best_y_factor) {
00119                                 best_y_factor=factor;
00120                                 yd=s;
00121                         }
00122                         if (factor > worst_y_factor) worst_y_factor=factor;
00123                 }
00124                 // Figure out the quality?
00125                 // We assume the result is poor if the 
00126                 // worst factor is very near the best factor
00127                 int qual_x = (worst_x_factor - best_x_factor)/y_res;
00128                 int qual_y = (worst_y_factor - best_y_factor)/x_res;
00129                 quality = std::min(qual_x, qual_y);
00130         }
00131 
00132         // Swap memories
00133         long *tmp;
00134         tmp=hor; hor=horprev; horprev=tmp;
00135         tmp=ver; ver=verprev; verprev=tmp;
00136 
00137         // Return confidence (bigger is better)
00138         // Smaller than 10 is poor, and you almost certainly have 
00139         // problems with quality values less than 5.
00140         //printf("%d\n", quality);
00141         return quality;
00142 }
00143 
00144 void TrackerPsa::Compensate(double *x, double *y) {
00145         *x += xd; *y += yd;
00146 }
00147 
00148 TrackerPsaRot::TrackerPsaRot(int _max_shift) : TrackerPsa(_max_shift) {
00149         rotd = 0;
00150         rot=new double [360];
00151         rotprev=new double[360];
00152         rot_count=new int[360];
00153 }
00154 
00155 TrackerPsaRot::~TrackerPsaRot() {
00156         if (rot) delete [] rot;
00157         if (rotprev) delete [] rotprev;
00158         if (rot_count) delete [] rot_count;
00159 }
00160 
00161 double TrackerPsaRot::Track(IplImage *img) {
00162         long best_rot_factor=99999999;
00163         double conf1 = TrackerPsa::Track(img);
00164 
00165         if (framecount == 1) {
00166                 rotd=0;
00167         }
00168         else {
00169                 memset(rot, 0, sizeof(double)*360);
00170                 memset(rot_count, 0, sizeof(int)*360);
00171                 for (int y=0; y<y_res; y++) {
00172                         for (int x=0; x<x_res; x++) {
00173                                 double y2 = y-(y_res/2)-(yd);
00174                                 double x2 = x-(x_res/2)-(xd);
00175                                 double r = sqrt((double)y2*y2 + x2*x2);
00176                                 int theta = int(atan2((double)y2, (double)x2)*180/3.14159265);
00177                                 if (theta < 0) theta+=360;
00178                                 if ((y >= 0) && (y < img->height) &&
00179                                         (x >= 0) && (x < img->width))
00180                                 {
00181                                         rot[theta] += (unsigned char)cvGet2D(img, y, x).val[0];
00182                                         rot_count[theta] ++;
00183                                 }
00184                         }
00185                 }
00186                 for (int i=0; i<360; i++) {
00187                         rot[i] /= rot_count[i];
00188                 }
00189                 for (int s=0; s<45; (s>0 ? s=-s : s=-s+1)) {
00190                         long factor=0;
00191                         for (int theta=0; theta<360; theta++) {
00192                                 int theta2 = theta+s;
00193                                 if (theta2 < 0) theta2+=360;
00194                                 if (theta2 >= 360) theta2-=360;
00195                                 factor += (long)labs(long(rot[theta2] - rotprev[theta]));
00196                         }
00197                         if (factor < best_rot_factor) {
00198                                 best_rot_factor=factor;
00199                                 rotd=s;
00200                         }
00201                 }
00202         }
00203         // Remember rotation based on center
00204         memset(rotprev, 0, sizeof(double)*360);
00205         memset(rot_count, 0, sizeof(int)*360);
00206         for (int y=0; y<y_res; y++) {
00207                 for (int x=0; x<x_res; x++) {
00208                         double y2 = y-(y_res/2);
00209                         double x2 = x-(x_res/2);
00210                         double r = sqrt((double)y2*y2 + x2*x2);
00211                         int theta = int(atan2((double)y2, (double)x2)*180/3.14159265);
00212                         if (theta < 0) theta+=360;
00213                         if ((y >= 0) && (y < img->height) &&
00214                                 (x >= 0) && (x < img->width))
00215                         {
00216                                 rotprev[theta] += (unsigned char)cvGet2D(img, y, x).val[0];
00217                                 rot_count[theta] ++;
00218                         }
00219                 }
00220         }
00221         for (int i=0; i<360; i++) {
00222                 rotprev[i] /= rot_count[i];
00223         }
00224         return conf1 + best_rot_factor;
00225 }
00226 
00227 void TrackerPsaRot::Compensate(double *x, double *y)
00228 {
00229         double xx = *x - (x_res/2);
00230         double yy = *y - (y_res/2);
00231         double kosini = cos(rotd*3.1415926535/180);
00232         double sini = sin(rotd*3.1415926535/180);
00233         *x = ((kosini * xx) - (sini * yy)) + xd + (x_res/2);
00234         *y = ((sini * xx) + (kosini * yy)) + yd + (y_res/2);
00235 }
00236 
00237 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sun Oct 5 2014 22:16:26