TrackerBaseAffine2.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 "TrackerBaseAffine2.hh"
#include "MathAlgo/SVD.hh"
#include "Filter/Gauss.hh"
#include "Base/Math/Matrix3x3.hh"
#include "Base/Image/ImageIO.hh"
#include "Image/AffineMapping.hh"
#include "Base/Common/FileHandling.hh"
#include "MathAlgo/UnscentedTransform.hh"
#include "Base/Image/ImageIO.hh"


const double DEFAULT_MAX_AFFINECHANGE = 0.3;

using namespace BIAS;
using namespace std;

namespace BIAS
{

// functions from trackerbaseaffine.cpp
void Compute4by1ErrorVector(KLT_TYPE* imgdiff,
                            KLT_TYPE* gradx,
                            KLT_TYPE* grady,
                            const int hw,
                            const int hh,
                            BIAS::Vector<KLT_TYPE>& e);

void Compute4by4GradientMatrix(KLT_TYPE* gradx,
                               KLT_TYPE* grady,
                               const int hw,
                               const int hh,
                               BIAS::Matrix<KLT_TYPE>& T);

int  GaussJordanElimination(BIAS::Matrix<KLT_TYPE>& a,
                            BIAS::Vector<KLT_TYPE>& b);

void Compute6by1ErrorVector(KLT_TYPE* imgdiff,
                            KLT_TYPE* gradx,
                            KLT_TYPE* grady,
                            const int hw,
                            const int hh,
                            BIAS::Vector<KLT_TYPE>& e);

void Compute6by6GradientMatrix(KLT_TYPE* gradx,
                               KLT_TYPE* grady,
                               const int hw,
                               const int hh,
                               BIAS::Matrix<KLT_TYPE>& T);

/**
 * @class AffineTransform
 * @brief transform 6D uncertainty into 2d image uncertainty in same image
 * @internal
 */
class AffineTransform: public UnscentedTransform {
public:
  AffineTransform() {
    dx_ = 0;
    dy_ = 0;
    A_.SetZero();
    tx_ = 0;
    ty_ = 0;
    SetAlpha(1.0);
  }
  virtual int Transform_(const Vector<double>& src,
                         Vector<double>& dst) const {
    BIASASSERT(src.size()==6);
    dst.newsize(2);
    //cout<<"trying "<<src<<": with A "<<A_<<" and t "<<tx_<<";"<<ty_<<" ";
    double tempx = A_[0][0] * dx_ + A_[0][1] * dy_;// + tx_;
    double tempy = A_[1][0] * dx_ + A_[1][1] * dy_;// + ty_;
    dst[0] = src[0] * tempx + src[1] * tempy + src[4];
    dst[1] = src[2] * tempx + src[3] * tempy + src[5];
    //cout<<"("<<dx_<<";"<<dy_<<")->"<<dst<<endl;
    return 0;
  }
  double dx_,dy_;
  Matrix2x2<double> A_;
  double tx_,ty_;
};

/**
 * @class AffineBackTransform
 * @brief transform 6D uncertainty into 2d image uncertainty in same image
 * @internal
 */
class AffineBackTransform: public UnscentedTransform {
public:
  AffineBackTransform() {
    dx_ = 0;
    dy_ = 0;
    A_.SetZero();
    tx_ = 0;
    ty_ = 0;
    SetAlpha(1.0);
  }
  virtual int Transform_(const Vector<double>& src,
                         Vector<double>& dst) const {
    BIASASSERT(src.size()==6);
    dst.newsize(2);
    double tempx = src[0] * dx_ + src[1] * dy_ + src[4];
    double tempy = src[2] * dx_ + src[3] * dy_ + src[5];
    dst[0] = A_[0][0] * tempx + A_[0][1] * tempy + tx_;
    dst[1] = A_[1][0] * tempx + A_[1][1] * tempy + ty_;

    return 0;
  }
  double dx_,dy_;
  Matrix2x2<double> A_;
  double tx_,ty_;
};


template <class StorageType>
TrackerBaseAffine2<StorageType>::
TrackerBaseAffine2(bool only4params)
{
  BIASASSERT(!only4params);
  _MaxAffineChange = DEFAULT_MAX_AFFINECHANGE;
  _SimilarityTransformOnly           = only4params;
  affineNormalization_ = false;
  initialCov_.newsize(6,6);
  initialCov_.SetIdentity();
  initialCov_ *= 0.001;
  initialCov_[4][4] = 0.05;
  initialCov_[5][5] = 0.05;


}

template <class StorageType>
int TrackerBaseAffine2<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 BIAS_DEBUG
  if (AffinePred.NormFrobenius()<1e-10) {
    BIASERR("affine guess seems unitialized, set at least identity");
  }
#endif
  residuumMAD_ = 0;
  residuumMSD_ = 0;

  int res = TrackAffine_(p1.GetData(), p2.GetData(), AffinePred,
                         result.GetData(), AffineResult,
                         error, iter, Cov_, residuumMAD_, residuumMSD_,
                         _SimilarityTransformOnly, true);
  return res;
}


