TrackerBaseInterface.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS),
however, it contains other contributions.
For these marked parts a different license may apply, please read
the documentation of the marked parts for further details. 
For the rest of this file read the following:

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 "TrackerBaseInterface.hh"
#include <algorithm>

#include <Base/Common/BIASpragma.hh>

using namespace BIAS;
using namespace std;

//////////////////////////////////////////////////////////////////////////
//                 operators
//////////////////////////////////////////////////////////////////////////

ostream& BIAS::operator<<(ostream& os, const enum ERejectionType& rt)
{
  switch (rt){
  case RT_Invalid:
    os << "invalid";
    break;
  case RT_MAD:
    os << "RT_MAD";
    break; 
  case RT_X84:
    os << "RT_X84";
    break; 
  case RT_X84M:
    os << "RT_X84M";
    break; 
  default:
    os << "unkown rejection type "<<(int)rt;
    break;
  }
  return os;
}


ostream& BIAS::operator<<(std::ostream& os, const enum ETrackerResult& res)
{
  switch (res){
  case TRACKER_SUCCESS:
    os << "TRACKER_SUCCESS";
    break;
  case TRACKER_NOSTRUCTURE:
    os << "TRACKER_NOSTRUCTURE";
    break;
  case TRACKER_OUTOFIMAGE:
    os << "TRACKER_OUTOFIMAGE";
    break;
  case TRACKER_TOOMANYITER:
    os << "TRACKER_TOOMANYITER";
    break;
  case TRACKER_OUTOFROI:
    os << "TRACKER_OUTOFROI";
    break;
  case TRACKER_MEANABSDIF:
    os << "TRACKER_MEANABSDIF";
    break;
  case TRACKER_IGNOREPOINT:
    os << "TRACKER_IGNOREPOINT";
    break;
  case TRACKER_X84:
    os << "TRACKER_X84";
    break;
  case TRACKER_ODDAFFINEWARP:
    os << "TRACKER_ODDAFFINEWARP";
    break;
  case TRACKER_ERRORINCREASED:
    os << "TRACKER_ERRORINCREASED";
    break;
  default:
     os << "unkown tracker result code "<<(int)res;
    break;
  }
  return os;
}


//////////////////////////////////////////////////////////////////////////
//                 implementation
//////////////////////////////////////////////////////////////////////////

template <class StorageType>
TrackerBaseInterface<StorageType>::
TrackerBaseInterface()
  : Debug()
{
  _HalfWinSize     = 3;
  _WinSize = 2 * _HalfWinSize + 1;
  _MaxIterations   = 10;
  _MaxError        = 0.01;
  _MaxResiduumMAD  = 10.0;
  _RejectionType   = RT_MAD;
  
  _im1         = NULL;
  _gradim1x    = NULL;
  _gradim1y    = NULL;

  _im2         = NULL;
  _gradim2x    = NULL;
  _gradim2y    = NULL;

  _ComputeFilteredImages = false;

  _maxx1 = _maxy1 = _minx1 = _miny1 = 0;
  _maxx2 = _maxy2 = _minx2 = _miny2 = 0;
  _LastHalfWinSize = 0;
  _AffineBrightnessInvariance = false;
}

template <class StorageType>
TrackerBaseInterface<StorageType>::
~TrackerBaseInterface()
{
}

