TrackerBaseWeighted.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 "TrackerBaseWeighted.hh"

using namespace BIAS;
using namespace std;

#define SMALL_DET 1e-8

template <class StorageType>
void TrackerBaseWeighted<StorageType>::
SetWeightMatrix(const Matrix<KLT_TYPE>& weight)
{
  // set new matrix
  _weight = weight;
  _useGaussianWeightMatrix = false;
}

template <class StorageType>
int TrackerBaseWeighted<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)
{
  // check if a weight matrix was set before
  if (_useGaussianWeightMatrix)    
    CalcGaussianWeightMatrix_();
  else
  {
    // test if setted weight matrix has correct number of rows and cols
    if ((int)_weight.GetRows() != _WinSize ||
        (int)_weight.GetCols() != _WinSize)
    {
      BIASERR("Matrix height and width does not match window size!");
      BIASABORT;
    }    
  }
  
  BIASCDOUT(D_TRACKERB_KLT,"p1: " << p1[0] << ", " << p1[1] 
            << "\tp2: " <<p2[0] << ", " << p2[1]<<endl);

  int res=TRACKER_SUCCESS;
  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;

  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];
  //  MyBilinearRegion(_ida1,_gradx1,_grady1,p1[0],p1[1],hws,_bl1,_gx1,_gy1);
  BilinearRegion1_(p1[0],p1[1],hws);
  do {
    _gxx=_gxy=_gyy=ex=ey=0.0;
    //     MyBilinearRegion(_ida2,_gradx2,_grady2,result[0],result[1],hws,
    //                      _bl2,_gx2,_gy2);
    BilinearRegion2_(result[0],result[1],hws);
    // initialize G and e
    for (rh=0; rh<winsize; rh++){
      for (ch=0; ch<winsize; ch++){
        _gsx[rh][ch] = 
          _weight[rh][ch] * (_gx1[rh][ch] + _gx2[rh][ch]);
        _gsy[rh][ch] = 
          _weight[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 = _weight[rh][ch] * (_bl1[rh][ch] - _bl2[rh][ch]);
        ex += _gsx[rh][ch]*dI;
        ey += _gsy[rh][ch]*dI;
      } 
    }
    BIASCDOUT(D_TRACKERB_KLT, "_gxx: "<<_gxx<<" _gxy: "<<_gxy
              <<" _gyy: "<<_gyy<<" ex: "<<ex<<" ey: "<<ey<<endl);
    det = _gxx*_gyy-_gxy*_gxy;
    if (fabs(det)<SMALL_DET) {
      BIASCDOUT(D_TRACKERB_KLT, 
                "unable to invert G, det: "<<det<<endl);

      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");

    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\n");
      res = TRACKER_OUTOFIMAGE;
      break;
    }
  } while (error>maxerr && iter<maxiter);
    
  if (res==0 && error>maxerr) {
    res = TRACKER_TOOMANYITER;
    BIASCDOUT(D_TRACKERB_KLT,"error > maxerror : returning -3\n");
  } else if (res==0) {
    BilinearRegion2_(result[0],result[1],hws);
  }

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

  return res;
}

template <class StorageType>
void TrackerBaseWeighted<StorageType>::
CalcGaussianWeightMatrix_()
{  
  _weight.newsize(_WinSize, _WinSize);
  // now calculate the gaussian weight matrix
  double sigma = ((double)_WinSize)/6.0;
  double fac = 1.0 / (2.0 * sigma * sigma);
  double sum=0.0, tmp;
  register int i, j, x, y, xp, xm, yp, ym;
  const int hws=_HalfWinSize;
  for (i= hws; i >= 0; i--){
    for (j= hws; j >= 0; j--){
      tmp=exp(-(double)(i*i+j*j)*fac);
      yp=hws+i;
      ym=hws-i;
      xp=hws+j;
      xm=hws-j;
      _weight[ym][xm] =_weight[yp][xm] = _weight[ym][xp] = _weight[yp][xp] = 
        (KLT_TYPE)(tmp);
    }
  }
    
  for (y=0; y<_WinSize; y++){
    for (x=0; x<_WinSize; x++){
      sum+=_weight[y][x];
    } 
  }
  BIASCDOUT(D_TRACKERB_INIT, "_weight: "<<_weight<<endl);
  BIASCDOUT(D_TRACKERB_INIT, "_weight sum before normalization: "<<sum<<endl);

  //normalization
  sum=1.0/sum; // division is slow
  for (i= hws; i >= 0; i--){
    for (j= hws; j >= 0; j--){
      yp=hws+i;
      ym=hws-i;
      xp=hws+j;
      xm=hws-j;
      tmp=_weight[ym][xm];
      _weight[ym][xm] = _weight[yp][xm] = _weight[ym][xp] =
        _weight[yp][xp] = tmp * sum;
    }
  }
  BIASCDOUT(D_TRACKERB_INIT, "_weight: "<<_weight<<endl);
#ifdef BIAS_DEBUG
  sum=0.0;
  for (y=0; y<_WinSize; y++){
    for (x=0; x<_WinSize; x++){
      sum+=_weight[y][x];
    } 
  }
  BIASCDOUT(D_TRACKERB_INIT,"_weight sum after normalization: "<<sum<<endl);
#endif  
}


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

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