// affine transformation may maginify region. in this case remove
// high frequencies
double GetBaseLevel(const Vector<double>& apar, bool backdirection) {
  Matrix2x2<double> A;
  A[0][0] = apar[0];
  A[0][1] = apar[1];
  A[1][0] = apar[2];
  A[1][1] = apar[3];

  Matrix<double> ATA;
  A.GetSystemMatrix(ATA);
  SVD mysvd(ATA);
  Vector<double> S = mysvd.GetS();

  double basesmooth = (backdirection)
    ? sqrt(0.5*(1.0/S[0]+1.0/S[1]))
    : sqrt(0.5*(S[0]+S[1]));
  if (basesmooth<=1.0) return 0;
  else return(log2(basesmooth));

}

#define INVALID_PIXEL -1e10

template <class StorageType>
void TrackerBaseAffine2<StorageType>::
BilinearRegion1_(KLT_TYPE x, KLT_TYPE y,
                 int hws, const Vector<double>& apar,
                 const Matrix<double>& Cov) {

  // if detATA<1 smooth this image, because we wont have high frequencies
  // in im2, which will be magnified
  double basesmooth = GetBaseLevel(apar, true);
  cout<<"basesmooth for image 1 is "<<basesmooth<<" with apar "<<apar<<endl;
  const double maxLayer = _pgradim2x.size()-1;

  AffineBackTransform AT;
  Matrix3x3<double> H(MatrixZero), tmp;
  H[0][0] = apar[0];
  H[0][1] = apar[1];
  H[1][0] = apar[2];
  H[1][1] = apar[3];
  H[0][2] = apar[4];
  H[1][2] = apar[5];
  H[2][2] = 1.0;
#ifdef BIASASSERT_ISACTIVE
  int ires =
#endif
    H.GetInverse(tmp);
  BIASASSERT(ires ==0);
  H = tmp;
  cout<<"inverted A is "<<H<<endl;
  AT.A_[0][0] = H[0][0];
  AT.A_[0][1] = H[0][1];
  AT.A_[1][0] = H[1][0];
  AT.A_[1][1] = H[1][1];
  AT.tx_ = H[0][2];
  AT.ty_ = H[1][2];



  Vector<double> offset;
  Matrix<double> dstcov;

  //  Image<StorageType>& image =  *_im1;
  //Image<StorageType>& gradx = *this->_gradim1x;
  //Image<StorageType>& grady = *this->_gradim1y;
  Matrix<KLT_TYPE>& bl = this->_bl1;
  Matrix<KLT_TYPE>& gx = this->_gx1;
  Matrix<KLT_TYPE>& gy = this->_gy1;
  bl.SetZero();
  gx.SetZero();
  gy.SetZero();

  //  StorageType** image_ida = image.GetImageDataArray();
  //StorageType** gradx_ida = gradx.GetImageDataArray();
  //StorageType** grady_ida = grady.GetImageDataArray();

  int x_floor = (int)floor(x);
  int y_floor = (int)floor(y);
  int x_floor_end = x_floor+hws+1;
  int y_floor_end = y_floor+hws+1;
  KLT_TYPE brdy = y - (KLT_TYPE)(y_floor);
  KLT_TYPE brdx = x - (KLT_TYPE)(x_floor);
  //KLT_TYPE brdxy = brdx*brdy;
  cout<<"affine parameters "<<apar <<"with predicted cov "<<Cov<<endl;
  cout<<"image 1 scales: "<<endl;
  int rf, rc, cf, cc, r, c;
  for (rf=y_floor-hws, rc=rf+1, r=0; rf<y_floor_end; rf++, rc++, r++) {
    for (cf=x_floor-hws, cc=cf+1, c=0; cf<x_floor_end; cf++, cc++, c++) {

      ///////////  pyramid layer
      AT.dx_ = cf-x_floor;
      AT.dy_ = rf-y_floor;
      AT.Transform(apar,Cov, offset,dstcov);
      // d==0.5   =>    use layer 0
      double layer = log2(sqrt(dstcov.Trace()));
      cout<<fixed<<setprecision(2)<<setw(6)<<layer<<" ";
      if (layer<0.0) layer = 0;
      if (layer>maxLayer) layer=maxLayer;

      const double scale = 1.0/exp2(rint(layer));

      const double myx = cf+brdx, myy = rf+brdy;

      if (_pgradim1x[int(rint(layer))]->IsInROI(scale*myx,scale*myy)) {
        ///////////  pyramid layer
        _pim1.GetTrilinearImageValue(myx, myy, layer, bl[r][c]);
        BIASASSERT(!BIAS_ISINF( bl[r][c]));
        BIASASSERT(bl[r][c]<1e10);
        _pgradim1x.GetTrilinearImageValue(myx, myy, layer, gx[r][c]);
        BIASASSERT(!BIAS_ISINF( gx[r][c]));
        _pgradim1y.GetTrilinearImageValue(myx, myy, layer, gy[r][c]);
        BIASASSERT(!BIAS_ISINF( gy[r][c]));
      } else {
        bl[r][c] =  gx[r][c] =  gy[r][c] = INVALID_PIXEL;
      }
      //cout<<r<<";"<<c<<" ";
    }
    cout<<endl;
  }
  if (_AffineBrightnessInvariance)  dev1_ = this->NormalizeRegion_(bl, gx, gy);
  else dev1_ = 1.0;
}