template <class StorageType>
void TrackerBaseInterface<StorageType>::
Init(Image<StorageType>& im1, Image<StorageType>& im2, 
     Image<StorageType>& gradx1, Image<StorageType>& grady1, 
     Image<StorageType>& gradx2, Image<StorageType>& grady2)
{
  BIASASSERT(im2.SamePixelAndChannelCount(im1));
  BIASASSERT(gradx1.SamePixelAndChannelCount(im1));
  BIASASSERT(grady1.SamePixelAndChannelCount(im1));
  BIASASSERT(gradx2.SamePixelAndChannelCount(im1));
  BIASASSERT(grady2.SamePixelAndChannelCount(im1));

  unsigned ytlx, ytly, ybrx, ybry;
  grady1.GetROI()->GetCorners(ytlx, ytly, ybrx, ybry);
  unsigned xtlx, xtly, xbrx, xbry;
  gradx1.GetROI()->GetCorners(xtlx, xtly, xbrx, xbry);
  unsigned minx1, miny1, maxx1, maxy1;
  minx1=(ytlx>xtlx)?(ytlx):(xtlx);
  miny1=(ytly>xtly)?(ytly):(xtly);
  maxx1=(ybrx<xbrx)?(ybrx):(xbrx); 
  maxy1=(ybry<xbry)?(ybry):(xbry);

  grady2.GetROI()->GetCorners(ytlx, ytly, ybrx, ybry);
  gradx2.GetROI()->GetCorners(xtlx, xtly, xbrx, xbry);
  unsigned minx2, miny2, maxx2, maxy2;
  minx2=(ytlx>xtlx)?(ytlx):(xtlx);
  miny2=(ytly>xtly)?(ytly):(xtly);
  maxx2=(ybrx<xbrx)?(ybrx):(xbrx); 
  maxy2=(ybry<xbry)?(ybry):(xbry);

  _im1 = &im1;
  _im2 = &im2;

  _gradim1x = &gradx1;
  _gradim2x = &gradx2; 
  _gradim1y = &grady1;
  _gradim2y = &grady2;
  
  bool resize = (_HalfWinSize != _LastHalfWinSize);
  if (resize) {
    Resize_(_HalfWinSize);
  }

  _minx1=minx1+_HalfWinSize+1;
  _miny1=miny1+_HalfWinSize+1;
  _maxx1=maxx1-_HalfWinSize-1;
  _maxy1=maxy1-_HalfWinSize-1;
  _minx2=minx2+_HalfWinSize+1;
  _miny2=miny2+_HalfWinSize+1;
  _maxx2=maxx2-_HalfWinSize-1;
  _maxy2=maxy2-_HalfWinSize-1;

  BIASCDOUT(D_TRACKERB_INIT, "tracking roi im1 : ( "<<_minx1<<", "<<_miny1
            <<")  <-->  ( "<<_maxx1<<", "<<_maxy1<<")\n");
  BIASCDOUT(D_TRACKERB_INIT, "tracking roi im2 : ( "<<_minx2<<", "<<_miny2
            <<")  <-->  ( "<<_maxx2<<", "<<_maxy2<<")\n");

  BIASASSERT(_HalfWinSize>0);
  BIASASSERT(_maxy1>_miny1 && _maxx1>_minx1);
  BIASASSERT(_maxy2>_miny2 && _maxx2>_minx2);

  _ComputeFilteredImages = false;
}

