vpTemplateTrackerZNCCForwardAdditional.cpp

/****************************************************************************
 *
 * $Id: vpTemplateTrackerZNCCForwardAdditional.cpp 4574 2014-01-09 08:48:51Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Template tracker.
 *
 * Authors:
 * Amaury Dame
 * Aurelien Yol
 * Fabien Spindler
 *
 *****************************************************************************/
#include <visp/vpTemplateTrackerZNCCForwardAdditional.h>
#include <visp/vpImageFilter.h>

vpTemplateTrackerZNCCForwardAdditional::vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp):vpTemplateTrackerZNCC(warp)
{
  useCompositionnal=false;
}

void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
{
  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);

  vpImage<double> dIxx,dIxy,dIyx,dIyy;
  vpImageFilter::getGradX(dIx, dIxx, fgdG,taillef);
  vpImageFilter::getGradY(dIx, dIxy, fgdG,taillef);

  vpImageFilter::getGradX(dIy, dIyx, fgdG,taillef);
  vpImageFilter::getGradY(dIy, dIyy, fgdG,taillef);

  Warp->computeCoeff(p);
  double IW,dIWx,dIWy;
  double Tij;
  int i,j;
  double i2,j2;
  int Nbpoint=0;

  double moyTij=0;
  double moyIW=0;
  double denom=0;
  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;
    X2[0]=j;X2[1]=i;

    Warp->computeDenom(X1,p);

    j2=X2[0];i2=X2[1];

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
    {
      Tij=ptTemplate[point].val;

      if(!blur)
        IW=I.getValue(i2,j2);
      else
        IW=BI.getValue(i2,j2);

      Nbpoint++;
      moyTij+=Tij;
      moyIW+=IW;
    }
  }
  moyTij=moyTij/Nbpoint;
  moyIW=moyIW/Nbpoint;
  Hdesire=0;
  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;
    X2[0]=j;X2[1]=i;

    Warp->computeDenom(X1,p);

    j2=X2[0];i2=X2[1];

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
    {
      Tij=ptTemplate[point].val;

      if(!blur)
        IW=I.getValue(i2,j2);
      else
        IW=BI.getValue(i2,j2);

      dIWx=dIx.getValue(i2,j2);
      dIWy=dIy.getValue(i2,j2);
      //Calcul du Hessien
      Warp->dWarp(X1,X2,p,dW);
      double *tempt=new double[nbParam];
      for(unsigned int it=0;it<nbParam;it++)
        tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;


      double prod=(Tij-moyTij);

      double d_Ixx=dIxx.getValue(i2,j2);
      double d_Iyy=dIyy.getValue(i2,j2);
      double d_Ixy=dIxy.getValue(i2,j2);

      for(unsigned int it=0;it<nbParam;it++)
        for(unsigned int jt=0;jt<nbParam;jt++)
          Hdesire[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
              +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
      /*Hdesire[0][0]+=prod*d_Ixx;
      Hdesire[1][0]+=prod*d_Ixy;
      Hdesire[0][1]+=prod*d_Ixy;
      Hdesire[1][1]+=prod*d_Iyy;*/

      denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
      delete[] tempt;
    }


  }

  Hdesire=Hdesire/sqrt(denom);
  vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire);
  HLMdesireInverse=HLMdesire.inverseByLU();
  //std::cout<<"Hdesire = "<<Hdesire<<std::endl;

}