// ***************** parts, which are not under LGPL *************************
// The function TrackAffine_ is based on the affine tracker by Thorsten
// Thormaehlen. Some of the worker functions have been rewritten, some
// have been adapted, see below for the conditions on this software.

template <class StorageType>
int TrackerBaseAffine2<StorageType>::
TrackAffine_(KLT_TYPE op1[2], KLT_TYPE op2[2],
             const Matrix2x2<KLT_TYPE>& op2A,
             KLT_TYPE oresult[2],  Matrix2x2<KLT_TYPE>& oresultA,
             KLT_TYPE& error, int &iteration, Matrix<KLT_TYPE>& Cov,
             KLT_TYPE& mad, KLT_TYPE& msd,
             bool SimilarityTransformOnly, bool ComputeCovariance)

{

#ifdef BIAS_DEBUG
  static int featurecounter = 0;
  featurecounter++;
#endif

  KLT_TYPE p1[2]={op1[0],op1[1]};
  KLT_TYPE p2[2]={op2[0],op2[1]};
  Matrix2x2<KLT_TYPE> p2A = op2A;
  KLT_TYPE result[2]={oresult[0], oresult[1]};
  Matrix2x2<KLT_TYPE> resultA = oresultA;


  //this->AddDebugLevel(D_TRACKERB_AFFINE | D_TRACKERB_KLT);
  BIASDOUT(D_TRACKERB_AFFINE, "Called Affine for "
           <<p1[0]<<";"<<p1[1]<<", guess is "<<p2[0]<<";"<<p2[1]
           <<" affine guess is "<< p2A);
  cout<<"window size is "<< _WinSize<<endl;
  const int width = _WinSize;
  const int height = _WinSize;
  const int hw = width/2;
  const int hh = height/2;

  const KLT_TYPE fhw = hw;
  const KLT_TYPE fhh = hh;

  if (_minx1 >= p1[0] || _miny1 >= p1[1] || _maxx1 < p1[0] || _maxy1 < p1[1]){
    BIASDOUT(D_TRACKERB_KLT, "original point is too close to border: "
             <<p1[0]<<";"<<p1[1]);
    return  TRACKER_OUTOFIMAGE;
  }


  const int max_iterations = _MaxIterations;
  const KLT_TYPE maxerr    = _MaxError;
  const KLT_TYPE th_aff    = _MaxAffineChange;

  KLT_TYPE dx=0, dy=0;
  iteration = 0;
  int status = 0;

  const unsigned int num_params = SimilarityTransformOnly?4:6;

  Matrix<KLT_TYPE> T(num_params, num_params);
  Vector<KLT_TYPE> a(num_params);



  Cov = initialCov_;

  Matrix<double> Cov2(Cov);

  resultA = p2A;
  KLT_TYPE *Axx = &(resultA[0][0]);
  KLT_TYPE *Axy = &(resultA[0][1]);
  KLT_TYPE *Ayx = &(resultA[1][0]);
  KLT_TYPE *Ayy = &(resultA[1][1]);

  result[0] = p2[0];
  result[1] = p2[1];

  bool convergence = false;


  // Allocate memory for windows
  KLT_TYPE *imgdiff = new KLT_TYPE[width * height];
  KLT_TYPE *gradx   = new KLT_TYPE[width * height];
  KLT_TYPE *grady   = new KLT_TYPE[width * height];

  // compute reference window only ONCE
  // BilinearRegion1_(p1[0], p1[1], hh);



  // we could compute pyramid size from initial parameter uncertainty here ...
  int ps = int(rint(log2(double(_im1->GetWidth()+_im1->GetHeight()))))-6;


  Image<StorageType> im1, gradim1x, gradim1y;
  Image<StorageType> im2, gradim2x, gradim2y;

  Matrix3x3<double> R2(MatrixIdentity);

  if (affineNormalization_) {

    // when encoding the affine transformation into a 3x3 matrix and operating
    // on homogeneous points, we get
    // x2 = A * x1
    // If we now normalize the region in im1 on which we track (by R1) we get
    // x2 = A' * R1 * x1

    AffineMapping<StorageType, StorageType> ShapeNormalizer;
    ShapeNormalizer.SetAffineTransformation(R1_[0][0], R1_[0][1],
                                            R1_[1][0], R1_[1][1],
                                            R1_[0][2], R1_[1][2]);

    im1.Init(10*hw, 10*hw);
    ShapeNormalizer.MapDirect( *this->_im1, im1);
    gradim1x.Init(10*hw, 10*hw);
    ShapeNormalizer.MapDirect( *this->_gradim1x, gradim1x);
    gradim1y.Init(10*hw, 10*hw);
    ShapeNormalizer.MapDirect( *this->_gradim1y, gradim1y);

    // adapt coordinates in first image and A prediction
    HomgPoint2D x1(p1[0], p1[1]);
    x1 = R1_ * x1;
    result[0] = p1[0] = x1[0];
    result[1] = p1[1] = x1[1];



    // We want to normalize the region in the other image by R2
    // x2 = R2^-1 * A" * R1 * x1
    // such that if the prediction is correct we end up with an identic image
    // A=Apred => A"=id => Apred = R2^-1 * R1 =>  R2 = R1 * Apred^-1



    Matrix3x3<double> A3x3(MatrixZero), A3x3I;
    A3x3[0][0] = resultA[0][0];
    A3x3[0][1] = resultA[0][1];
    A3x3[1][0] = resultA[1][0];
    A3x3[1][1] = resultA[1][1];
    A3x3[0][2] = p2[0];
    A3x3[1][2] = p2[1];
    A3x3[2][2] = 1.0;

    // compute R2
#ifdef BIASASSERT_ISACTIVE
    int ires =
#endif
      A3x3.GetInverse(A3x3I);
    BIASASSERT(ires==0);
    R2 = R1_ * A3x3I;
    ShapeNormalizer.SetAffineTransformation(R2[0][0], R2[0][1],
                                            R2[1][0], R2[1][1],
                                            R2[0][2], R2[1][2]);
    im2.Init(10*hw, 10*hw);
    ShapeNormalizer.MapDirect( *_im2, im2);
    gradim2x.Init(10*hw, 10*hw);
    ShapeNormalizer.MapDirect( *_gradim2x, gradim2x);
    gradim2y.Init(10*hw, 10*hw);
    ShapeNormalizer.MapDirect( *_gradim2y, gradim2y);

    // adapt coordinates in second image amd prediction
    p2A.SetIdentity();
    resultA.SetIdentity();

#ifdef BIAS_DEBUG
    //ImageIO::Save("_im1", im1);
    //ImageIO::Save("_im2", im2);
    //ImageIO::Save("_gradim1x",gradim1x);
    //ImageIO::Save("_gradim1y",gradim1y);
    //ImageIO::Save("_gradim2x",gradim2x);
    //ImageIO::Save("_gradim2y",gradim2y);
    ImageIO::Save("_im1", im1);
    ImageIO::Save("_im2", im2);
    ImageIO::Save("_gradim1x",gradim1x);
    ImageIO::Save("_gradim1y",gradim1y);
    ImageIO::Save("_gradim2x",gradim2x);
    ImageIO::Save("_gradim2y",gradim2y);
#endif

  } else {
    im1 = *_im1;
    gradim1x = *this->_gradim1x ;
    gradim1y = *this->_gradim1y;
    im2 = *_im2;
    gradim2x = *this->_gradim2x;
    gradim2y = *this->_gradim2y;
  }





  BIASWARN("Create image pyramids ...");
  Gauss<StorageType, StorageType> theGauss;
  theGauss.SetSigma(0.8);
  _pim1.SetLowPassFilter(theGauss);
  //_pim1.SetPyramidSize(ps);
  _pgradim1x.SetLowPassFilter(theGauss);
  //_pgradim1x.SetPyramidSize(ps);
  _pgradim1y.SetLowPassFilter(theGauss);
  //_pgradim1y.SetPyramidSize(ps);
  _pim2.SetLowPassFilter(theGauss);
  //_pim2.SetPyramidSize(ps);
  _pgradim2x.SetLowPassFilter(theGauss);
  //_pgradim2x.SetPyramidSize(ps);
  _pgradim2y.SetLowPassFilter(theGauss);
  //_pgradim2y.SetPyramidSize(ps);

  if (!_pim1.IsEmpty()) _pim1.Clear();
  _pim1.Init(im1, ps);
  if (!this->_pgradim1x.IsEmpty()) this->_pgradim1x.Clear();
  this->_pgradim1x.Init(gradim1x, ps);
  if (!this->_pgradim1y.IsEmpty()) this->_pgradim1y.Clear();
  this->_pgradim1y.Init(gradim1y, ps);
  if (!_pim2.IsEmpty()) _pim2.Clear();
  _pim2.Init(im2, ps);
  if (!_pgradim2x.IsEmpty()) _pgradim2x.Clear();
  _pgradim2x.Init(gradim2x, ps);
  if (!_pgradim2y.IsEmpty()) _pgradim2y.Clear();
  _pgradim2y.Init(gradim2y, ps);


#ifdef BIAS_DEBUG
  _pim1.WriteImages("pim1");
  _pgradim1x.WriteImages("pgx1");
  _pgradim1y.WriteImages("pgy1");
  _pim2.WriteImages("pim2");
  _pgradim2x.WriteImages("pgx2");
  _pgradim2y.WriteImages("pgy2");
#endif



  BIASASSERT(_pim1.GetPositionOffset()==0.0);
  Vector<double> apar(6);
  bool lastconvergence = false;
  do  {
    //cout<<"Cov2 is "<<Cov2<<endl;
    apar[0] = *Axx;
    apar[1] = *Axy;
    apar[2] = *Ayx;
    apar[3] = *Ayy;
    apar[4] = result[0];
    apar[5] = result[1];
    BilinearRegion1_(p1[0], p1[1], hh, apar, Cov);

    KLT_TYPE ul_x =  result[0] + *Axy *  fhh  - *Axx * fhw;
    KLT_TYPE ul_y =  result[1] + *Ayy *  fhh  - *Ayx * fhw;
    KLT_TYPE ll_x =  result[0] - *Axx *  fhw  - *Axy * fhh;
    KLT_TYPE ll_y =  result[1] - *Ayx *  fhw  - *Ayy * fhh;
    KLT_TYPE ur_x =  result[0] + *Axx *  fhw  + *Axy * fhh;
    KLT_TYPE ur_y =  result[1] + *Ayx *  fhw  + *Ayy * fhh;
    KLT_TYPE lr_x =  result[0] + *Axx *  fhw  - *Axy * fhh;
    KLT_TYPE lr_y =  result[1] + *Ayx *  fhw  - *Ayy * fhh;

    if ( ul_x  < _minx2 ||  ul_x  > _maxx2 ||
         ll_x  < _minx2 ||  ll_x  > _maxx2 ||
         ur_x  < _minx2 ||  ur_x  > _maxx2 ||
         lr_x  < _minx2 ||  lr_x  > _maxx2 ||
         ul_y  < _miny2 ||  ul_y  > _maxy2 ||
         ll_y  < _miny2 ||  ll_y  > _maxy2 ||
         ur_y  < _miny2 ||  ur_y  > _maxy2 ||
         lr_y  < _miny2 ||  lr_y  > _maxy2) {
      status = TRACKER_OUTOFIMAGE;
      BIASDOUT( D_TRACKERB_KLT, "out of image");
      break;
    }

    // compute gradients and apply dev2_ to gradients
    GetGradientWinAffine_(result[0], result[1],
                          *Axx, *Ayx , *Axy, *Ayy,
                          hw, hh, imgdiff, gradx, grady, Cov);
#ifdef BIAS_DEBUG
    if (this->GetDebugLevel() & D_TRACKERB_AFFINE) {
      stringstream ss;
      ss<<"Af2_"<<setw(3)<<setfill('0')<<int(featurecounter)<<"-"
        <<setw(4)<<setfill('0')<<int(iteration)<<"-";
      string imname = ss.str();
      Image<float> im1(2*hw+1, 2*hh+1), im2(2*hw+1, 2*hh+1);
      im1.SetZero();
      im2.SetZero();
      for (int y=0; y<2*hh+1; y++) {
        for (int x=0; x<2*hw+1; x++) {
          im1.GetImageDataArray()[y][x] = float(_bl1[y][x]);
          im2.GetImageDataArray()[y][x] = float(_bl2[y][x]);
        }
      }
      //ImageIO::Save(imname+FileHandling::toString("1"), im1);
      //ImageIO::Save(imname+FileHandling::toString("2"), im2);
      ImageIO::Save(imname+FileHandling::toString("1"), im1);
      ImageIO::Save(imname+FileHandling::toString("2"), im2);
    }
#endif

    if (SimilarityTransformOnly){

      Compute4by1ErrorVector(imgdiff, gradx, grady, hw, hh, a);
      Compute4by4GradientMatrix(gradx, grady, hw, hh, T);

      status = GaussJordanElimination(T,a);

      *Axx += a[0];
      *Ayx += a[1];
      *Ayy = *Axx;
      *Axy = -(*Ayx);

      result[0] += dx = a[2];
      result[1] += dy = a[3];

      BIASERR("no cov!");
      BIASABORT;

    } else {

      Compute6by1ErrorVector(imgdiff, gradx, grady, hw, hh, a);
      Compute6by6GradientMatrix(gradx, grady, hw, hh, T);



      msd = 0;
      const unsigned int wh = width*height;
      register KLT_TYPE* fw = imgdiff;
      for (unsigned int h=0; h<wh ; h++) {
        const KLT_TYPE cur = (KLT_TYPE)fabsf((float) (*fw) );
        fw++;
        msd += cur*cur;
      }

      msd /= dev1_*dev1_;
      //      double sigma = msd / (double)(wh - SimilarityTransformOnly?4:6);
      msd /= double(wh);
      //SVD covsvd(T);
      //too bad:
      //Cov = covsvd.Invert() * sigma;


      BIASDOUT(D_TRACKERB_KLT, "inhom. eq. system is "
               <<setprecision(4)<<setw(6)<<T<<" "<< a);

      status = GaussJordanElimination(T,a);

      *Axx += a[0];
      *Ayx += a[1];
      *Axy += a[2];
      *Ayy += a[3];

      result[0] += dx = a[4];
      result[1] += dy = a[5];


      Cov.SetZero();
#define DATHRESH 0.05
      for (unsigned int i=0; i<4; i++) {
        if (fabs(a[i])<DATHRESH) Cov[i][i] = 4.0*a[i]*a[i];
        else Cov[i][i] = 4.0*DATHRESH*DATHRESH;
      }
#define DXTHRESH 0.25
      if (fabs(dx)<DXTHRESH) Cov[4][4] = 4.0*dx*dx;
      else Cov[4][4]= 4.0*DXTHRESH*DXTHRESH;
      if (fabs(dy)<DXTHRESH) Cov[5][5] = 4.0*dy*dy;
      else Cov[5][5]= 4.0*DXTHRESH*DXTHRESH;

      cout<<"heuristic cov "<<Cov<<" and last cov "<<Cov2<<endl;
      // make symmetric and average with 1/3 old cov from last iteration
      for (unsigned int i=0; i<6; i++) {
        for (unsigned int j=i; j<6; j++) {
          Cov[i][j] = 0.25*(Cov[i][j]+Cov[j][i]+Cov2[i][j]+Cov2[j][i]);
        }
      }
      for (unsigned int i=1; i<6; i++) {
        for (unsigned int j=0; j<i; j++) {
          Cov[i][j] = Cov[j][i];
        }
      }
      // remember old cov for next iteration
      Cov2 = Cov;
      cout<<endl<<"Cov is now "<<Cov<<endl;




    }
    BIASDOUT(D_TRACKERB_KLT, "parameter update a is : "<< a);
    double posdiag =
      (*Axx + *Axy) * (*Axx + *Axy) + (*Ayx + *Ayy) * (*Ayx + *Ayy);

    double negdiag =
      (*Axy - *Axx) * (*Axy - *Axx) + (*Ayy - *Ayx) * (*Ayy - *Ayx);

    double ratio = (posdiag>negdiag) ? negdiag / posdiag : posdiag / negdiag;

    // factor 3 is allowed
    if (ratio < 0.09) {
      status = TRACKER_ODDAFFINEWARP;
      BIASDOUT(D_TRACKERB_KLT, "diag ratio is too big: "<< sqrt(ratio));
      break;
    }

    double scalex = (*Axx * *Axx) + (*Ayx * *Ayx);
    double scaley = (*Ayy * *Ayy) + (*Axy * *Axy);


    ratio = (scalex>scaley) ? scaley / scalex : scalex / scaley;
    // factor 3 is allowed
    if (ratio < 0.09) {
      status = TRACKER_ODDAFFINEWARP;
      BIASDOUT(D_TRACKERB_KLT, "axes ratio is too big: "<< sqrt(ratio));
      break;
    }


    // old upper left corner - new upper left corner
    ul_x -=  result[0] - *Axx * fhw + *Axy * fhh;
    ul_y -=  result[1] - *Ayx * fhw + *Ayy * fhh;
    // old lower left corner - new lower left corner
    ll_x -=  result[0] - *Axx * fhw - *Axy * fhh;
    ll_y -=  result[1] - *Ayx * fhw - *Ayy * fhh;
    // old upper right corner - new upper right corner
    ur_x -=  result[0] + *Axx * fhw + *Axy * fhh;
    ur_y -=  result[1] + *Ayx * fhw + *Ayy * fhh;
    // old lower right corner - new lower right corner
    lr_x -=  result[0] + *Axx * fhw - *Axy * fhh;
    lr_y -=  result[1] + *Ayx * fhw - *Ayy * fhh;

    convergence = (fabs(dx) < maxerr && fabs(dy) < maxerr  &&
                   fabs(ul_x) < th_aff && fabs(ul_y) < th_aff &&
                   fabs(ll_x) < th_aff && fabs(ll_y) < th_aff &&
                   fabs(ur_x) < th_aff && fabs(ur_y) < th_aff &&
                   fabs(lr_x) < th_aff && fabs(lr_y) < th_aff);
    if (convergence) {
      if (!lastconvergence) {
        // convergence is true for the first time, check another time with
        // possibly more high-frequent image content
        convergence = false;
        lastconvergence = true;
        Cov.SetZero();
        // set small cov to see best resolution
        for (int i=0; i<Cov.num_rows(); i++) {
          Cov[i][i] = 0.0001;
        }

      }
    } else lastconvergence = false;

    if (status == TRACKER_NOSTRUCTURE) {
      BIASDOUT( D_TRACKERB_KLT, "no structure for tracking");
      break;
    }
    iteration++;
  }  while ( !convergence  && iteration < max_iterations);
  if (iteration >= max_iterations) {
    status = TRACKER_TOOMANYITER;
    BIASDOUT( D_TRACKERB_KLT, "too many iter: "<<iteration
              <<" maxiter="<<max_iterations);
  }
  mad = 0;
  msd = 0;
  if ((status==TRACKER_TOOMANYITER) || (status==TRACKER_SUCCESS)) {

    // if we compute the difference here, we must check the range again

    const unsigned int wh = width*height;
    register KLT_TYPE* fw = imgdiff;
    for (unsigned int h=0; h<wh ; h++) {
      const KLT_TYPE cur = (KLT_TYPE)fabsf((float) (*fw) );
      fw++;
      mad += cur;
      msd += cur*cur;
    }

    mad /= double(wh)*dev1_;
    msd /= dev1_*dev1_;
    double sigma = msd / (double)(wh - SimilarityTransformOnly?4:6);
    msd /= double(wh);

    if (ComputeCovariance) {
      if (SimilarityTransformOnly)
        Compute4by4GradientMatrix(gradx, grady, hw, hh, T);
      else
        Compute6by6GradientMatrix(gradx, grady, hw, hh, T);
      SVD covsvd(T);
      Cov = covsvd.Invert() * sigma;
    } else {
      // pseudo covariance (last step size)
      Cov.SetZero();
      if (SimilarityTransformOnly) {
        Cov[2][2] = dx * dx;
        Cov[3][3] = dy * dy;
      } else {
        Cov[4][4] = dx * dx;
        Cov[5][5] = dy * dy;
      }
    }
  }

  error = (fabs(dx)>fabs(dy))?fabs(dx):fabs(dy);

  // Check whether window is out of bounds
  if (result[0]<_minx2 || result[0] > _maxx2  ||
      result[1]<_miny2 || result[1] > _maxy2)
    status = TRACKER_OUTOFIMAGE;


  // Free memory
  delete imgdiff;
  delete gradx;
  delete grady;

  BIASDOUT( D_TRACKERB_KLT, "status at end is "<<status);




  if (affineNormalization_) {

    HomgPoint2D hresult(result[0],result[1]);
    Matrix3x3<double> R2i;
#ifdef BIASASSERT_ISACTIVE
    int ires =
#endif
      R2.GetInverse(R2i);
    BIASASSERT(ires==0);
    hresult = R2i * hresult;
    oresult[0] = hresult[0];
    oresult[1] = hresult[1];

    Matrix3x3<double> hresultA(MatrixIdentity);;
    hresultA[0][0] = resultA[0][0];
    hresultA[0][1] = resultA[0][1];
    hresultA[1][0] = resultA[1][0];
    hresultA[1][1] = resultA[1][1];



    hresultA = R2i * hresultA;


    oresultA[0][0] = hresultA[0][0];
    oresultA[0][1] = hresultA[0][1];
    oresultA[1][0] = hresultA[1][0];
    oresultA[1][1] = hresultA[1][1];

    BIASWARN("Covariance is not yet adapted to coordinate system transform!");



  } else {
    oresult[0] = result[0];
    oresult[1] = result[1];
    oresultA = resultA;
  }





  // Return appropriate value
  return status;
}

