TrackerBaseSimple.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS 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.

BIAS 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 BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "TrackerBaseSimple.hh"

using namespace BIAS;
using namespace std;

#define SMALL_DET 1e-8

template <class StorageType>
int TrackerBaseSimple<StorageType>::
Track_(Vector2<KLT_TYPE>& p1, Vector2<KLT_TYPE>& p2,
       Vector2<KLT_TYPE>& result, KLT_TYPE& error, 
       int &iter,
       const Matrix2x2<KLT_TYPE>& AffinePred,
       Matrix2x2<KLT_TYPE>& AffineResult)
{
  
#ifdef TIMING
  TimeMeasure timer;
  timer.Start();
#endif
  
  if (_HalfWinSize != _LastHalfWinSize){
    this->Resize_(_HalfWinSize);
  }
  BIASCDOUT(D_TRACKERB_KLT,"trying to track p1: (" << p1[0] << ", " << p1[1] 
            << ") <--> p2: (" <<p2[0] << ", " << p2[1]<<")\n");

  int res=0;
  int rh, ch; 
  register KLT_TYPE dI, ex, ey, dx, dy, det;
  //KLT_TYPE lsterror=FLT_MAX;
  const int hws=_HalfWinSize;
  const int winsize=_WinSize;
  const int maxiter=_MaxIterations;
  const KLT_TYPE maxerr=_MaxError;

  BIASCDOUT(D_TRACKERB_INIT, "using hws: "<<hws<<"\tmaxiter: "<<maxiter
            <<"\tmaxerr: "<<maxerr<<endl);

  if (p1[0]<_minx1 || p1[0]>=_maxx1 || 
      p1[1]<_miny1 || p1[1]>=_maxy1 ||
      p2[0]<_minx2 || p2[0]>=_maxx2 || 
      p2[1]<_miny2 || p2[1]>=_maxy2 ){
    BIASCDOUT(D_TRACKERB_KLT, "p1 ("<<p1[0]<<", "<<p1[1]<<")   p2 ("<<p2[0]
              <<", "<<p2[1]<<")\n");
    BIASCDOUT(D_TRACKERB_KLT, "tracking roi im1 : ( "<<_minx1<<", "<<_miny1
              <<")  <-->  ( "<<_maxx1<<", "<<_maxy1<<")\n");
    BIASCDOUT(D_TRACKERB_KLT, "tracking roi im2 : ( "<<_minx2<<", "<<_miny2
              <<")  <-->  ( "<<_maxx2<<", "<<_maxy2<<")\n");
    BIASCDOUT(D_TRACKERB_KLT, "points do not lie in ROI, returning -4\n");
    return TRACKER_OUTOFROI;
  } else {
    BIASCDOUT(D_TRACKERB_KLT, "tracking roi im1 : ( "<<_minx1<<", "<<_miny1
              <<")  <-->  ( "<<_maxx1<<", "<<_maxy1<<")\n");
    BIASCDOUT(D_TRACKERB_KLT, "tracking roi im2 : ( "<<_minx2<<", "<<_miny2
              <<")  <-->  ( "<<_maxx2<<", "<<_maxy2<<")\n");
  }

  iter=0;

  result[0]=p2[0];
  result[1]=p2[1];
  BilinearRegion1_(p1[0],p1[1],hws);
  BIASCDOUT(D_TRACKERB_BILINEAR, "_bl1: "<<_bl1<<"\n_gx1: "<<_gx1
            <<"\n_gy1: "<<_gy1<<endl);
#ifdef TIMING
  timer.Stop();
  cerr << "TrackerBase MyBilinearRegion1_ took "<<timer.GetRealTime()
       <<" us = "<<timer.GetCycleCount()<<" cycles \n";
  timer.Reset();
  timer.Start();
#endif

  do {
    _gxx=_gxy=_gyy=ex=ey=0.0;
    BilinearRegion2_(result[0],result[1],hws);
    BIASCDOUT(D_TRACKERB_BILINEAR, "_bl2: "<<_bl2<<"\n_gx2: "<<_gx2
              <<"\n_gy2: "<<_gy2<<endl);
   
    // initialize G and e
    for (rh=0; rh<winsize; rh++){
      for (ch=0; ch<winsize; ch++){
        _gsx[rh][ch] = _gx1[rh][ch] + _gx2[rh][ch];
        _gsy[rh][ch] = _gy1[rh][ch] + _gy2[rh][ch];
        _gxx += _gsx[rh][ch]*_gsx[rh][ch];
        _gyy += _gsy[rh][ch]*_gsy[rh][ch];
        _gxy += _gsx[rh][ch]*_gsy[rh][ch];
        dI = _bl1[rh][ch] - _bl2[rh][ch];
        ex += _gsx[rh][ch]*dI;
        ey += _gsy[rh][ch]*dI;
#ifdef BIAS_DEBUG
        if (BIAS_ISNAN(_gxx) || BIAS_ISINF(_gxx) ||
            BIAS_ISNAN(_gxy) || BIAS_ISINF(_gxy) ||
            BIAS_ISNAN(_gyy) || BIAS_ISINF(_gyy) ) {
          cout << "_gx1: "<<_gx1<<"\n_gx2: "<<_gx2
               << "\n_gy1: "<<_gy1<<"\n_gy2: "<<_gy2
               << "\n_bl1: "<<_bl1<<"\n_bl2: "<<_bl2
               << "\np1: "<<p1[0]<<" "<<p1[1]<<endl
               << "\np2: "<<p2[0]<<" "<<p2[1]<<endl
               << "\nroi1 ("<<_minx1<<", "<<_miny1<<") <--> ("
               <<_maxx1<<", "<<_maxy1<<")"
               << "\nroi2 ("<<_minx2<<", "<<_miny2<<") <--> ("
               <<_maxx2<<", "<<_maxy2<<")\n";
        }
        BIASASSERT(!BIAS_ISNAN(_gxx) && !BIAS_ISINF(_gxx));
        BIASASSERT(!BIAS_ISNAN(_gxy) && !BIAS_ISINF(_gxy));
        BIASASSERT(!BIAS_ISNAN(_gyy) && !BIAS_ISINF(_gyy));
#endif
      } 
    }
    BIASCDOUT(D_TRACKERB_KLT, "iter: "<<iter<<"  _gxx: "<<_gxx<<" _gxy: "<<_gxy
              <<" _gyy: "<<_gyy<<" ex: "<<ex<<" ey: "<<ey<<endl);
    det = _gxx*_gyy-_gxy*_gxy;
    if (fabs(det)<SMALL_DET) {
      BIASDOUT(D_TRACKERB_KLT, 
               "unable to invert G, det: "<<det);
      res = TRACKER_NOSTRUCTURE;
      break;
    }
    _idet=1.0/det;
    dx = (_gyy*ex - _gxy*ey)*_idet; 
    dy = (_gxx*ey - _gxy*ex)*_idet; 

    BIASCDOUT(D_TRACKERB_KLT, "     det: "<<det<<" _idet: "<<_idet
              <<" dx: "<<dx<<" dy: "<<dy<<endl);
    
    //error = sqrt(dx*dx + dy*dy);
    error = (fabs(dx)>fabs(dy))?fabs(dx):fabs(dy);

    /*
    if (error>lsterror){
      BIASCDOUT(D_TRACKERB_KLT, 
                "error increased ("
                <<lsterror<<" -> "<<error
                <<"), stopping iteration and returning 1\n");
      res = TRACKER_ERRORINCREASED;
      error=lsterror;
      break;
      } */
    //lsterror=error;

    result[0] += dx;
    result[1] += dy;

    BIASCDOUT(D_TRACKERB_KLT,"     disp after "<<iter<<" iteration: "
              <<result[0]-p2[0]<<", "<<result[1]-p2[1]<<" error: "<< error
              << "  ("<<result[0]<<", "<<result[1]<<")\n");

    //#define D_TRACK
#ifdef D_TRACK
    fprintf(stdout, "iter: %i\t_gxx: %f\t_gxy: %f\t_gyy: %f\tex: %f\tey: %f\n",
            iter, _gxx, _gxy, _gyy, ex, ey);
    fprintf(stdout, "      \tdx: %f\tdy: %f\tcoo: (%f, %f)\n", 
            dx, dy, result[0], result[1]);
#endif

    iter++;
    
    if (result[0]<_minx2 || result[0]>=_maxx2 ||
        result[1]<_miny2 || result[1]>=_maxy2){
      BIASCDOUT(D_TRACKERB_KLT, 
                "feature slid out of image, returning -2");
      res = TRACKER_OUTOFIMAGE;
      break;
    }
  } while (error>maxerr && iter<maxiter);
    
  if (res==0 && error>maxerr && iter==maxiter) {
    res = TRACKER_TOOMANYITER;
    BIASCDOUT(D_TRACKERB_KLT,"error > "<<maxerr
              <<" and max iteration count reached: "<<iter<<" returning -3");
  } else if (res==0) {
    BilinearRegion2_(result[0],result[1],hws);
  }

#ifdef TIMING
  timer.Stop();
  cerr << "i "<<iter<<" res "<<res<<" ";
  cerr << "TrackSimple_ took "<<timer.GetRealTime()<<" us = "
       <<timer.GetCycleCount()<<" cycles\n";
#endif
  return res;
}

//////////////////////////////////////////////////////////////////////////
//                 instantiation
//////////////////////////////////////////////////////////////////////////
namespace BIAS{
template class TrackerBaseSimple<unsigned char>; 
template class TrackerBaseSimple<float>; 

// fill in instances as required
#ifdef BUILD_IMAGE_INT
template class TrackerBaseSimple<int>; 
#endif
#ifdef BUILD_IMAGE_CHAR
template class TrackerBaseSimple<char>; 
#endif
#ifdef BUILD_IMAGE_SHORT
template class TrackerBaseSimple<short>; 
#endif
#ifdef BUILD_IMAGE_USHORT
template class TrackerBaseSimple<unsigned short>; 
#endif
#ifdef BUILD_IMAGE_UINT
template class TrackerBaseSimple<unsigned int>; 
#endif
#ifdef BUILD_IMAGE_DOUBLE
#endif
}