void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
  double erreur=0;
  int Nbpoint=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);

  /*vpImage<double> dIxx,dIxy,dIyx,dIyy;
  getGradX(dIx, dIxx, fgdG,taillef);
  getGradY(dIx, dIxy, fgdG,taillef);

  getGradX(dIy, dIyx, fgdG,taillef);
  getGradY(dIy, dIyy, fgdG,taillef);*/

  dW=0;

  //double lambda=lambdaDep;
  double IW,dIWx,dIWy;
  double Tij;
  unsigned int iteration=0;
  int i,j;
  double i2,j2;
  double alpha=2.;
  do
  {
    Nbpoint=0;
    erreur=0;
    G=0;
    H=0 ;
    Warp->computeCoeff(p);
    double moyTij=0;
    double moyIW=0;
    double denom=0;
    for(unsigned int point=0;point<templateSize;point++)
    {
      i=ptTemplate[point].y;
      j=ptTemplate[point].x;
      X1[0]=j;X1[1]=i;

      Warp->computeDenom(X1,p);
      Warp->warpX(X1,X2,p);

      j2=X2[0];i2=X2[1];
      if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
      {
        Tij=ptTemplate[point].val;

        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);

        Nbpoint++;
        moyTij+=Tij;
        moyIW+=IW;
      }
    }

    if(! Nbpoint) {
      throw(vpException(vpException::divideByZeroError,
            "Cannot track the template: no point")) ;
    }

    moyTij=moyTij/Nbpoint;
    moyIW=moyIW/Nbpoint;
    //vpMatrix d2Wx(nbParam,nbParam);
    //vpMatrix d2Wy(nbParam,nbParam);
    for(unsigned int point=0;point<templateSize;point++)
    {
      i=ptTemplate[point].y;
      j=ptTemplate[point].x;
      X1[0]=j;X1[1]=i;

      Warp->computeDenom(X1,p);
      Warp->warpX(X1,X2,p);

      j2=X2[0];i2=X2[1];
      if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
      {
        Tij=ptTemplate[point].val;

        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);

        dIWx=dIx.getValue(i2,j2);
        dIWy=dIy.getValue(i2,j2);
        //Calcul du Hessien
        Warp->dWarp(X1,X2,p,dW);
        double *tempt=new double[nbParam];
        for(unsigned int it=0;it<nbParam;it++)
          tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;


        double prod=(Tij-moyTij);
        for(unsigned int it=0;it<nbParam;it++)
          G[it]+=prod*tempt[it];

        /*  Warp->d2Warp(X1,X2,p,d2Wx,d2Wy);
        for(int it=0;it<nbParam;it++)
          for(int jt=0;jt<nbParam;jt++)
            H[it][jt]+=prod*(d2Wx[it][jt]*dIWx+d2Wx[it][jt]*dIWy);*/
        /*double d_Ixx=dIxx.getValue(i2,j2);
        double d_Iyy=dIyy.getValue(i2,j2);
        double d_Ixy=dIxy.getValue(i2,j2);

        for(int it=0;it<nbParam;it++)
          for(int jt=0;jt<nbParam;jt++)
            H[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
                +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));*/
        /*H[0][0]+=prod*d_Ixx;
        H[1][0]+=prod*d_Ixy;
        H[0][1]+=prod*d_Ixy;
        H[1][1]+=prod*d_Iyy;*/

        double er=(Tij-IW);
        erreur+=(er*er);
        denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
        delete[] tempt;
      }


    }
    /*std::cout<<"G="<<G<<std::endl;
    std::cout<<"H="<<H<<std::endl;
    std::cout<<" denom="<<denom<<std::endl;*/
    G=G/sqrt(denom);
    //std::cout<<G<<std::endl;
    H=H/sqrt(denom);

    if(Nbpoint==0)std::cout<<"plus de point dans template suivi"<<std::endl;

    try
    {
      //vpMatrix::computeHLM(H,lambda,HLM);
      //dp=1.*HLM.inverseByLU()*G;
      dp=1.*HLMdesireInverse*G;
    }
    catch(...)
    {
      std::cout<<"probleme inversion"<<std::endl;
      break;
    }

    dp=gain*dp;
    if(useBrent)
    {
      alpha=2.;
      computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
      dp=alpha*dp;
    }
    p-=dp;
    iteration++;
  }
  while( /*( erreur_prec-erreur<50) && */(iteration < iterationMax));

  //std::cout<<"erreur "<<erreur<<std::endl;
  nbIteration=iteration;
}