TrackerBaseHomography.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 "TrackerBaseHomography.hh"
#include "Geometry/HMatrix.hh"
#include "MathAlgo/Lapack.hh"
#include "MathAlgo/SVD.hh"

using namespace BIAS;
using namespace std;


template <class StorageType>
int TrackerBaseHomography<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)
{  
  HMatrix H;
  for (unsigned int i=0; i<2; i++) {
    for (unsigned int j=0; j<2; j++) {    
      H[i][j] = AffinePred[i][j];
    }
    H[i][2] = p2[i]-p1[i];
    H[2][i]=0;
  }
  H[2][2]=1;

  const int hh =  _WinSize/2;

  HomgPoint2D x1;
  x1[2] = 1;

  Matrix<double> Jh(2,3);
  Jh[0][1] = Jh[1][0] = 0;

  Matrix<double> x1mat(1,3);
  Matrix<double> I3(3,3);
  I3.SetIdentity();

  Matrix<double> grad(1,2);
  
  BilinearRegion1_(p1[0], p1[1], hh);

  Matrix<double> N(8,8);
  Vector<double> y(8);
  Vector<double> a(8);

  for (iter=0; iter<_MaxIterations; iter++) {
    N.SetZero();
    y.SetZero();

    double Omega=0;
    for (int i = -hh ; i <= hh ; i++) {
      x1[0] = i;
      for (int j = -hh ; j <= hh ; j++)  {
        x1[1] = j;

        x1mat[0][0] = x1[0];
        x1mat[0][1] = x1[1];
        x1mat[0][2] = x1[2];

        HomgPoint2D x2=H*x1;
        Jh[0][0] = Jh[1][1] = 1/x2[2];
        Jh[0][2] = -x2[0]/(x2[2]*x2[2]);
        Jh[1][2] = -x2[1]/(x2[2]*x2[2]);
        
        double ex2_x = x2[0]/x2[2] + p1[0];
        double ex2_y = x2[1]/x2[2] + p1[1];
        grad[0][0] = _gradim2x->FastBilinearInterpolationGrey(ex2_x, ex2_y);
        grad[0][1] = _gradim2y->FastBilinearInterpolationGrey(ex2_x, ex2_y);

        Matrix<double> x1I3;
        x1mat.Kronecker(I3,x1I3);
        Matrix<double> dh=grad*Jh*x1I3;

        for (unsigned int t=0; t<8; t++)
          a[t] = dh[0][t];

        BilinearRegion2_(ex2_x, ex2_y, 0);

        double l = _bl1[j+hh][i+hh] - _bl2[0][0];
        Omega += l*l;

        N += a.OuterProduct(a);
        y += a*l;        
      }
    }

    Vector<double> update = Lapack_LU_linear_solve(N, y);
    
    // line search
    double alpha=1;

    double Omega_new;
    do {
      for (unsigned i=0,k=0; i<3; i++) {
        for (unsigned j=0; (j<3) && (k<8); j++,k++) {
          H[j][i] += alpha*update[k];
        }
      }
   
      Omega_new=0;
      for (int i = -hh ; i <= hh ; i++) {
        x1[0] = i;
        for (int j = -hh ; j <= hh ; j++)  {
          x1[1] = j;
          HomgPoint2D x2=H*x1;
          double ex2_x = x2[0]/x2[2] + p1[0];
          double ex2_y = x2[1]/x2[2] + p1[1];
          BilinearRegion2_(ex2_x, ex2_y, 0);
          double l = _bl1[j+hh][i+hh] - _bl2[0][0];
          Omega_new += l*l;
        }
      }
      if (Omega_new>Omega) {
        for (unsigned i=0,k=0; i<3; i++) {
          for (unsigned j=0; (j<3) && (k<8); j++,k++) {
            H[j][i] -= alpha*update[k];
          }
        }
        alpha /= 2;
      }
    } while ((alpha>1e-4) && (Omega_new>Omega));
    
    //    cout << "alpha = " << alpha << endl;

    double relativeUpdate = alpha*alpha*update.ScalarProduct(N*update);

    //    cout << "Relative update = " << relativeUpdate << endl;    

    if (relativeUpdate<1e-4)
      break;     
  }

//   SVD svdN(N);
//   cout << "Singular values = " << svdN.GetS() << endl;


  for (unsigned int i=0; i<2; i++) {
    result[i] = p1[i] + H[i][2];
    for (unsigned int j=0; j<2; j++) {
      AffineResult[i][j] = H[i][j] - H[i][2]*H[2][j];
    }
  }


  //  cout << "H = " << H << endl;

  return 0;
}

template <class StorageType>
void TrackerBaseHomography<StorageType>::
EvaluateResult_(KLT_TYPE& mad, KLT_TYPE& msd, Matrix<KLT_TYPE>& CovMatrix) {
}


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

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