template <class StorageType>
void TrackerBaseInterface<StorageType>::
Init(Image<StorageType>& im1, Image<StorageType>& im2,
     FilterMask& lowpass_mask, FilterMask& gradx_mask, 
     FilterMask& grady_mask)
{
  BIASASSERT(im2.SamePixelAndChannelCount(im1));
  
  _im1 = &im1;
  _im2 = &im2;

  // initialize filter configuration
  _ComputeFilteredImages = true;
  _LowpassFilter        = lowpass_mask.IsSeparable() ?
                          LOWPASS_FILTERMASK_SEPARABLE : LOWPASS_FILTERMASK;
  _GradXFilter          = gradx_mask.IsSeparable() ?
                          GRAD_FILTERMASK_SEPARABLE : GRAD_FILTERMASK;
  _GradYFilter          = grady_mask.IsSeparable() ?
                          GRAD_FILTERMASK_SEPARABLE : GRAD_FILTERMASK;
  _LowpassMask          = lowpass_mask;
  _GradXMask            = gradx_mask;
  _GradYMask            = grady_mask;
  _LowpassMaskSum       = ComputeMaskSum(lowpass_mask);
  _GradXMaskSum         = ComputeMaskSum(gradx_mask);
  _GradYMaskSum         = ComputeMaskSum(grady_mask);

  // compute required additional size for lowpass filtered window.
  unsigned int m = 0;
  if (gradx_mask.IsSeparable())
  {
    m = max(m, gradx_mask.GetSepfh()->Size());
    m = max(m, gradx_mask.GetSepfv()->Size());
  }
  else
  {
    m = max(m, gradx_mask.GetKernelf()->GetRows());
    m = max(m, gradx_mask.GetKernelf()->GetCols());
  }
  if (grady_mask.IsSeparable())
  {
    m = max(m, grady_mask.GetSepfh()->Size());
    m = max(m, grady_mask.GetSepfv()->Size());
  }
  else
  {
    m = max(m, grady_mask.GetKernelf()->GetRows());
    m = max(m, grady_mask.GetKernelf()->GetCols());
  }
  _LowpassWindowExtraSize = (m-1)/2;

  _minx1 = _HalfWinSize + _LowpassWindowExtraSize + 2;
  _maxx1 = im1.GetWidth() - _HalfWinSize - _LowpassWindowExtraSize - 2;
  _miny1 = _HalfWinSize + _LowpassWindowExtraSize + 2;
  _maxy1 = im1.GetHeight() - _HalfWinSize - _LowpassWindowExtraSize - 2;

  _minx2 = _HalfWinSize + _LowpassWindowExtraSize + 2;
  _maxx2 = im2.GetWidth() - _HalfWinSize - _LowpassWindowExtraSize - 2;
  _miny2 = _HalfWinSize + _LowpassWindowExtraSize + 2;
  _maxy2 = im2.GetHeight() - _HalfWinSize - _LowpassWindowExtraSize - 2;


  // check if optimized lowpass filter can be used.
  if (lowpass_mask.IsSeparable() &&
      *lowpass_mask.GetSepfh() == Vector3<FM_FLOAT>(1, 2, 1) &&
      *lowpass_mask.GetSepfv() == Vector3<FM_FLOAT>(1, 2, 1))
  {
    _LowpassFilter = LOWPASS_BINOMIAL3X3;
  }
  if (!lowpass_mask.IsSeparable() &&
      lowpass_mask.GetKernelf()->GetRows() == 3 &&
      lowpass_mask.GetKernelf()->GetCols() == 3 &&
      *lowpass_mask.GetKernelf() == Matrix<float>(Matrix3x3<FM_FLOAT>(1,2,1,2,4,2,1,2,1)))
  {
    _LowpassFilter = LOWPASS_BINOMIAL3X3;
  }

  // check if optimized gradient x filter can be used.
  if (gradx_mask.IsSeparable() &&
      *gradx_mask.GetSepfh() == Vector3<FM_FLOAT>(-1,0,1) &&
      *gradx_mask.GetSepfv() == Vector3<FM_FLOAT>(1,2,1))
  {
    _GradXFilter = GRAD_SOBEL3X3;
  }

  if (!gradx_mask.IsSeparable() &&
      gradx_mask.GetKernelf()->GetRows() == 3 &&
      gradx_mask.GetKernelf()->GetCols() == 3 &&
      *gradx_mask.GetKernelf() == Matrix<float>(Matrix3x3<FM_FLOAT>(-1,0,1,-2,0,2,-1,0,1)))
  {
    _GradXFilter = GRAD_SOBEL3X3;
  }

  // check if optimized gradient y filter can be used.
  if (grady_mask.IsSeparable() &&
      *grady_mask.GetSepfh() == Vector3<FM_FLOAT>(1,2,1) &&
      *grady_mask.GetSepfv() == Vector3<FM_FLOAT>(-1,0,1))
  {
    _GradYFilter = GRAD_SOBEL3X3;
  }
  if (!grady_mask.IsSeparable() &&
      grady_mask.GetKernelf()->GetRows() == 3 &&
      grady_mask.GetKernelf()->GetCols() == 3 &&
      *grady_mask.GetKernelf() == Matrix<float>(Matrix3x3<FM_FLOAT>(-1,-2,-1,0,0,0,1,2,1)))
  {
    _GradYFilter = GRAD_SOBEL3X3;
  }

  bool resize = (_HalfWinSize != _LastHalfWinSize);
  if (resize) {
    Resize_(_HalfWinSize);
  }
}

template <class StorageType>
int TrackerBaseInterface<StorageType>::
Track(HomgPoint2D& p1, HomgPoint2D& p2, HomgPoint2D& p2tracked, 
      KLT_TYPE& error, int &iter,
      KLT_TYPE& residuumMAD, KLT_TYPE& residuumMSD,
      Matrix<KLT_TYPE>& cov,
      const Matrix2x2<KLT_TYPE>& AffinePred,
      Matrix2x2<KLT_TYPE>* AffineResult)
{  
  // don't track points with w != 1
  if (p1[2] != 1.0)
  {
    return TRACKER_IGNOREPOINT;
  }

  if (_HalfWinSize != _LastHalfWinSize){
    Resize_(_HalfWinSize);
  }

  // run specialized Track_
  Vector2<KLT_TYPE> p1vec(p1[0], p1[1]);
  Vector2<KLT_TYPE> p2vec(p2[0], p2[1]);
  Vector2<KLT_TYPE> tracked;
  Matrix2x2<KLT_TYPE> affineResult(MatrixIdentity);
  Matrix2x2<KLT_TYPE> id(MatrixIdentity);
  int result = Track_(p1vec, p2vec, tracked, error, iter,
                      AffinePred.NormFrobenius()<1e-10 ? id : AffinePred, affineResult);
  if (result != 0)
  {
    return result;
  }

  // fill output parameters
  p2tracked = HomgPoint2D(tracked[0], tracked[1]);
  if (AffineResult != NULL)
  {
    *AffineResult = affineResult;
  }
  
  // evaluate result
  EvaluateResult_(residuumMAD, residuumMSD, cov);

  return 0;
}


