TrackerPsa.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
3  *
4  * Copyright 2007-2012 VTT Technical Research Centre of Finland
5  *
6  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
7  * <http://www.vtt.fi/multimedia/alvar.html>
8  *
9  * ALVAR is free software; you can redistribute it and/or modify it under the
10  * terms of the GNU Lesser General Public License as published by the Free
11  * Software Foundation; either version 2.1 of the License, or (at your option)
12  * any later version.
13  *
14  * This library is distributed in the hope that it will be useful, but WITHOUT
15  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17  * for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with ALVAR; if not, see
21  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
22  */
23 
24 #include "TrackerPsa.h"
25 
26 using namespace std;
27 
28 namespace alvar {
29 using namespace std;
30 
31 TrackerPsa::TrackerPsa(int _max_shift) {
32  max_shift = _max_shift;
33  x_res = 0; y_res = 0;
34  hor = 0; horprev = 0;
35  ver = 0; verprev = 0;
36  framecount = 0;
37 }
38 
39 TrackerPsa::~TrackerPsa() {
40  if (hor) delete [] hor;
41  if (horprev) delete [] horprev;
42  if (ver) delete [] ver;
43  if (verprev) delete [] verprev;
44 }
45 
46 double TrackerPsa::Track(IplImage *img) {
47  long best_x_factor=99999999;
48  long best_y_factor=99999999;
49  long worst_x_factor=0;
50  long worst_y_factor=0;
51  double quality=0;
52 
53  //std::cout<<"Moi1"<<std::endl;
54  // Init
55  if ((x_res != img->width) || (y_res != img->height)) {
56  x_res = img->width;
57  y_res = img->height;
58  if (hor) delete [] hor;
59  if (horprev) delete [] horprev;
60  if (ver) delete [] ver;
61  if (verprev) delete [] verprev;
62  hor = new long [x_res];
63  horprev = new long [x_res];
64  ver = new long [y_res];
65  verprev = new long [y_res];
66  framecount=0;
67  }
68  framecount++;
69 
70  // Make shift tables
71  memset(hor, 0, sizeof(long)*x_res);
72  memset(ver, 0, sizeof(long)*y_res);
73  for (int y=0; y<y_res; y++) {
74  for (int x=0; x<x_res; x++) {
75  unsigned char c = (unsigned char)cvGet2D(img, y, x).val[0];
76  hor[x] += c;
77  ver[y] += c;
78  }
79  }
80 
81  // If this is first frame -- no motion
82  if (framecount == 1) {
83  xd=0; yd=0;
84  }
85 
86  // Otherwais detect for motion
87  else {
88  // Sequence for s: 0, 1, -1, 2, -2, ... -(max_shift-1), (max_shift-1)
89  for (int s=0; s<max_shift; (s>0 ? s=-s : s=-s+1) ) {
90  long factor=0;
91  long factorfirst=0;
92  int count=0;
93  for (int x=0; x<x_res; x++) {
94  int x2 = x+s;
95  if ((x2 > 0) && (x2<x_res)) {
96  factor += labs(hor[x2] - horprev[x]);
97  count++;
98  }
99  }
100  factor /= count;
101  if (factor < best_x_factor) {
102  best_x_factor=factor;
103  xd=s;
104  }
105  if (factor > worst_x_factor) worst_x_factor=factor;
106  }
107  for (int s=0; s<max_shift; (s>0 ? s=-s : s=-s+1)) {
108  long factor=0;
109  int count=0;
110  for (int y=0; y<y_res; y++) {
111  int y2 = y+s;
112  if ((y2 > 0) && (y2<y_res)) {
113  factor += labs(ver[y2] - verprev[y]);
114  count++;
115  }
116  }
117  factor /= count;
118  if (factor < best_y_factor) {
119  best_y_factor=factor;
120  yd=s;
121  }
122  if (factor > worst_y_factor) worst_y_factor=factor;
123  }
124  // Figure out the quality?
125  // We assume the result is poor if the
126  // worst factor is very near the best factor
127  int qual_x = (worst_x_factor - best_x_factor)/y_res;
128  int qual_y = (worst_y_factor - best_y_factor)/x_res;
129  quality = std::min(qual_x, qual_y);
130  }
131 
132  // Swap memories
133  long *tmp;
134  tmp=hor; hor=horprev; horprev=tmp;
135  tmp=ver; ver=verprev; verprev=tmp;
136 
137  // Return confidence (bigger is better)
138  // Smaller than 10 is poor, and you almost certainly have
139  // problems with quality values less than 5.
140  //printf("%d\n", quality);
141  return quality;
142 }
143 
144 void TrackerPsa::Compensate(double *x, double *y) {
145  *x += xd; *y += yd;
146 }
147 
148 TrackerPsaRot::TrackerPsaRot(int _max_shift) : TrackerPsa(_max_shift) {
149  rotd = 0;
150  rot=new double [360];
151  rotprev=new double[360];
152  rot_count=new int[360];
153 }
154 
155 TrackerPsaRot::~TrackerPsaRot() {
156  if (rot) delete [] rot;
157  if (rotprev) delete [] rotprev;
158  if (rot_count) delete [] rot_count;
159 }
160 
161 double TrackerPsaRot::Track(IplImage *img) {
162  long best_rot_factor=99999999;
163  double conf1 = TrackerPsa::Track(img);
164 
165  if (framecount == 1) {
166  rotd=0;
167  }
168  else {
169  memset(rot, 0, sizeof(double)*360);
170  memset(rot_count, 0, sizeof(int)*360);
171  for (int y=0; y<y_res; y++) {
172  for (int x=0; x<x_res; x++) {
173  double y2 = y-(y_res/2)-(yd);
174  double x2 = x-(x_res/2)-(xd);
175  double r = sqrt((double)y2*y2 + x2*x2);
176  int theta = int(atan2((double)y2, (double)x2)*180/3.14159265);
177  if (theta < 0) theta+=360;
178  if ((y >= 0) && (y < img->height) &&
179  (x >= 0) && (x < img->width))
180  {
181  rot[theta] += (unsigned char)cvGet2D(img, y, x).val[0];
182  rot_count[theta] ++;
183  }
184  }
185  }
186  for (int i=0; i<360; i++) {
187  rot[i] /= rot_count[i];
188  }
189  for (int s=0; s<45; (s>0 ? s=-s : s=-s+1)) {
190  long factor=0;
191  for (int theta=0; theta<360; theta++) {
192  int theta2 = theta+s;
193  if (theta2 < 0) theta2+=360;
194  if (theta2 >= 360) theta2-=360;
195  factor += (long)labs(long(rot[theta2] - rotprev[theta]));
196  }
197  if (factor < best_rot_factor) {
198  best_rot_factor=factor;
199  rotd=s;
200  }
201  }
202  }
203  // Remember rotation based on center
204  memset(rotprev, 0, sizeof(double)*360);
205  memset(rot_count, 0, sizeof(int)*360);
206  for (int y=0; y<y_res; y++) {
207  for (int x=0; x<x_res; x++) {
208  double y2 = y-(y_res/2);
209  double x2 = x-(x_res/2);
210  double r = sqrt((double)y2*y2 + x2*x2);
211  int theta = int(atan2((double)y2, (double)x2)*180/3.14159265);
212  if (theta < 0) theta+=360;
213  if ((y >= 0) && (y < img->height) &&
214  (x >= 0) && (x < img->width))
215  {
216  rotprev[theta] += (unsigned char)cvGet2D(img, y, x).val[0];
217  rot_count[theta] ++;
218  }
219  }
220  }
221  for (int i=0; i<360; i++) {
222  rotprev[i] /= rot_count[i];
223  }
224  return conf1 + best_rot_factor;
225 }
226 
227 void TrackerPsaRot::Compensate(double *x, double *y)
228 {
229  double xx = *x - (x_res/2);
230  double yy = *y - (y_res/2);
231  double kosini = cos(rotd*3.1415926535/180);
232  double sini = sin(rotd*3.1415926535/180);
233  *x = ((kosini * xx) - (sini * yy)) + xd + (x_res/2);
234  *y = ((sini * xx) + (kosini * yy)) + yd + (y_res/2);
235 }
236 
237 } // namespace alvar
alvar
Main ALVAR namespace.
Definition: Alvar.h:174
alvar::TrackerPsa
TrackerPsa implements a very simple PSA tracker
Definition: TrackerPsa.h:42
alvar::TrackerPsa::yd
double yd
Track result y-translation in pixels
Definition: TrackerPsa.h:54
s
XmlRpcServer s
alvar::TrackerPsaRot::Track
double Track(IplImage *img)
Track using PSA with rotation.
Definition: TrackerPsa.cpp:161
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
height
int height
Definition: GlutViewer.cpp:160
alvar::TrackerPsa::xd
double xd
Track result x-translation in pixels
Definition: TrackerPsa.h:52
alvar::TrackerPsaRot::~TrackerPsaRot
~TrackerPsaRot()
Destructor.
Definition: TrackerPsa.cpp:155
TrackerPsa.h
This file implements a PSA tracker.
alvar::TrackerPsaRot::rotd
double rotd
Track result rotation in degrees
Definition: TrackerPsa.h:75
width
int width
Definition: GlutViewer.cpp:159
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
alvar::TrackerPsaRot::Compensate
virtual void Compensate(double *x, double *y)
Definition: TrackerPsa.cpp:227
r
r
alvar::TrackerPsa::Track
double Track(IplImage *img)
Track using PSA.
Definition: TrackerPsa.cpp:46

ar_track_alvar
Author(s): Scott Niekum
autogenerated on Mon Jun 10 2019 12:47:04