template <class StorageType>
void TrackerBaseAffine2<StorageType>::
EvaluateResult_(KLT_TYPE& mad, KLT_TYPE& msd, Matrix<KLT_TYPE>& CovMatrix) {
  // these values have already been computed in the track_ function !!!
  if (_SimilarityTransformOnly) {
    CovMatrix.newsize(4,4);
  } else {
    CovMatrix.newsize(6,6);
  }
  CovMatrix = Cov_;

  mad = residuumMAD_;
  msd = residuumMSD_;
}


/**********************************************************************
* CONSISTENCY CHECK OF FEATURES BY AFFINE MAPPING (BEGIN)
*
* Created by: Thorsten Thormaehlen (University of Hannover) June 2004
* thormae@tnt.uni-hannover.de
*
* Permission is granted to any individual or institution to use, copy, modify,
* and distribute this part of the software, provided that this complete
* authorship and permission notice is maintained, intact, in all copies.
*
* This software is provided  "as is" without express or implied warranty.
*
*
*/

template <class StorageType>
void TrackerBaseAffine2<StorageType>::
GetGradientWinAffine_(const KLT_TYPE& x, const KLT_TYPE& y,
                      const KLT_TYPE& Axx, const KLT_TYPE& Ayx,
                      const KLT_TYPE& Axy, const KLT_TYPE& Ayy,
                      const int hw, const int hh,
                      KLT_TYPE* imgdiff,
                      KLT_TYPE* out_gradx,
                      KLT_TYPE* out_grady,
                      const Matrix<double>& Cov) {
  BIASASSERT(!_ComputeFilteredImages);
  const double maxLayer = _pgradim2x.size()-1;
  Vector<double> apar(6);
  apar[0] = Axx;
  apar[1] = Axy;
  apar[2] = Ayx;
  apar[3] = Ayy;
  apar[4] = x;
  apar[5] = y;

  AffineTransform AT;
  Matrix3x3<double> H(MatrixZero), tmp;
  H[0][0] = apar[0];
  H[0][1] = apar[1];
  H[1][0] = apar[2];
  H[1][1] = apar[3];
  H[0][2] = apar[4];
  H[1][2] = apar[5];
  H[2][2] = 1.0;
#ifdef BIASASSERT_ISACTIVE
  int ires =
#endif
    H.GetInverse(tmp);
  BIASASSERT(ires==0);
  H = tmp;
  AT.A_[0][0] = H[0][0];
  AT.A_[0][1] = H[0][1];
  AT.A_[1][0] = H[1][0];
  AT.A_[1][1] = H[1][1];
  AT.tx_ = H[0][2];
  AT.ty_ = H[1][2];
  cout<<"inverted A is "<<H<<endl;
  Vector<double> offset;
  Matrix<double> dstcov;

  // if detATA>1 smooth this image to avoid aliasing, because the template
  // in image one is smaller than this
  double basesmooth = GetBaseLevel(apar, false);
  cout<<"basesmooth for image 2 is "<<basesmooth<<" with apar "<<apar<<endl;
  {
    register int i, j;
    BIASASSERT(hh==hw);
    KLT_TYPE* imgdifforig = imgdiff;

    mean1_ = mean2_ = 0;
    BIASASSERT(_bl1.num_rows()==_bl1.num_cols());
    BIASASSERT(_bl1.num_rows()==2*hw+1);
    KLT_TYPE mi,mj;
    KLT_TYPE *pbl1 = _bl1.GetData();
    KLT_TYPE *pbl2 = _bl2.GetData();
    cout<<"image 2 scales for apar "<<apar<<endl;
    for (j = -hh ; j <= hh ; j++) {
      for (i = -hw ; i <= hw ; i++)  {


        Matrix<double> pt(2,6, MatrixZero);
        pt[0][0] = pt[1][2] = i;
        pt[0][1] = pt[1][3] = j;
        pt[0][4] = pt[1][5] = 1.0;
        dstcov = pt * Cov * pt.Transpose();

        ///////////  pyramid layer
        //AT.dx_ = i;
        //AT.dy_ = j;
        //AT.Transform(apar,Cov, offset,dstcov);



        if (fabs(float(i+j))==hh+hw) {
          cout<<apar<<" with cov "<<Cov<<" maps "<<i<<";"
              <<j<<" with cov "<<dstcov<<endl;
        }
        // d==0.5   =>    use layer 0
        double layer = log2(sqrt(dstcov.Trace()));
        cout<<fixed<<setprecision(2)<<setw(6)<< layer <<" ";
        if (layer<=0.0) layer = basesmooth;
        else layer += basesmooth;
        if (layer>maxLayer) layer=maxLayer;

        const double scale = 1.0/exp2(rint(layer));
        ///////////  pyramid layer

        mi = x + Axx * i + Axy * j;
        mj = y + Ayx * i + Ayy * j;


        if (_pim2[int(rint(layer))]->IsInROI(scale*mi, scale*mj) &&
            (this->_bl1[hh+j][hw+i]>INVALID_PIXEL)) {

          _pim2.GetTrilinearImageValue(mi, mj, layer, *pbl2);
          mean2_ += *pbl2;
          BIASASSERT(!BIAS_ISINF(*pbl2));
          *imgdiff = *pbl1++ - *pbl2++;
        } else {
          pbl1++;
          *pbl2++  = INVALID_PIXEL;
          *imgdiff = 0;
        }
        //cout<<setprecision(2)<<setw(5)<<*imgdiff<<" ";

        imgdiff++;
      }
      cout<<endl;
    }

    if (_AffineBrightnessInvariance) {
      pbl2 = _bl2.GetData();
      const KLT_TYPE thearea = (2*hh+1)*(2*hw+1);
      mean2_ /= thearea;
      dev2_ = 0;
      double diff;
      for (j = -hh ; j <= hh ; j++)
        for (i = -hw ; i <= hw ; i++) {
          diff = (*pbl2++ -= mean2_);
          dev2_ += diff * diff;
        }
      if (fabs(dev2_)>1e-10) dev2_ = sqrt(thearea / dev2_);
      else dev2_ = 1.0;
      imgdiff = imgdifforig;
      pbl1 = _bl1.GetData();
      pbl2 = _bl2.GetData();
      for (j = -hh ; j <= hh ; j++) {
        for (i = -hw ; i <= hw ; i++) {
          *imgdiff++ =(*pbl1++) - (*pbl2++ *= dev2_);
        }
      }
    }
  }

  register int i, j;
  KLT_TYPE mi, mj;
  if (_AffineBrightnessInvariance) {
    BIASWARN("no smoothing yet!");
    BIASABORT;
    for (j = -hh ; j <= hh ; j++) {
      for (i = -hw ; i <= hw ; i++)  {
        mi = Axx * i + Axy * j;
        mj = Ayx * i + Ayy * j;

        // normalize by precomputed dev2_
        *out_gradx++ = _gradim2x->FastBilinearInterpolationGrey(x+mi,
                                                                y+mj)*dev2_;
        *out_grady++ = _gradim2y->FastBilinearInterpolationGrey(x+mi,
                                                                y+mj)*dev2_;
      }
    }
  } else {
    cout<<"Image 2 accesses: "<<endl;
    for (j = -hh ; j <= hh ; j++) {
      for (i = -hw ; i <= hw ; i++)  {

        ///////////  pyramid layer
        AT.dx_ = i;
        AT.dy_ = j;
        AT.Transform(apar,Cov, offset,dstcov);
        // d==0.5   =>    use layer 0
        double layer = log2(sqrt(dstcov.Trace()))+basesmooth;
        //cout<<setprecision(2)<<setw(4)<<"["<<layer<<":] ";

        if (layer<=0.0) layer = 0;
        if (layer>maxLayer) layer=maxLayer;

        const double scale = 1.0/exp2(layer);
        ///////////  pyramid layer

        mi = x + Axx * i + Axy * j;
        mj = y + Ayx * i + Ayy * j;

        if (_pgradim2x[int(rint(layer))]->IsInROI(scale*mi, scale*mj) &&
            (this->_bl1[hh+j][hw+i]>INVALID_PIXEL)) {
          double tmp;
          _pgradim2x.GetTrilinearImageValue(mi,mj, layer, tmp);
          *out_gradx = scale * tmp;
          _pgradim2y.GetTrilinearImageValue(mi,mj, layer, tmp);
          *out_grady = scale * tmp;
        } else {
          *out_gradx = 0;
          *out_grady = 0;
        }
        //cout<<setprecision(2)<<setw(4)<< *out_gradx <<" "<< *out_grady<<" ";
        out_gradx++;
        out_grady++;
      }
      cout<<endl;
    }
  }

}
}//end namespace BIAS

/**************** end affine part  ********************************/


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

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