template <class StorageType>
void TrackerBaseInterface<StorageType>::
Track(std::vector<HomgPoint2D>& p1, std::vector<HomgPoint2D>& p2, 
      std::vector<HomgPoint2D>& p2tracked, std::vector<KLT_TYPE>& error, 
      std::vector<int>& numiter, std::vector<KLT_TYPE>& residuiMAD, 
      std::vector<KLT_TYPE>& residuiMSD, std::vector<int>& res, 
      std::vector<Matrix<KLT_TYPE> >& cov,
      const std::vector<Matrix2x2<KLT_TYPE> >& AffinePred,
      std::vector<Matrix2x2<KLT_TYPE> >* AffineResult)
{
#ifdef TIMING
  TimeMeasure timer;
  timer.Start();
#endif
  
  const unsigned num=p1.size();
  BIASASSERT(num==p2.size() && num==p2tracked.size() && num==error.size());
  BIASASSERT(num==numiter.size() && num==res.size() && num==residuiMSD.size()&&
             num==residuiMAD.size() && num==cov.size());
  BIASASSERT(num>0);

  // track points
  for (unsigned int i = 0; i < num; i++)
  {
    res[i] = Track(p1[i], p2[i], p2tracked[i], error[i], numiter[i],
           residuiMAD[i], residuiMSD[i], cov[i],
                   AffinePred[i], AffineResult ? &(*AffineResult)[i] : NULL);
  }

  // reject points
  switch(_RejectionType)
  {
  case RT_MAD:
    RejectMAD_(residuiMAD, res);
    break;
  case RT_X84:
    RejectX84_(residuiMSD, residuiMAD, res);
    break;
  case RT_X84M:
    RejectX84_(residuiMSD, residuiMAD, res);
    break;
  case RT_Invalid:break; // rejection disabled ...
  default:
    BIASERR("unknown rejection method " << _RejectionType);
    break;
  }
    
#ifdef TIMING
  timer.Stop();
  cerr << "TrackerBaseInterface tracking took "<<timer.GetRealTime()<<" us\n";
#endif
}


///////////////////////////////////////////////////////////////////
//  tracking
///////////////////////////////////////////////////////////////////

template <class StorageType>
void TrackerBaseInterface<StorageType>::
EvaluateResult_(KLT_TYPE& mad, KLT_TYPE& msd, Matrix<KLT_TYPE>& CovMatrix)
{
#ifdef TIMING
  TimeMeasure timer;
  timer.Start();
#endif
  CovMatrix.newsize(2, 2);
  KLT_TYPE* cov = CovMatrix.GetData();
  
  KLT_TYPE *f1=_bl1.GetData(), *f2=_bl2.GetData(), *end = f1+_WinSize*_WinSize;
  mad = 0.0;
  msd = 0.0;
  KLT_TYPE tmp_res=0.0;
  int validpixels = 0;
  while (f1<end){
    if ((*f1 == TRACKERBASE_INVALID_PIXEL) || 
        (*f2 == TRACKERBASE_INVALID_PIXEL)) {
      f1++;
      f2++;
      continue;
    }
    tmp_res = *f1++ - *f2++;
    msd += tmp_res * tmp_res;
    mad += fabs(tmp_res);
    validpixels++;
  }
 
  // now calculate the covariance matrix
  // see Reinhards dissertation page 67, equation 5.9
  // this is the inverse of the matrix 
  // the reference variance is sigma
  double sigmasquared = DBL_MAX;
  if (validpixels>3) {
    sigmasquared = msd / (double)(validpixels - 3);

    // compute mean squared difference
    msd /= KLT_TYPE(validpixels);
    mad /= KLT_TYPE(validpixels);
  } else {
    msd = DBL_MAX;
    mad = DBL_MAX;
  }
  // the inverse of the structure tensor matrix is given by
  // 1/det * | _gyy -_gxy| 
  //         |-_gxy  _gxx|
  cov[0] = _gyy * _idet * sigmasquared;
  cov[1] = cov[2]= -_gxy * _idet * sigmasquared;
  cov[3] = _gxx * _idet * sigmasquared;
// #ifdef BIAS_DEBUG
//     cout << "mad "<<mad<<" sigmasq "<<sigmasquared<<" cov[0][0] "<<cov[0]
//          <<" cov[1][1] "<<cov[3];
//     double meanx=0.0, meany=0.0;
//     for (int x=0; x<7; x++){
//       for (int y=0; y<7; y++){
//         meanx+=fabs(_gx1[y][x]);
//         meany+=fabs(_gy1[y][x]);
//       }
//     }
//     meanx/=49.0;
//     meany/=49.0;
//     cout <<"  gx: "<<meanx <<" gy: "<<meany<<" idet "<<_idet<<" _gxx "
//          <<_gxx<<" _gyy "<<_gyy<<endl;
// #endif

#ifdef TIMING
  timer.Stop();
  cerr << "TrackerBaseInterface::EvaluateResult_ took "<<timer.GetRealTime()<<" us = "
       <<timer.GetCycleCount()<<" cycles\n";
#endif
}

