Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
Tracker.cpp
1 /*
2 This file is part of the BIAS library (Basic ImageAlgorithmS).
3 
4 Copyright (C) 2003-2009 (see file CONTACT for details)
5  Multimediale Systeme der Informationsverarbeitung
6  Institut fuer Informatik
7  Christian-Albrechts-Universitaet Kiel
8 
9 
10 BIAS is free software; you can redistribute it and/or modify
11 it under the terms of the GNU Lesser General Public License as published by
12 the Free Software Foundation; either version 2.1 of the License, or
13 (at your option) any later version.
14 
15 BIAS is distributed in the hope that it will be useful,
16 but WITHOUT ANY WARRANTY; without even the implied warranty of
17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 GNU Lesser General Public License for more details.
19 
20 You should have received a copy of the GNU Lesser General Public License
21 along with BIAS; if not, write to the Free Software
22 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 */
24 
25 
26 #include "Tracker.hh"
27 #include "TrackerBaseSimple.hh"
28 #include "TrackerBaseAffine.hh"
29 #include "TrackerBaseAffine2.hh"
30 #include "TrackerBaseHomography.hh"
31 #include "TrackerBaseWeighted.hh"
32 #include "TrackerBaseWeighted1D.hh"
33 #include <Base/Image/ImageIO.hh>
34 #include <Base/Common/BIASpragma.hh>
35 #include <Base/Image/ImageConvert.hh>
36 #include <Filter/GradientSobel3x3.hh>
37 #include <Filter/Binomial.hh>
38 
39  //#define TIMING
40 #ifdef TIMING
41 #include <Base/Debug/TimeMeasure.hh>
42 #endif
43 
44 using namespace BIAS;
45 using namespace std;
46 
47 #ifdef BIAS_DEBUG
48 double totalitersum=0.0;
49 #endif
50 
51 #define DFLT_PYSIZE 4
52 
53 
54 //////////////////////////////////////////////////////////////////////////
55 // implementation
56 //////////////////////////////////////////////////////////////////////////
57 
58 template <class StorageType, class CalculationType>
59 Tracker<StorageType, CalculationType>::
60 Tracker(int tracker_type) {
61  this->tb_ = NULL;
62  SetTrackerType(tracker_type);
63 
64  PyIndex_ = 0;
65 
66  MaxIter_.newsize(DFLT_PYSIZE);
67  for (int i=0; i<DFLT_PYSIZE; i++) MaxIter_[i]=5;
68 
69  HalfWinSize_.newsize(DFLT_PYSIZE);
70  for (int i=0; i<DFLT_PYSIZE; i++) HalfWinSize_[i]=3;
71 
72  MaxError_.newsize(DFLT_PYSIZE);
73  for (int i=0; i<DFLT_PYSIZE; i++) MaxError_[i]=0.1;
74 
75  MaxResiduumMAD_.newsize(DFLT_PYSIZE);
76  for (int i=0; i<DFLT_PYSIZE; i++) MaxResiduumMAD_[i]=10.0;
77 
78  UseCoarserPointIfLost_ = false;
79 
80  // filter masks
81  LowpassFM_ = FilterMask(Vector3<FM_FLOAT>( 1, 2, 1),
82  Vector3<FM_FLOAT>( 1, 2, 1));
83  GradXFM_ = FilterMask(Vector3<FM_FLOAT>(-1, 0, 1),
84  Vector3<FM_FLOAT>( 1, 2, 1));
85  GradYFM_ = FilterMask(Vector3<FM_FLOAT>( 1, 2, 1),
86  Vector3<FM_FLOAT>(-1, 0, 1));
87 
88  LastImage_ = NULL;
89  LastGradX_ = NULL;
90  LastGradY_ = NULL;
91 }
92 
93 
94 template <class StorageType, class CalculationType>
95 Tracker<StorageType, CalculationType>::
96 ~Tracker()
97 {
98  if (tb_)
99  delete tb_;
100 }
101 
102 template <class StorageType, class CalculationType>
103 void Tracker<StorageType, CalculationType>::
104 ReplaceLastImages(PyramidImageInterface<CalculationType> *pim,
105  PyramidImageInterface<CalculationType> *gradx,
106  PyramidImageInterface<CalculationType> *grady)
107 {
108  // BIASASSERT((pim && gradx && grady) || (!pim && !gradx && !grady));
109  // BIASASSERT(!pim || (pim->Size() == gradx->Size() &&
110  // pim->Size() == grady->Size()));
111 
112  if (LastImage_) { delete LastImage_; LastImage_ = NULL; }
113  if (LastGradX_) { delete LastGradX_; LastGradX_ = NULL; }
114  if (LastGradY_) { delete LastGradY_; LastGradY_ = NULL; }
115 
116  if (pim ){
117  LastImage_ = pim->ShallowClone();
118  }
119  if(gradx && grady) {
120  LastGradX_ = gradx->ShallowClone();
121  LastGradY_ = grady->ShallowClone();
122  }
123 }
124 
125 template <class StorageType, class CalculationType>
126 void Tracker<StorageType, CalculationType>::
127 ClearPoints()
128 {
129  Points_.clear();
130  PredictedPoints_.clear();
131  NumIter_.clear();
132  DisplInLastIter_.clear();
133  ResiduiMAD_.clear();
134  ResiduiMSD_.clear();
135  res_.clear(); // rabe, 11.11.2004
136  Cov_.clear();
137 }
138 
139 template <class StorageType, class CalculationType>
140 void Tracker<StorageType, CalculationType>::
141 ResizeInternals_(const int pysize)
142 {
143  if (Points_.size() != (unsigned)pysize){
144  Points_.resize(pysize);
145  PredictedPoints_.resize(pysize);
146  NumIter_.resize(pysize);
147  DisplInLastIter_.resize(pysize);
148  ResiduiMAD_.resize(pysize);
149  ResiduiMSD_.resize(pysize);
150  res_.resize(pysize);
151  Cov_.resize(pysize);
152  AffineResults_.resize(pysize);
153  }
154 }
155 
156 template <class StorageType, class CalculationType>
157 void Tracker<StorageType, CalculationType>::
158 ResizeInternals_(const int pysize, const int num_points)
159 {
160  ResizeInternals_(pysize);
161 
162  // dherzog: replaced '<numnew' by '!=numnew', because Track() asserts sizes
163  if ((int)Points_[PyIndex_].size() != num_points){
164  HomgPoint2D pz(42.0, 42.0, 1.0);
165  p2tracked_.resize(num_points, pz);
166  AffinePrediction_.resize(num_points);
167  for (int p = PyIndex_; p < pysize; p++){
168  res_[p].resize(num_points);
169  Points_[p].resize(num_points);
170  PredictedPoints_[p].resize(num_points);
171  NumIter_[p].resize(num_points);
172  DisplInLastIter_[p].resize(num_points);
173  ResiduiMAD_[p].resize(num_points);
174  ResiduiMSD_[p].resize(num_points);
175  Cov_[p].resize(num_points);
176  AffineResults_[p].resize(num_points);
177  }
178  }
179 }
180 
181 template <class StorageType, class CalculationType>
182 void Tracker<StorageType, CalculationType>::
183 ReserveInternals_(const int pysize, const int newsize)
184 {
185  int pyindex = PyIndex_;
186  for (int p=pyindex; p<pysize; p++){
187  res_[p].reserve(newsize);
188  Points_[p].reserve(newsize);
189  PredictedPoints_[p].reserve(newsize);
190  NumIter_[p].reserve(newsize);
191  DisplInLastIter_[p].reserve(newsize);
192  ResiduiMAD_[p].reserve(newsize);
193  ResiduiMSD_[p].reserve(newsize);
194  Cov_[p].reserve(newsize);
195  AffineResults_[p].resize(newsize);
196  }
197 }
198 
199 
200 template <class StorageType, class CalculationType>
201 void Tracker<StorageType, CalculationType>::
202 Reset()
203 {
204  ClearPoints();
205  ReplaceLastImages(NULL, NULL, NULL);
206 }
207 
208 template <class StorageType, class CalculationType>
209 void Tracker<StorageType, CalculationType>::
210 DetermineROI_(int &tlx, int &tly, int &brx, int &bry) const
211 {
212  BIASASSERT(LastImage_ && LastGradX_ && LastGradY_);
213  (*LastImage_)[PyIndex_]->GetROI()->GetCorners(tlx, tly, brx, bry);
214  int gxtlx, gxtly, gxbrx, gxbry;
215  (*LastGradX_)[PyIndex_]->GetROI()->GetCorners(gxtlx, gxtly, gxbrx, gxbry);
216  int gytlx, gytly, gybrx, gybry;
217  (*LastGradY_)[PyIndex_]->GetROI()->GetCorners(gytlx, gytly, gybrx, gybry);
218 
219  tlx = max(tlx, max(gxtlx, gytlx));
220  tly = max(tly, max(gxtly, gytly));
221  brx = min(brx, min(gxbrx, gybrx));
222  bry = min(bry, min(gxbry, gybry));
223 
224  tlx += HalfWinSize_[PyIndex_];
225  tly += HalfWinSize_[PyIndex_];
226  brx -= HalfWinSize_[PyIndex_];
227  bry -= HalfWinSize_[PyIndex_];
228 }
229 
230 
231 template <class StorageType, class CalculationType>
232 int Tracker<StorageType, CalculationType>::
233 FillUpPoints(const vector<HomgPoint2D>& points,
234  const vector<float>& qual)
235 {
236  // compute MinBorderDist
237  int lpx, lpy, gx, gy;
238  LowpassFM_.GetSize(lpx, lpy);
239  GradXFM_.GetSize(gx, gy);
240  int MinBorderDist =
241  max((lpx-1)/2, (lpy-1)/2) +
242  max((gx-1)/2, (gy-1)/2) +
243  HalfWinSize_[PyIndex_];
244 
245  //AddDebugLevel(D_TRACKER_FILL_UP);
246  BIASASSERT(LastImage_!=NULL);
247  BIASASSERT(Points_.size()>0);
248  const int pysize=(int)LastImage_->size();
249  const double factor=1.0/LastImage_->GetRescaleFactor();
250  const double offset=LastImage_->GetPositionOffset();
251  int numnew=points.size();
252  const int size=Points_[0].size();
253  const int pyindex=PyIndex_;
254  HomgPoint2D p2d;
255  Matrix2x2<double> zero2x2; zero2x2.SetZero();
256  BIASCDOUT(D_TRACKER_INIT, "min distance to image border "
257  <<MinBorderDist<<endl);
258  int tlx, tly, brx, bry;
259  (*LastImage_)[pyindex]->GetROI()->GetCorners(tlx, tly, brx, bry);
260  const double minx=MinBorderDist+tlx;
261  const double miny=MinBorderDist+tly;
262  const double maxx=brx-MinBorderDist;
263  const double maxy=bry-MinBorderDist;
264 
265 #ifdef BIAS_DEBUG
266  BIASCDOUT(D_TRACKER_FILL_UP, "minx" << minx << "miny" << miny <<
267  "maxx" << maxx << "maxy" << maxy << endl);
268 #endif
269  int n=0, num=0;
270  while (n<numnew && (points[n].IsAtInfinity() ||
271  !(points[n][0]>minx && points[n][1]>miny &&
272  points[n][0]<maxx && points[n][1]<maxy)))
273  n++;
274 #ifdef BIAS_DEBUG
275  if (n<numnew)
276  BIASCDOUT(D_TRACKER_FILL_UP, "next new point is "<<points[n]<<endl);
277 #endif
278  for (int i=0; i<size; i++){
279  if (n<numnew){
280  if (Points_[pyindex][i].IsAtInfinity()){
281  p2d=points[n];
282  p2d.Homogenize();
283  BIASCDOUT(D_TRACKER_FILL_UP, setw(3)<<i<<" : replacing "
284  <<Points_[pyindex][i]<<" with "<<p2d<<endl);
285  for (int p=pyindex; p<pysize; p++){
286  Points_[p][i]=p2d;
287  NumIter_[p][i]=-1;
288  DisplInLastIter_[p][i]=-1.0;
289  ResiduiMAD_[p][i]=-1.0;
290  ResiduiMSD_[p][i]=-1.0;
291  p2d[0]-=offset;
292  p2d[1]-=offset;
293  p2d[0]*=factor;
294  p2d[1]*=factor;
295  res_[p][i]=(int)rint(qual[n]);
296  Cov_[p][i]=zero2x2;
297  }
298  n++; num++;
299  while (n<numnew && ( points[n].IsAtInfinity() ||
300  !(points[n][0]>minx && points[n][1]>miny &&
301  points[n][0]<maxx && points[n][1]<maxy)))
302  n++;
303 #ifdef BIAS_DEBUG
304  if (n<numnew)
305  BIASCDOUT(D_TRACKER_FILL_UP, "next new point is "<<points[n]<<endl);
306 #endif
307  } else {
308 #ifdef BIAS_DEBUG
309  BIASCDOUT(D_TRACKER_FILL_UP, setw(3)<<i<<" : "<<Points_[pyindex][i]
310  <<" is still valid: ");
311  for (int p=pyindex+1; p<pysize; p++){
312  BIASCDOUT(D_TRACKER_FILL_UP, p<<": "<<Points_[p][i]);
313  }
314  BIASCDOUT(D_TRACKER_FILL_UP, endl);
315 #endif
316  }
317  } else {
318  num=-num;
319  BIASCDOUT(D_TRACKER_FILL_UP, "not enought points in points "
320  <<numnew<<endl);
321  break;
322  }
323  }
324  return num;
325 }
326 
327 
328 template <class StorageType, class CalculationType>
329 int Tracker<StorageType, CalculationType>::
330 ReplacePoints(const vector<HomgPoint2D>& points,
331  const vector<float>& qual)
332 {
333  assert("deprecated, uese ReplaceAllPoints instead" && false);
334  //AddDebugLevel(D_TRACKER_FILL_UP);
335  BIASASSERT(LastImage_!=NULL);
336  BIASASSERT(Points_.size()>0);
337  const int pysize=(int)LastImage_->size();
338  const double factor=1.0/LastImage_->GetRescaleFactor();
339  const double offset=LastImage_->GetPositionOffset();
340  const int numnew=points.size();
341 #ifdef BIASASSERT_ISACTIVE
342  const int size=Points_[0].size();
343  BIASASSERT(size==numnew);
344 #endif
345  const int pyindex=PyIndex_;
346  HomgPoint2D p2d;
347  Matrix2x2<double> zero2x2; zero2x2.SetZero();
348 
349  int n=0, num=0;
350  while (n<numnew){
351  while (n<numnew && points[n].IsAtInfinity()) n++;
352  if (n<numnew){
353 #ifdef BIAS_DEBUG
354  BIASCDOUT(D_TRACKER_FILL_UP, "next new point is "<<points[n]<<endl);
355 #endif
356  p2d=points[n];
357  p2d.Homogenize();
358  BIASCDOUT(D_TRACKER_FILL_UP, setw(3)<<n<<" : replacing "
359  <<Points_[pyindex][n]<<" with "<<p2d<<endl);
360  for (int p=pyindex; p<pysize; p++){
361  Points_[p][n]=p2d;
362  NumIter_[p][n]=-1;
363  DisplInLastIter_[p][n]=-1.0;
364  ResiduiMAD_[p][n]=-1.0;
365  ResiduiMSD_[p][n]=-1.0;
366  p2d[0]-=offset;
367  p2d[1]-=offset;
368  p2d[0]*=factor;
369  p2d[1]*=factor;
370  res_[p][n]=(int)rint(qual[n]);
371  Cov_[p][n]=zero2x2;
372  }
373  n++; num++;
374  }
375  }
376  BIASCDOUT(D_TRACKER_FILL_UP, "replaced "<<num<<" points"<<endl);
377  return n;
378 }
379 
380 
381 template <class StorageType, class CalculationType>
382 int Tracker<StorageType, CalculationType>::
383 ReplaceAllPoints(const vector<HomgPoint2D>& points,
384  const vector<float>& qual)
385 {
386  //AddDebugLevel(D_TRACKER_FILL_UP);
387  BIASASSERT(LastImage_!=NULL);
388  const int numnew=points.size();
389  const int pysize=(int)LastImage_->size();
390  const double factor=1.0/LastImage_->GetRescaleFactor();
391  const double offset=LastImage_->GetPositionOffset();
392  const int pyindex=PyIndex_;
393  // dherzog: replaced '<numnew' by '!=numnew', because Track() asserts sizes
394  if ((int)Points_.size() != pysize || (int)Points_[pyindex].size()!=numnew) {
395  ClearPoints();
396  ResizeInternals_(pysize, numnew);
397  }
398 
399  HomgPoint2D p2d;
400  Matrix2x2<double> zero2x2; zero2x2.SetZero();
401 
402  for (int n=0; n<numnew; n++){
403  p2d = points[n];
404  p2d.Homogenize();
405  BIASASSERT(!p2d.IsAtInfinity());
406  BIASCDOUT(D_TRACKER_FILL_UP, setw(3)<<n<<" : replacing "
407  <<Points_[pyindex][n]);
408  for (int p=pyindex; p<pysize; p++){
409  res_[p][n] = (int)rint(qual[n]);
410  Points_[p][n] = p2d;
411  PredictedPoints_[p][n] = p2d;
412  BIASASSERT(!Points_[p][n].IsAtInfinity());
413  BIASASSERT(!PredictedPoints_[p][n].IsAtInfinity());
414  NumIter_[p][n] = -1;
415  DisplInLastIter_[p][n] = -1.0;
416  ResiduiMAD_[p][n] = -1.0;
417  ResiduiMSD_[p][n] = -1.0;
418  Cov_[p][n] = zero2x2;
419 
420  p2d[0] -= offset;
421  p2d[1] -= offset;
422  p2d[0] *= factor;
423  p2d[1] *= factor;
424  }
425  BIASCDOUT(D_TRACKER_FILL_UP, " with "<<Points_[pyindex][n]<<endl);
426  }
427  BIASCDOUT(D_TRACKER_FILL_UP, "replaced "<<numnew<<" points"<<endl);
428 
429  PadPoints_(numnew);
430 
431  return numnew;
432 }
433 
434 
435 #ifdef BIAS_DEBUG
436 static int Tracker_GlobalImageCounter=0;
437 #endif
438 
439 template <class StorageType, class CalculationType>
440 int Tracker<StorageType, CalculationType>::
441 PreparePyramide(const Image<StorageType>& im,
442  PyramidImageInterface<CalculationType>& pim,
443  PyramidImageInterface<CalculationType>& gx,
444  PyramidImageInterface<CalculationType>& gy,
445  FilterNTo2N<CalculationType, CalculationType>& grad,
446  FilterNToN<StorageType, CalculationType>& lowpass)
447 {
448 #ifdef TIMING
449  TimeMeasure timer;
450  timer.Start();
451 #endif
452  int fres=0, gres=0;
453 #ifdef BIAS_DEBUG
454  BIASASSERT(im.GetChannelCount()==1);
455  BIASASSERT(im.GetColorModel()==ImageBase::CM_Grey);
456 #endif
457  // cout << "pim before init:\n";
458  // pim.Dump();
459 
460  const unsigned py_size = pim.size();
461  if (pim.size()==0 || !im.SamePixelAndChannelCount(*pim[0])){
462  if (!pim.IsEmpty()) pim.Clear();
463  BIAS::Image<CalculationType> im2;
464  ImageConvert::ConvertST(im,im2,im2.GetStorageType());
465  pim.Init(im2,py_size);
466  //pim.Init(im.GetWidth(), im.GetHeight(), 1, py_size); //this doesn'T create any image surfaces anymore...
467  pim.SetZero();
468  }
469  if (gx.size()==0 || !im.SamePixelAndChannelCount(*gx[0])){
470  if (!gx.IsEmpty()) gx.Clear();
471  Image<CalculationType> gr(im.GetWidth(),im.GetHeight()); //dummy image
472  gx.Init(gr,py_size);
473  //gx.Init(im.GetWidth(), im.GetHeight(), 1, py_size);
474  gx.SetZero();
475  }
476  if (gy.size()==0 || !im.SamePixelAndChannelCount(*gy[0])){
477  if (!gy.IsEmpty()) gy.Clear();
478  Image<CalculationType> gr(im.GetWidth(),im.GetHeight()); //dummy image
479  gy.Init(gr,py_size);
480  //gy.Init(im.GetWidth(), im.GetHeight(), 1, py_size);
481  gy.SetZero();
482  }
483 
484  // cout << "pim after init:\n";
485  // pim.Dump();
486 
487 #ifdef TIMING
488  timer.Stop();
489  cerr << "initializing pyramid images took "<<timer.GetRealTime()<<" us\n";
490  timer.Reset();
491  timer.Start();
492 #endif
493 
494 #ifdef BIAS_DEBUG
495  ostringstream os;
496  if (DebugLevelIsSet(D_TRACKER_WRITE_IM)){
497  os.str("");
498  os << "input-im"<<setw(3)<<setfill('0')<<Tracker_GlobalImageCounter
499  <<".mip";
500  Image<StorageType> tmp_im=im;
501  //if (ImageIO::Save(os.str(), tmp_im)!=0){
502  if (ImageIO::Save(os.str(), tmp_im)!=0){
503  BIASERR("error writing "<<os.str());
504  }
505  }
506 #endif
507 
508  //lowpass_->AddDebugLevel(D_CONV_KERNEL);
509  //lowpass_->AddDebugLevel(D_FILTERBASE_CALLSTACK);
510  //lowpass_->AddDebugLevel(D_WRITE_IMAGES);
511  if((fres=lowpass.Filter(im, *(pim[0])))!=0){
512  BIASERR("filtering failed "<<fres);
513  }
514  // lowpass_->PrintKernel();
515 
516 #ifdef BIAS_DEBUG
517  if (DebugLevelIsSet(D_TRACKER_WRITE_IM)){
518  os.str("");
519  os << "pyim-lowpass-im"<<setw(3)<<setfill('0')<<Tracker_GlobalImageCounter
520  << "-pyind"<<setw(1)<<setfill('0')<<0<<".mip";
521  //if (ImageIO::Save(os.str(), *pim[0])!=0){
522  if (ImageIO::Save(os.str(), *pim[0])!=0){
523  BIASERR("error writing "<<os.str());
524  }
525  }
526 #endif
527 
528  //lowpass_->RemoveDebugLevel(D_CONV_KERNEL);
529 #ifdef TIMING
530  timer.Stop();
531  cerr << "lowpass took "<<timer.GetRealTime()<<" us\n";
532  timer.Reset();
533  timer.Start();
534 #endif
535 
536  pim.Downsample();
537 
538 #ifdef TIMING
539  timer.Stop();
540  cerr << "downsampling took "<<timer.GetRealTime()<<" us\n";
541  timer.Reset();
542  timer.Start();
543 #endif
544 
545  const int pysize=pim.size(); // comes from parameter
546  for (int p=0; p<pysize; p++){
547  if((gres=grad.Filter(*pim[p], *gx[p], *gy[p]))!=0){
548  BIASERR("gradient failed "<<gres);
549  break;
550  }
551 #ifdef BIAS_DEBUG
552  if (DebugLevelIsSet(D_TRACKER_WRITE_IM)){
553  os.str("");
554  os << "pyim-im"<<setw(3)<<setfill('0')<<Tracker_GlobalImageCounter
555  << "-pyind"<<setw(1)<<setfill('0')<<p<<".mip";
556  //if (ImageIO::Save(os.str(), *pim[p])!=0){
557  if (ImageIO::Save(os.str(), *pim[p])!=0){
558  BIASERR("error writing "<<os.str());
559  }
560  os.str("");
561  os << "pygx-im"<<setw(3)<<setfill('0')<<Tracker_GlobalImageCounter
562  << "-pyind"<<setw(1)<<setfill('0')<<p<<".mip";
563  //if (ImageIO::Save(os.str(), *gx[p])!=0){
564  if (ImageIO::Save(os.str(), *gx[p])!=0){
565  BIASERR("error writing "<<os.str());
566  }
567  os.str("");
568  os << "pygy-im"<<setw(3)<<setfill('0')<<Tracker_GlobalImageCounter
569  << "-pyind"<<setw(1)<<setfill('0')<<p<<".mip";
570  //if (ImageIO::Save(os.str(), *gy[p])!=0){
571  if (ImageIO::Save(os.str(), *gy[p])!=0){
572  BIASERR("error writing "<<os.str());
573  }
574  }
575 #endif
576  }
577 #ifdef BIAS_DEBUG
578  Tracker_GlobalImageCounter++;
579 #endif
580 #ifdef TIMING
581  timer.Stop();
582  cerr << "gradients took "<<timer.GetRealTime()<<" us\n";
583  timer.Reset();
584  timer.Start();
585 #endif
586  return (fres==0 && gres==0)?0:-1;
587 }
588 
589 template <class StorageType, class CalculationType>
590 int Tracker<StorageType, CalculationType>::
591 PreparePyramide(const Image<StorageType>& im,
592  PyramidImageInterface<CalculationType>& pim)
593 {
594 #ifdef BIAS_DEBUG
595  BIASASSERT(im.GetChannelCount()==1);
596  BIASASSERT(im.GetColorModel()==ImageBase::CM_Grey);
597 #endif
598 
599  const unsigned py_size = pim.size();
600  if (pim.size()==0 || !im.SamePixelAndChannelCount(*pim[0])){
601  if (!pim.IsEmpty()) pim.Clear();
602  pim.Init(im.GetWidth(), im.GetHeight(), 1, py_size);
603  pim.SetZero();
604  }
605 
606 
607  if (im.GetStorageType()==pim[0]->GetStorageType()) {
608  *pim[0] = im;
609  } else {
610  ImageConvert::ConvertST(im, *pim[0], pim[0]->GetStorageType());
611  }
612 
613  pim.Downsample();
614 
615  return 0;
616 }
617 
618 template <class StorageType, class CalculationType>
619 int Tracker<StorageType, CalculationType>::
620 PreparePyramide(const Image<StorageType>& im,
621  PyramidImageInterface<CalculationType>& pim,
622  PyramidImageInterface<CalculationType>& gx,
623  PyramidImageInterface<CalculationType>& gy)
624 {
625  GradientSobel3x3<CalculationType, CalculationType> grad;
626  Binomial<StorageType, CalculationType> lowpass;
627  return PreparePyramide(im, pim, gx, gy, grad, lowpass);
628 }
629 
630 template <class StorageType, class CalculationType>
631 int Tracker<StorageType, CalculationType>::
632 Track(PyramidImageInterface<CalculationType>& pim)
633 {
634  return Track(pim, Points_[PyIndex_]);
635 }
636 
637 template <class StorageType, class CalculationType>
638 int Tracker<StorageType, CalculationType>::
639 Track(PyramidImageInterface<CalculationType>& pim,
640  const vector<HomgPoint2D>& prediction,
641  const vector<Matrix2x2<KLT_TYPE> >& AffinePred)
642 {
643 #ifdef TIMING
644  TimeMeasure timer;
645  timer.Start();
646 #endif
647  int num=0;
648  const int pysize=(int)pim.size();
649  const double factor=pim.GetRescaleFactor();
650  const double offset=pim.GetPositionOffset();
651  const Matrix2x2<double> K(factor, 0.0, 0.0, factor);
652  //tb_->AddDebugLevel(D_TRACKERB_GKLT);
653  //tb_->AddDebugLevel(D_TRACKERB_INIT);
654  //tb_->AddDebugLevel(D_TRACKERB_REJECT);
655 
656  BIASASSERT(PyIndex_>=0 && PyIndex_<pysize);
657 
658  if (LastImage_){
659 #ifdef BIAS_DEBUG
660  if ((&pim)==LastImage_){
661  BIASERR("do not remove image between successive calls of Track\n"
662  <<&pim<<"!="<<LastImage_);
663  // BIASERR("do not remove image between successive calls of Track\n"
664  // <<(int)(&pim)<<"!="<<(int)(LastImage_));
665  }
666  BIASASSERT((&pim)!=LastImage_);
667  // AddDebugLevel(D_TRACKER_IMAGES);
668  if (DebugLevelIsSet(D_TRACKER_IMAGES)){
669  pim.WriteImages("act-py"); LastImage_->WriteImages("last-py");
670  }
671 #endif
672 
673  const int pyindex=PyIndex_;
674  const int nump=Points_[pyindex].size();
675 
676  // fill PredictedPoints_ and Points_
677  PredictedPoints_[pyindex]=prediction;
678 
679  const double ifac=1.0/factor;
680  for (int p=pyindex+1; p<pysize; p++){
681  for (int i=0; i<nump; i++){
682  Points_[p][i][0]=(Points_[p-1][i][0]-offset)*ifac;
683  Points_[p][i][1]=(Points_[p-1][i][1]-offset)*ifac;
684  PredictedPoints_[p][i][0]=(PredictedPoints_[p-1][i][0]-offset)*ifac;
685  PredictedPoints_[p][i][1]=(PredictedPoints_[p-1][i][1]-offset)*ifac;
686  }
687  }
688 
689  const bool Affine = (dynamic_cast<TrackerBaseAffine<CalculationType>*>(tb_) != NULL) || (dynamic_cast<TrackerBaseAffine2<CalculationType>*>(tb_) != NULL) ;
690  if (Affine) {
691  if ((int)AffinePred.size()==nump) {
692  // copy from vector
693  AffinePrediction_ = AffinePred;
694  } else {
695  // have no prediction, start with identity
696  AffinePrediction_.resize(nump);
697  for (int i=0; i<nump; i++) {
698  AffinePrediction_[i].SetIdentity();
699  }
700  }
701  }
702 
703 #ifdef TIMING
704  timer.Stop();
705  cerr << "initializing "<<nump<<" points took "<<timer.GetRealTime()<<" us\n";
706  timer.Reset();
707  timer.Start();
708 #endif
709 
710  // now do the actual tracking, starting on smallest pyramid level
711  for (int p=pysize-1;p>=pyindex; p--){
712  BIASCDOUT(D_TRACKER_TRACK, "---- pyramid level "<<p<<" ----\n");
713 
714  tb_->SetHalfWinSize(HalfWinSize_[p]);
715  tb_->SetMaxIterations(MaxIter_[p]);
716  tb_->SetMaxError(MaxError_[p]);
717  tb_->SetMaxResiduumMAD(MaxResiduumMAD_[p]);
718 
719  // check if size of weight matrices match pyramid size
720  if ((int)WeightMatrices_.size() == pysize)
721  {
722  tb_->SetWeightMatrix(WeightMatrices_[p]);
723  }
724 
725 
726  if (DebugLevelIsSet(D_TRACKER_IMAGES)){
727  pim.WriteImages("act1-py"); LastImage_->WriteImages("last1-py");
728  }
729  tb_->Init(*(*LastImage_)[p], *pim[p], LowpassFM_,
730  GradXFM_, GradYFM_);
731 #ifdef TIMING
732  timer.Stop();
733  cerr << "initializing tb took "<<timer.GetRealTime()<<" us\n";
734  timer.Reset();
735  timer.Start();
736 #endif
737 
738  tb_->Track(Points_[p], PredictedPoints_[p], p2tracked_,
739  DisplInLastIter_[p], NumIter_[p], ResiduiMAD_[p],
740  ResiduiMSD_[p], res_[p],Cov_[p], AffinePrediction_,
741  &AffineResults_[p]);
742 
743  if (Affine) {
744  // update prediction with last estimate
745  AffinePrediction_ = AffineResults_[p];
746  }
747 
748 #ifdef TIMING
749  timer.Stop();
750  cerr << "tracking tb took "<<timer.GetRealTime()<<" us\n";
751  timer.Reset();
752  timer.Start();
753 #endif
754 
755  // check for result
756  for (int i=0; i<nump; i++){
757  BIASCDOUT(D_TRACKER_TRACK, setw(3)<<i<<" ("<<Points_[p][i][0]<<", "
758  <<Points_[p][i][1]<<") <--> ("<<PredictedPoints_[p][i][0]
759  <<", "<<PredictedPoints_[p][i][1]<<") ");
760  if (res_[p][i]>=0){
761  Points_[p][i]=p2tracked_[i];
762  BIASCDOUT(D_TRACKER_TRACK, " -> ("<<p2tracked_[i][0]<<", "
763  <<p2tracked_[i][1]<<") in "<<NumIter_[p][i]
764  <<" iterations with err "<<DisplInLastIter_[p][i]
765  <<" code "<<res_[p][i]
766  <<" residuumMAD "<<ResiduiMAD_[p][i]
767  <<" residuumMSD "<<ResiduiMSD_[p][i]<<endl);
768  if (p==pyindex) {
769  num++;
770  } else {
771  PredictedPoints_[p-1][i][0]=Points_[p][i][0]*factor+offset;
772  PredictedPoints_[p-1][i][1]=Points_[p][i][1]*factor+offset;
773  }
774  } else { // if (res_[p][i]>=0){
775  BIASCDOUT((D_TRACKER_ERROR | D_TRACKER_TRACK), " error tracking: "
776  <<Points_[p][i]<<" on py level "<<p<<" result code "
777  <<res_[p][i]<<endl);
778  // cerr <<" error tracking: "<<res_[p][i]<<endl;
779  // cerr << setw(3)<<i<<" ("<<Points_[p][i][0]<<", "
780  // <<Points_[p][i][1]<<") <--> ("<<PredictedPoints_[p][i][0]
781  // <<", "<<PredictedPoints_[p][i][1]<<") "<<endl;
782 
783  if ( UseCoarserPointIfLost_ &&
784  (p!=(pysize-1) && res_[p+1][i]>=0 && p!=pyindex) ) {
785  PredictedPoints_[p-1][i][0]=Points_[p+1][i][0]*factor+offset;
786  PredictedPoints_[p-1][i][1]=Points_[p+1][i][1]*factor+offset;
787 
788  if (res_[p+1][i]!=0){
789  res_[p][i]=res_[p+1][i];
790  NumIter_[p][i]=NumIter_[p+1][i];
791  DisplInLastIter_[p][i]=DisplInLastIter_[p+1][i]*factor;
792  ResiduiMAD_[p][i]=ResiduiMAD_[p+1][i];
793  ResiduiMSD_[p][i]=ResiduiMSD_[p+1][i];
794  // if a random variable is transformed by a linear function
795  // with matrix F, the covariance transformed as J*cov*J^T,
796  // where J is the jacobian of F. Here the Jacobian is a
797  // matrix where the only entries on the main diagonal are the
798  // factors
799  Cov_[p][i]=(factor*factor)*Cov_[p+1][i];
800  }
801  } else {
802  // manifest the failure in homogenous coordinate
803  for (int k=pyindex; k<pysize; k++)
804  Points_[k][i][2]=0.0;
805  }
806  } // if (res_[p][i]>=0){
807  }
808 
809 #ifdef TIMING
810  timer.Stop();
811  cerr << "checking results of tb took "<<timer.GetRealTime()<<" us\n";
812  timer.Reset();
813  timer.Start();
814 #endif
815 
816 #ifdef BIAS_DEBUG
817  if (DebugLevelIsSet(D_TRACKER_NUMITER)){
818  //check number of iterations
819  double itersum=0.0;
820  for (unsigned h=0; h<NumIter_[p].size(); h++)
821  itersum+=NumIter_[p][h];
822  //cout << p << " average number of iterations "
823  // <<itersum/(double)NumIter_[p].size()<<endl;
824  totalitersum+=itersum;
825  BIASCDOUT(D_TRACKER_NUMITER, "totalitersum "<<totalitersum<<endl);
826  }
827 #endif
828  }
829 
830  //DumpResults(); cerr << endl << endl;
831 
832 #ifdef TIMING
833  timer.Stop();
834  cerr << "propagating result codes through pyramide took "<<timer.GetRealTime()<<" us\n";
835  timer.Reset();
836  timer.Start();
837 #endif
838 
839 
840 #ifdef BIAS_DEBUG
841  if (DebugLevelIsSet(D_TRACKER_WRITE_IM)){
842  Image<CalculationType> wim;
843  Image<CalculationType> gim;
844  pim.GetSingleImage(wim);
845  //ImageIO::Save("py2.mip", wim);
846  ImageIO::Save("py2.mip", wim);
847  LastImage_->GetSingleImage(wim);
848  //ImageIO::Save("py1.mip", wim);
849  ImageIO::Save("py1.mip", wim);
850  }
851 #endif
852 
853 
854  } else {
855  BIASCDOUT(D_TRACKER_TRACK, "initialization call\n");
856  }
857 
858  ReplaceLastImages(&pim, NULL, NULL);
859 
860 #ifdef TIMING
861  timer.Stop();
862  cerr << "updateing iternals took "<<timer.GetRealTime()<<" us\n";
863 #endif
864 
865 #ifdef BIAS_DEBUG
866  //WriteTrackLog_(num);
867 #endif
868 
869  return num;
870 }
871 
872 template <class StorageType, class CalculationType>
873 int Tracker<StorageType, CalculationType>::
874 Track(PyramidImageInterface<CalculationType>& pim,
875  PyramidImageInterface<CalculationType>& gradx,
876  PyramidImageInterface<CalculationType>& grady)
877 {
878  return Track(pim, gradx, grady, Points_[PyIndex_]);
879 }
880 
881 template <class StorageType, class CalculationType>
882 int Tracker<StorageType, CalculationType>::
883 Track(PyramidImageInterface<CalculationType>& pim,
884  PyramidImageInterface<CalculationType>& gradx,
885  PyramidImageInterface<CalculationType>& grady,
886  const vector<HomgPoint2D>& prediction,
887  const vector<Matrix2x2<KLT_TYPE> >& AffinePred)
888 {
889 #ifdef TIMING
890  TimeMeasure timer;
891  timer.Start();
892 #endif
893  int num=0;
894  const int pysize=(int)pim.size();
895  const double factor=pim.GetRescaleFactor();
896  const double offset=pim.GetPositionOffset();
897  const Matrix2x2<double> K(factor, 0.0, 0.0, factor);
898  //tb_->AddDebugLevel(D_TRACKERB_GKLT);
899  //tb_->AddDebugLevel(D_TRACKERB_INIT);
900  //tb_->AddDebugLevel(D_TRACKERB_REJECT);
901 
902  BIASASSERT(PyIndex_>=0 && PyIndex_<pysize);
903 
904  if (LastImage_ && LastGradX_ && LastGradY_){
905 #ifdef BIAS_DEBUG
906  if ((&pim)==LastImage_ || (&gradx)==LastGradX_ || (&grady)==LastGradY_ ){
907  /*
908  BIASERR("do not remove image between successive calls of Track\n"
909  <<(int)(&pim)<<"!="<<(int)(LastImage_)<<","<<(int)(&gradx)<<"!="
910  <<(int)(LastGradX_)<<","<<(int)(&grady)<<"!="<<(int)(LastGradY_));
911  */
912  BIASERR("do not remove image between successive calls of Track\n"
913  <<&pim<<"!="<<LastImage_<<","<<&gradx<<"!="
914  <<LastGradX_<<","<<&grady<<"!="<<LastGradY_);
915  }
916  BIASASSERT((&pim)!=LastImage_ && (&gradx)!=LastGradX_ &&
917  (&grady)!=LastGradY_ );
918  if (DebugLevelIsSet(D_TRACKER_IMAGES)){
919  pim.WriteImages("act-py"); LastImage_->WriteImages("last-py");
920  gradx.WriteImages("act-gx"); LastGradX_->WriteImages("last-gx");
921  grady.WriteImages("act-gy"); LastGradY_->WriteImages("last-gy");
922  }
923 #endif
924 
925  //cout << Points_.size() << endl;
926  //cout << PyIndex_ << endl;
927 
928  const int pyindex=PyIndex_;
929  const int nump=Points_[pyindex].size();
930 
931  // fill PredictedPoints_ and Points_
932  PredictedPoints_[pyindex]=prediction;
933 
934  const double ifac=1.0/factor;
935  for (int p=pyindex+1; p<pysize; p++){
936  for (int i=0; i<nump; i++){
937  Points_[p][i][0]=(Points_[p-1][i][0]-offset)*ifac;
938  Points_[p][i][1]=(Points_[p-1][i][1]-offset)*ifac;
939  PredictedPoints_[p][i][0]=(PredictedPoints_[p-1][i][0]-offset)*ifac;
940  PredictedPoints_[p][i][1]=(PredictedPoints_[p-1][i][1]-offset)*ifac;
941  }
942  }
943 
944  const bool Affine = (dynamic_cast<TrackerBaseAffine<CalculationType>*>(tb_) != NULL) ||(dynamic_cast<TrackerBaseAffine2<CalculationType>*>(tb_) != NULL);
945  if (Affine) {
946  if ((int)AffinePred.size()==nump) {
947  // copy from vector
948  AffinePrediction_ = AffinePred;
949  } else {
950  // have no prediction, start with identity
951  AffinePrediction_.resize(nump);
952  for (int i=0; i<nump; i++) {
953  AffinePrediction_[i].SetIdentity();
954  }
955  }
956  }
957 
958 #ifdef TIMING
959  timer.Stop();
960  cerr << "initializing "<<nump<<" points took "<<timer.GetRealTime()<<" us\n";
961  timer.Reset();
962  timer.Start();
963 #endif
964 
965  // now do the actual tracking, starting on smalles pyramid level
966  for (int p=pysize-1;p>=pyindex; p--){
967  BIASCDOUT(D_TRACKER_TRACK, "---- pyramid level "<<p<<" ----\n");
968 
969  tb_->SetHalfWinSize(HalfWinSize_[p]);
970  tb_->SetMaxIterations(MaxIter_[p]);
971  tb_->SetMaxError(MaxError_[p]);
972  tb_->SetMaxResiduumMAD(MaxResiduumMAD_[p]);
973 
974  // check if size of weight matrices match pyramid size
975  if ((int)WeightMatrices_.size() == pysize)
976  {
977  tb_->SetWeightMatrix(WeightMatrices_[p]);
978  }
979 
980  tb_->Init(*(*LastImage_)[p], *pim[p], *(*LastGradX_)[p],
981  *(*LastGradY_)[p], *gradx[p], *grady[p]);
982 #ifdef TIMING
983  timer.Stop();
984  cerr << "initializing tb took "<<timer.GetRealTime()<<" us\n";
985  timer.Reset();
986  timer.Start();
987 #endif
988 
989  tb_->Track(Points_[p], PredictedPoints_[p], p2tracked_,
990  DisplInLastIter_[p], NumIter_[p], ResiduiMAD_[p],
991  ResiduiMSD_[p], res_[p],Cov_[p], AffinePrediction_,
992  &AffineResults_[p]);
993 
994  if (Affine) {
995  // update prediction with last estimate
996  AffinePrediction_ = AffineResults_[p];
997  }
998 
999 #ifdef TIMING
1000  timer.Stop();
1001  cerr << "tracking tb took "<<timer.GetRealTime()<<" us\n";
1002  timer.Reset();
1003  timer.Start();
1004 #endif
1005 
1006  // check for result
1007  for (int i=0; i<nump; i++){
1008  BIASCDOUT(D_TRACKER_TRACK, setw(3)<<i<<" ("<<Points_[p][i][0]<<", "
1009  <<Points_[p][i][1]<<") <--> ("<<PredictedPoints_[p][i][0]
1010  <<", "<<PredictedPoints_[p][i][1]<<") ");
1011  if (res_[p][i]>=0){
1012  Points_[p][i]=p2tracked_[i];
1013  BIASCDOUT(D_TRACKER_TRACK, " -> ("<<p2tracked_[i][0]<<", "
1014  <<p2tracked_[i][1]<<") in "<<NumIter_[p][i]
1015  <<" iterations with err "<<DisplInLastIter_[p][i]
1016  <<" code "<<res_[p][i]
1017  <<" residuumMAD "<<ResiduiMAD_[p][i]
1018  <<" residuumMSD "<<ResiduiMSD_[p][i]
1019  <<" cov "<<Cov_[p][i] <<endl);
1020  if (p==pyindex) {
1021  num++;
1022  } else {
1023  PredictedPoints_[p-1][i][0]=Points_[p][i][0]*factor+offset;
1024  PredictedPoints_[p-1][i][1]=Points_[p][i][1]*factor+offset;
1025  }
1026  } else { // if (res_[p][i]>=0){
1027  BIASCDOUT((D_TRACKER_ERROR | D_TRACKER_TRACK), " error tracking: "
1028  <<Points_[p][i]<<" on py level "<<p<<" result code "
1029  <<res_[p][i]<<endl);
1030  // cerr <<" error tracking: "<<res_[p][i]<<endl;
1031  // cerr << setw(3)<<i<<" ("<<Points_[p][i][0]<<", "
1032  // <<Points_[p][i][1]<<") <--> ("<<PredictedPoints_[p][i][0]
1033  // <<", "<<PredictedPoints_[p][i][1]<<") "<<endl;
1034  if (!UseCoarserPointIfLost_) {
1035  // manifest the failure in homogenous coordinate
1036  // do manifest it in all pyramid levels smaller than actual
1037  // the bigger pyramid levels are taken care of by code below
1038  for (int k=p; k<pysize; k++)
1039  Points_[k][i][2]=0.0;
1040  } else {
1041  // try to propagate results from last successfull track
1042  if (p!=(pysize-1) && res_[p+1][i]>=0){
1043  if (p!=pyindex) {
1044  PredictedPoints_[p-1][i][0]=Points_[p+1][i][0]*factor+offset;
1045  PredictedPoints_[p-1][i][1]=Points_[p+1][i][1]*factor+offset;
1046  }
1047  if (res_[p+1][i]!=0){
1048  res_[p][i]=res_[p+1][i];
1049  NumIter_[p][i]=NumIter_[p+1][i];
1050  DisplInLastIter_[p][i]=DisplInLastIter_[p+1][i]*factor;
1051  ResiduiMAD_[p][i]=ResiduiMAD_[p+1][i];
1052  ResiduiMSD_[p][i]=ResiduiMSD_[p+1][i];
1053  // if a random variable is transformed by a linear function with
1054  // matrix F, the covariance transformed as J*cov*J^T, where J is
1055  // the jacobian of F. Here the Jacobian is a matrix where the
1056  // only entries on the main diagonal are the factors
1057  Cov_[p][i]=(factor*factor)*Cov_[p+1][i];
1058  }
1059  }
1060  }
1061  } // if (res_[p][i]>=0){
1062  }
1063 
1064 #ifdef TIMING
1065  timer.Stop();
1066  cerr << "checking results of tb took "<<timer.GetRealTime()<<" us\n";
1067  timer.Reset();
1068  timer.Start();
1069 #endif
1070 
1071 #ifdef BIAS_DEBUG
1072  if (DebugLevelIsSet(D_TRACKER_NUMITER)){
1073  //check number of iterations
1074  double itersum=0.0;
1075  for (unsigned h=0; h<NumIter_[p].size(); h++)
1076  itersum+=NumIter_[p][h];
1077  //cout << p << " average number of iterations "
1078  // <<itersum/(double)NumIter_[p].size()<<endl;
1079  totalitersum+=itersum;
1080  BIASCDOUT(D_TRACKER_NUMITER, "totalitersum "<<totalitersum<<endl);
1081  }
1082 #endif
1083  }
1084 
1085  //DumpResults(); cerr << endl << endl;
1086 
1087 #ifdef TIMING
1088  timer.Stop();
1089  cerr << "propagating result codes through pyramide took "<<timer.GetRealTime()<<" us\n";
1090  timer.Reset();
1091  timer.Start();
1092 #endif
1093 
1094 
1095 #ifdef BIAS_DEBUG
1096  if (DebugLevelIsSet(D_TRACKER_WRITE_IM)){
1097  Image<CalculationType> wim;
1098  Image<CalculationType> gim;
1099  pim.GetSingleImage(wim);
1100  //ImageIO::Save("py2.mip", wim);
1101  ImageIO::Save("py2.mip", wim);
1102  LastImage_->GetSingleImage(wim);
1103  //ImageIO::Save("py1.mip", wim);
1104  ImageIO::Save("py1.mip", wim);
1105  gradx.GetSingleImage(gim);
1106  //ImageIO::Save("gradx2.mip", gim);
1107  ImageIO::Save("gradx2.mip", gim);
1108  LastGradX_->GetSingleImage(gim);
1109  //ImageIO::Save("gradx1.mip", gim);
1110  ImageIO::Save("gradx1.mip", gim);
1111  grady.GetSingleImage(gim);
1112  //ImageIO::Save("grady2.mip", gim);
1113  ImageIO::Save("grady2.mip", gim);
1114  LastGradY_->GetSingleImage(gim);
1115  //ImageIO::Save("grady1.mip", gim);
1116  ImageIO::Save("grady1.mip", gim);
1117  }
1118 #endif
1119 
1120 
1121  } else {
1122  BIASCDOUT(D_TRACKER_TRACK, "initialization call\n");
1123  }
1124 
1125  ReplaceLastImages(&pim, &gradx, &grady);
1126 
1127 #ifdef TIMING
1128  timer.Stop();
1129  cerr << "updateing iternals took "<<timer.GetRealTime()<<" us\n";
1130 #endif
1131 
1132 #ifdef BIAS_DEBUG
1133  //WriteTrackLog_(num);
1134 #endif
1135 
1136  return num;
1137 }
1138 
1139 
1140 template <class StorageType, class CalculationType>
1141 void Tracker<StorageType, CalculationType>::
1142 WriteTrackLog_(int num)
1143 {
1144  static bool first = true;
1145  static int img_num = 0;
1146  const int min_err_code = -7;
1147  static ofstream os("tracker.log");
1148  if (first){
1149  // open the file
1150  if (!os.good()){
1151  BIASERR("error opening \"tracker.log\"");
1152  BIASABORT;
1153  }
1154  // write header
1155  os << "# reasons for lost tracks\n"
1156  << "#" << setw(4)<<"no"
1157  << setw(7) << "num"
1158  << setw(7) << "lost"
1159  << setw(7) << "ignrd";
1160  for (int r = 0; r >= min_err_code; r--){
1161  os << setw(7) << r;
1162  }
1163  os << endl;
1164  first = false;
1165  }
1166 
1167  const int pyindex = PyIndex_;
1168  if (!Points_.empty()){
1169  const int nump = Points_[pyindex].size();
1170  // count the different failure reasons
1171  // and compute mean and variance of covariances
1172  vector<int> reason(-min_err_code + 1, 0);
1173  int reas;
1174  for (int i=0; i<nump; i++){
1175  reas = res_[pyindex][i];
1176  if (reas < 0){
1177  if (reas<min_err_code) {
1178  BIASABORT;
1179  }
1180  reason[-reas]++;
1181  } else {
1182  reason[0]++;
1183  }
1184  }
1185 
1186 
1187  os << setw(5) << img_num
1188  << setw(7) << nump
1189  << setw(7) << nump - num - reason[6]
1190  << setw(7) << reason[6];
1191  for (int r = 0; r <= -min_err_code; r++){
1192  os << setw(7) << reason[r];
1193  }
1194  os << endl << flush;
1195 
1196  img_num++;
1197  }
1198 }
1199 
1200 
1201 template <class StorageType, class CalculationType>
1202 int Tracker<StorageType, CalculationType>::
1203 WriteTrackFile(deque<vector<HomgPoint2D> >& p, deque<vector<int> >& results,
1204  const string& name)
1205 {
1206  const int width=20;
1207  int i, k;
1208  ofstream os(name.c_str());
1209  if (!os){
1210  BIASERR("error opening "<<name);
1211  return -1;
1212  }
1213 
1214  os << "Feel free to place comments here.\n\n";
1215 
1216  os << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n";
1217  os << "!!! Warning: This is a KLT data file. Do not modify below this line !!!\n\n";
1218 
1219  os << "------------------------------\n";
1220  os << "KLT Feature Table\n";
1221  os << "------------------------------\n\n";
1222 
1223  os << "nFrames = "<<p.size()<<", nFeatures = "<<p[0].size()<<"\n\n";
1224 
1225  os << "feature | frame\n";
1226  os << " |";
1227  for (i = 0 ; i < (int)p.size() ; i++)
1228  os << setw(width)<<i;
1229  os << "\n";
1230  os << "--------+";
1231  for (i = 0 ; i < (int)p.size() ; i++)
1232  for (k=0; k<width; k++) os << "-";
1233  os << "\n";
1234  if (!os){
1235  BIASERR("error writing "<<name);
1236  return -1;
1237  }
1238  os.close();
1239 
1240 
1241  FILE *fp=fopen(name.c_str(), "ab");
1242  for (k=0; k<(int)p[0].size(); k++){ // k = feature
1243  fprintf(fp, "%7d | ", k);
1244  for (i=0; i<(int)p.size(); i++){ // i = image
1245  // cout << p[i][k] << endl;
1246  //cout << residui[i][k] << endl;
1247  if (!p[i][k].IsAtInfinity()){
1248  fprintf(fp, "(%7.3f,%7.3f)=%5d ", p[i][k][0], p[i][k][1],
1249  results[i][k]);
1250  } else {
1251  fprintf(fp, "(%7.3f,%7.3f)=%5d ", -1.0, -1.0, results[i][k]);
1252  }
1253  }
1254  fprintf(fp, "\n");
1255  }
1256 
1257  fclose(fp);
1258  return 0;
1259 }
1260 
1261 template <class StorageType, class CalculationType>
1262 void Tracker<StorageType, CalculationType>::
1263 PadPoints_(const int start)
1264 {
1265  const int pysize = (int)LastImage_->size();
1266  const int pyindex = PyIndex_;
1267  const int num = static_cast<int>(Points_[pyindex].size());
1268  for (int i=start; i<num; i++){
1269  for (int p=pyindex; p<pysize; p++){
1270  Points_[p][i][2] = 0.0;
1271  }
1272  }
1273 }
1274 
1275 template <class StorageType, class CalculationType>
1276 int Tracker<StorageType, CalculationType>::
1277 GetTrackerType()
1278 {
1279  // check Tracker Type
1280  if (dynamic_cast<TrackerBaseSimple<CalculationType> *>(tb_))
1281  return TT_Simple;
1282  if (dynamic_cast<TrackerBaseHomography<CalculationType> *>(tb_))
1283  return TT_Homography;
1284  if (dynamic_cast<TrackerBaseAffine<CalculationType> *>(tb_))
1285  {
1286  if ((dynamic_cast<TrackerBaseAffine<CalculationType> *>(tb_))->IsSimilarityTransformOnly())
1287  return TT_Similar;
1288  else
1289  return TT_Affine;
1290  }
1291  if (dynamic_cast<TrackerBaseAffine2<CalculationType> *>(tb_))
1292  {
1293  if ((dynamic_cast<TrackerBaseAffine2<CalculationType> *>(tb_))->IsSimilarityTransformOnly())
1294  return TT_Similar;
1295  else
1296  return TT_Affine2;
1297  }
1298  if (dynamic_cast<TrackerBaseWeighted<CalculationType> *>(tb_))
1299  return TT_SimpleWeighted;
1300  if (dynamic_cast<TrackerBaseWeighted1D<CalculationType> *>(tb_))
1301  return TT_Weighted1D;
1302 
1303  return TT_Invalid;
1304 }
1305 
1306 template <class StorageType, class CalculationType>
1307 void Tracker<StorageType, CalculationType>::
1308 SetTrackerType(int tracker_type)
1309 {
1310  if (tb_) {
1311  delete tb_;
1312  tb_ = NULL;
1313  }
1314  switch(tracker_type){
1315  case TT_Simple:
1316  tb_ = new TrackerBaseSimple<CalculationType>();
1317  break;
1318  case TT_Similar:
1319  tb_ = new TrackerBaseAffine<CalculationType>(true);
1320  break;
1321  case TT_Affine:
1322  tb_ = new TrackerBaseAffine<CalculationType>(false);
1323  break;
1324  case TT_Homography:
1325  tb_ = new TrackerBaseHomography<CalculationType>();
1326  break;
1327  case TT_Affine2:
1328  tb_ = new TrackerBaseAffine2<CalculationType>(false);
1329  break;
1330  case TT_SimpleWeighted:
1331  tb_ = new TrackerBaseWeighted<CalculationType>();
1332  break;
1333  case TT_Weighted1D:
1334  tb_ = new TrackerBaseWeighted1D<CalculationType>();
1335  break;
1336  default:
1337  BIASERR("Unknown tracker type");
1338  BIASABORT;
1339  break;
1340  }
1341 }
1342 
1343 template <class StorageType, class CalculationType>
1344 void Tracker<StorageType, CalculationType>::
1345 Vector2FilterMask_(Vector<double>& input, FilterMask& mask, bool separable)
1346 {
1347  if (separable){
1348  Vector<float> hkernel, vkernel;
1349  if (input.Size()<2u){
1350  BIASERR("invalid vector size "<<input.Size());
1351  BIASABORT;
1352  }
1353  const int hsize = (int)rint(input[0]), vsize = (int)rint(input[1]);
1354  if ((int)input.Size() != hsize + vsize + 2){
1355  BIASERR("invalid vector entries");
1356  BIASABORT;
1357  }
1358  hkernel.newsize(hsize);
1359  for (int i=0; i<hsize; i++){
1360  hkernel[i] = (float)(CalculationType)input[i+2];
1361  }
1362  vkernel.newsize(vsize);
1363  for (int i=0; i<vsize; i++){
1364  vkernel[i] = (float)(CalculationType)input[i+2+hsize];
1365  }
1366  mask.Init(hkernel, vkernel);
1367  } else {
1368  Matrix<float> kernel;
1369  if (input.Size()<2u){
1370  BIASERR("invalid vector size "<<input.Size());
1371  BIASABORT;
1372  }
1373  const int rows = (int)rint(input[0]), cols = (int)rint(input[1]);
1374  const int num = cols * rows;
1375  if ((int)input.Size() != num + 2){
1376  BIASERR("invalid vector entries");
1377  BIASABORT;
1378  }
1379  kernel.newsize(rows, cols);
1380  for (int i=0; i<num; i++){
1381  kernel.GetData()[i] = (float)(CalculationType)input[i+2];
1382  }
1383  mask.Init(kernel);
1384  }
1385 }
1386 
1387 //////////////////////////////////////////////////////////////////////////
1388 // instantiation
1389 //////////////////////////////////////////////////////////////////////////
1390 namespace BIAS{
1391 template class Tracker<unsigned char, float>;
1392 template class Tracker<float, float>;
1393 
1394 // fill in instances as required
1395 #ifdef BUILD_IMAGE_INT
1396 template class Tracker<unsigned char, int>;
1397 #endif
1398 #ifdef BUILD_IMAGE_CHAR
1399 //template class Tracker<unsigned char, char>;
1400 #endif
1401 #ifdef BUILD_IMAGE_SHORT
1402 #endif
1403 #ifdef BUILD_IMAGE_USHORT
1404 template class Tracker<unsigned short, float>;
1405 #ifdef BUILD_IMAGE_INT
1406 template class Tracker<unsigned short, int>;
1407 #endif
1408 #endif
1409 #ifdef BUILD_IMAGE_UINT
1410 #endif
1411 #ifdef BUILD_IMAGE_DOUBLE
1412 #endif
1413 }
BIAS::TrackerBaseAffine2
devel version of antialiased affine tracker
Definition: TrackerBaseAffine2.hh:37
BIAS::Tracker::GetTrackerType
int GetTrackerType()
return trackerbase type currently active
Definition: Tracker.cpp:1277
BIAS::PyramidImageInterface::Init
virtual void Init(const Image< StorageType > &image, const unsigned py_size)=0
BIAS::Tracker::ReserveInternals_
void ReserveInternals_(const int pysize, const int pointsize)
Definition: Tracker.cpp:183
BIAS::Tracker::PreparePyramide
int PreparePyramide(const Image< StorageType > &im, PyramidImageInterface< CalculationType > &pim, PyramidImageInterface< CalculationType > &gx, PyramidImageInterface< CalculationType > &gy, FilterNTo2N< CalculationType, CalculationType > &grad, FilterNToN< StorageType, CalculationType > &lowpass)
Prepares the pyramid images: Filters im to pim[0], downsamples pim[0] and calculates the gradients gx...
Definition: Tracker.cpp:441
BIAS::Tracker::FillUpPoints
int FillUpPoints(const std::vector< HomgPoint2D > &points, const std::vector< float > &qual)
Replaces points to which no correspondence could be established in the internal data structures...
Definition: Tracker.cpp:233
BIAS::HomgPoint2D
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
Definition: HomgPoint2D.hh:67
BIAS::FilterNTo2N::Filter
virtual int Filter(const Image< InputStorageType > &src, Image< OutputStorageType > &dst)
dst.GetChannelCount()==2*src.GetCHannelCount()
Definition: FilterNTo2N.cpp:58
BIAS::Tracker::ReplaceAllPoints
int ReplaceAllPoints(const std::vector< HomgPoint2D > &points, const std::vector< float > &qual)
Place every point from points even those at infinity, in the internal data structure Points_...
Definition: Tracker.cpp:383
BIAS::ImageBase::CM_Grey
gray values, 1 channel
Definition: ImageBase.hh:130
BIAS::PyramidImageInterface::size
unsigned size() const
deprecated interface
Definition: PyramidImageInterface.hh:102
BIAS::PyramidImageInterface::GetRescaleFactor
virtual double GetRescaleFactor() const =0
BIAS::Tracker::SetTrackerType
void SetTrackerType(int tracker_type)
Definition: Tracker.cpp:1308
BIAS::Tracker::WriteTrackFile
int WriteTrackFile(std::deque< std::vector< HomgPoint2D > > &p, std::deque< std::vector< int > > &results, const std::string &name)
write a track file in Birchfeld-style.
Definition: Tracker.cpp:1203
BIAS::PyramidImageInterface::IsEmpty
virtual bool IsEmpty() const =0
BIAS::Tracker::ReplacePoints
int ReplacePoints(const std::vector< HomgPoint2D > &points, const std::vector< float > &qual)
Place every point from points which is not at infinity, in the internal data structure Points_...
Definition: Tracker.cpp:330
BIAS::HomgPoint2D::Homogenize
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
Definition: HomgPoint2D.hh:215
BIAS::Tracker::PadPoints_
void PadPoints_(const int start)
sets all points with index&gt;=start in the internal data structures to infinity
Definition: Tracker.cpp:1263
TNT::Matrix::newsize
Matrix< T > & newsize(Subscript M, Subscript N)
Definition: cmat.h:269
BIAS::Tracker::ReplaceLastImages
void ReplaceLastImages(PyramidImageInterface< CalculationType > *pim, PyramidImageInterface< CalculationType > *gradx, PyramidImageInterface< CalculationType > *grady)
This function allows the replacement of the last images.
Definition: Tracker.cpp:104
BIAS::Tracker::ResizeInternals_
void ResizeInternals_(const int pysize)
Definition: Tracker.cpp:141
BIAS::GradientSobel3x3
gradient calculation with sobel 3 by 3 masks
Definition: GradientSobel3x3.hh:54
BIAS::Vector::Size
unsigned int Size() const
length of the vector
Definition: Vector.hh:143
BIAS::ImageBase::GetWidth
unsigned int GetWidth() const
Definition: ImageBase.hh:312
BIAS::Tracker::Tracker
Tracker(int tracker_type=TT_Simple)
Default constructor without param object.
Definition: Tracker.cpp:60
BIAS::Tracker::Reset
void Reset()
Calls ClearPoints and sets pointer to last image and gradients to zero.
Definition: Tracker.cpp:202
BIAS::PyramidImageInterface::SetZero
virtual void SetZero()=0
BIAS::Tracker::ClearPoints
void ClearPoints()
Clears the internal data structures, i.e.
Definition: Tracker.cpp:127
BIAS::PyramidImageInterface::ShallowClone
virtual PyramidImageInterface< StorageType > * ShallowClone() const =0
TNT::Vector::newsize
Vector< T > & newsize(Subscript N)
Definition: vec.h:220
BIAS::FilterNToN::Filter
virtual int Filter(const Image< InputStorageType > &src, Image< OutputStorageType > &dst)=0
virtual function for interface definition
BIAS::Tracker::~Tracker
virtual ~Tracker()
Definition: Tracker.cpp:96
BIAS::Tracker::DetermineROI_
void DetermineROI_(int &tlx, int &tly, int &brx, int &bry) const
Definition: Tracker.cpp:210
BIAS::HomgPoint2D::IsAtInfinity
bool IsAtInfinity() const
Definition: HomgPoint2D.hh:165
BIAS::PyramidImageInterface::GetSingleImage
virtual void GetSingleImage(Image< StorageType > &im) const =0
BIAS::ImageConvert::ConvertST
static int ConvertST(const BIAS::ImageBase &source, BIAS::ImageBase &dest, ImageBase::EStorageType targetST)
Function to convert the storage type of images e.g.
Definition: ImageConvert.cpp:2841
BIAS::ImageBase::GetChannelCount
unsigned int GetChannelCount() const
returns the number of Color channels, e.g.
Definition: ImageBase.hh:382
BIAS::ImageBase::GetHeight
unsigned int GetHeight() const
Definition: ImageBase.hh:319
BIAS::Image
The image template class for specific storage types.
Definition: Image.hh:78
BIAS::Matrix::GetData
T * GetData()
get the pointer to the data array of the matrix (for faster direct memeory access) ...
Definition: Matrix.hh:185
BIAS::ImageBase::SamePixelAndChannelCount
bool SamePixelAndChannelCount(const ImageBase &Image) const
checks if data area has same &quot;size&quot; as Image of other type
Definition: ImageBase.hh:73
BIAS::ImageIO::Save
static int Save(const std::string &filename, const ImageBase &img, const enum TFileFormat FileFormat=FF_auto, const bool sync=BIAS_DEFAULT_SYNC, const int c_jpeg_quality=BIAS_DEFAULT_IMAGE_QUALITY, const bool forceNewID=BIAS_DEFAULT_FORCENEWID, const bool &writeMetaData=true)
Export image as file using extrnal libs.
Definition: ImageIO.cpp:725
BIAS::Tracker::Vector2FilterMask_
void Vector2FilterMask_(Vector< double > &input, FilterMask &mask, bool separable)
Definition: Tracker.cpp:1345
BIAS::Vector3
class Vector3 contains a Vector of fixed dim.
Definition: Matrix.hh:53
BIAS::TimeMeasure::GetRealTime
double GetRealTime() const
return real time (=wall time clock) in usec JW For Win32: real-time is measured differently from user...
Definition: TimeMeasure.cpp:370
BIAS::ImageBase::GetColorModel
enum EColorModel GetColorModel() const
Definition: ImageBase.hh:407
BIAS::Matrix
matrix class with arbitrary size, indexing is row major.
Definition: GenerateGauss.hh:31
BIAS::PyramidImageInterface::WriteImages
virtual int WriteImages(const std::string &prefix) const =0
BIAS::Tracker::WriteTrackLog_
void WriteTrackLog_(int num)
logs reasons for lost tracks into tracker.log
Definition: Tracker.cpp:1142
BIAS::Tracker::Track
virtual int Track(PyramidImageInterface< CalculationType > &pim, PyramidImageInterface< CalculationType > &gradx, PyramidImageInterface< CalculationType > &grady)
Tracks all points from the internal data structure Points_.
Definition: Tracker.cpp:874
BIAS::ImageBase::GetStorageType
enum EStorageType GetStorageType() const
Definition: ImageBase.hh:414
BIAS::PyramidImageInterface::Downsample
virtual int Downsample()=0
BIAS::PyramidImageInterface::GetPositionOffset
virtual double GetPositionOffset() const =0
BIAS::FilterMask::Init
void Init(const Matrix< FM_INT > &kernel, int rs, bool ResetOtherType=FM_RESET_OTHER)
sets _sKernel and also _fKernel.
Definition: FilterMask.cpp:37
BIAS::FilterMask
A filter mask (or a kernel) used for convolution.
Definition: FilterMask.hh:61
BIAS::Matrix2x2::SetZero
void SetZero()
set the elements of this matrix to zero
Definition: Matrix2x2.hh:258
BIAS::Binomial
binomial low pass filter class
Definition: Binomial.hh:42
BIAS::TimeMeasure
class TimeMeasure contains functions for timing real time and cpu time.
Definition: TimeMeasure.hh:111
BIAS::Tracker
High level tracking class.
Definition: Tracker.hh:72
Generated on Tue Feb 6 2018 02:01:41 for Basic Image AlgorithmS Library by   doxygen 1.8.5