template <class StorageType>
int TrackerBaseInterface<StorageType>::
RejectMAD_(std::vector<KLT_TYPE>& sad, std::vector<int>& res)
{
  KLT_TYPE thresh=_MaxResiduumMAD;
  unsigned num = res.size();

  for (unsigned i=0; i<num; i++){
    if (res[i]>=TRACKER_SUCCESS && sad[i]>=thresh){
      res[i]=TRACKER_MEANABSDIF;
    }
  }
  return 0;
}

template <class StorageType>
int TrackerBaseInterface<StorageType>::
RejectX84_(std::vector<KLT_TYPE>& ssd, std::vector<KLT_TYPE>& sad, 
           std::vector<int>& res)
{
#ifdef TIMING
  TimeMeasure timer;
  timer.Start();
#endif
  int num=ssd.size();
  
  Median1D<KLT_TYPE> _Median; // median calculation class
    
  // sum of sq. grey value diff only for points where tracking succeeded
  vector<KLT_TYPE> _ssd_residui; 
  KLT_TYPE _median_ssd_residui; // median of _ssd_residui
    
  // _diff_from_median = fabs(_ssd_residui - _median_ssd_residui)
  vector<KLT_TYPE> _diff_from_median;
    
  // same as _diff_from_median but also for points where tracking was not
  // sucessfull
  vector<KLT_TYPE> _all_diff_from_median;
  
  _ssd_residui.reserve(num);
  _diff_from_median.reserve(num);
  _all_diff_from_median.resize(num);

  // remove all points where tracking failed
  for (int i=0; i<num; i++){
    if (res[i]>=TRACKER_SUCCESS){
      _ssd_residui.push_back(ssd[i]);
    } else {
   
    }
  }
  unsigned num2=_ssd_residui.size();
  if (num2==0) {
    // no points -> no x84 decision, return
    return 0;
  }
  BIASCDOUT(D_TRACKERB_REJECT, "using "<<num2
            <<" valid residuis for median calculation"<<endl);
#ifdef BIAS_DEBUG
  if (num2==0){
    BIASERR("no valid residui!");
    BIASBREAK;
  }
#endif

  _Median.Compute(_ssd_residui);
  _median_ssd_residui = _Median.GetMedian();




  BIASCDOUT(D_TRACKERB_REJECT,"median residuum is "<<_median_ssd_residui
            <<endl);

  for (unsigned i=0; i<num2; i++){
    _diff_from_median.push_back(fabs(_ssd_residui[i] - _median_ssd_residui));
    //  BIASCDOUT(D_TRACKERB_REJECT, setw(4)<<i<<" residuum "<<setprecision(2)
    //               <<setw(10)<<_ssd_residui[i]<<"\tdiff_from_median "
    //               <<setprecision(2)<<setw(10)<<_diff_from_median[i]<<endl);
  }

  _Median.Compute(_diff_from_median);
  KLT_TYPE threshold = 5.2 * _Median.GetMedian();
  BIASCDOUT(D_TRACKERB_REJECT, "median of diff from median is "
            <<_Median.GetMedian()<<endl);
  BIASCDOUT(D_TRACKERB_REJECT, "x84 threshold is "<<threshold<<endl);

  unsigned rejected=0;
  for (int i=0; i<num; i++){
    // do we want to reject point which are TOO good ?
    //    _all_diff_from_median[i] = fabs(ssd[i] - _median_ssd_residui); 
    // keep good points (diff<0) which are outside the interval median+-sigma
    // because they are very close to zero error.
    _all_diff_from_median[i] = ssd[i] - _median_ssd_residui;

    if (_RejectionType == RT_X84M) {
      //X84M: keeps all features with sad <= _MaxResiduumMAD
      if ((res[i]>=0 && (_all_diff_from_median[i] > threshold))
          &&(sad[i] > _MaxResiduumMAD)){
        res[i]=TRACKER_X84;
        BIASCDOUT(D_TRACKERB_REJECT, "rejecting point "<<setw(4)<<i
                  <<": sad = "<<sad[i]<<"/"<<_MaxResiduumMAD<<"  diff from median "<<fixed<<right
                  <<setprecision(2)<<setw(10)<<_all_diff_from_median[i]<<endl);
        rejected++;
      } else if (res[i]==TRACKER_MEANABSDIF && 
                 (_all_diff_from_median[i] < threshold)){
        res[i]=0;
        BIASCDOUT(D_TRACKERB_REJECT, "accepting point "<<setw(4)<<i
                  <<": sad = "<<sad[i]<<"  diff from median "<<fixed<<right
                  <<setprecision(2)<<setw(10)<<_all_diff_from_median[i]<<endl);
      }
    } else {
      //standard X84
      if (res[i]>=0 && (_all_diff_from_median[i] > threshold)){
        res[i]=TRACKER_X84;
        BIASCDOUT(D_TRACKERB_REJECT, "rejecting point "<<setw(4)<<i
                  <<": diff from median "<<fixed<<right<<setprecision(2)
                  <<setw(10)<<_all_diff_from_median[i]<<endl);
        rejected++;
      } else if (res[i]==TRACKER_MEANABSDIF && 
                 (_all_diff_from_median[i] < threshold)){
        res[i]=0;
        BIASCDOUT(D_TRACKERB_REJECT, "accepting point "<<setw(4)<<i
                  <<": diff from median "<<fixed<<right<<setprecision(2)
                  <<setw(10)<<_all_diff_from_median[i]<<endl);
      }
    }
  }
  BIASCDOUT(D_TRACKERB_REJECT, "rejected "<<rejected<<" points "<<endl);

#ifdef BIAS_DEBUG
  if (rejected>num2/2){
    cout << "rejected more than half of tracks ("<<rejected<<" of "<<num2
         <<")aborting\n";
    for (unsigned i=0; i<num2; i++){
      cout << setw(4)<<i<<" residuum "<<setprecision(4)<<setw(10)<<fixed
           <<right<<_ssd_residui[i]<<"\tdiff_from_median "<<setprecision(4)
           <<setw(10)<<fixed<<right<<_diff_from_median[i]<<endl;
    }
    
    sort(_ssd_residui.begin(), _ssd_residui.end());
    sort(_diff_from_median.begin(), _diff_from_median.end());
    cout << "sorted\n";
    for (unsigned i=0; i<num2; i++){
      cout << setw(4)<<i<<" residuum "<<setprecision(4)<<setw(10)<<fixed
           <<right<<_ssd_residui[i]<<"\tdiff_from_median "<<setprecision(4)
           <<setw(10)<<fixed<<right<<_diff_from_median[i]<<endl;
    }

    BIASABORT;
  }
#endif

#ifdef TIMING
  timer.Stop();
  cerr << "TrackerBase::Rejectx86 took "<<timer.GetRealTime()
       <<" us = "<<timer.GetCycleCount()<<" cycles \n";
#endif
  return 0;
}

template <class StorageType>
KLT_TYPE TrackerBaseInterface<StorageType>::
ComputeMaskSum(const FilterMask& mask)
{
  if (mask.IsSeparable())
  {
    const Vector<FM_FLOAT>& vert = *mask.GetSepfv();
    const Vector<FM_FLOAT>& horz = *mask.GetSepfh();
    KLT_TYPE sum = (KLT_TYPE)0;
    for (int i = 0; i < (int)vert.Size(); i++)
    {
      for (int j = 0; j < (int)horz.Size(); j++)
      {
    sum += (KLT_TYPE)fabs(vert[i] * horz[j]);
      }
    }
    return sum;
  }
  else
  {
    const Matrix<FM_FLOAT>& kernel = *mask.GetKernelf();
    KLT_TYPE sum = (KLT_TYPE)0;
    for (int i = 0; i < (int)kernel.GetRows(); i++)
    {
      for (int j = 0; j < (int)kernel.GetCols(); j++)
      {
    sum += (KLT_TYPE)fabs(kernel[i][j]);
      }
    }
    return sum;
  }
}

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

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