vpTemplateTrackerMIInverseCompositional.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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:
 * Example of template tracking.
 *
 * Authors:
 * Amaury Dame
 * Aurelien Yol
 * Fabien Spindler
 *
 *****************************************************************************/
#include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
#include <visp3/core/vpTrackingException.h>

#include <memory>

vpTemplateTrackerMIInverseCompositional::vpTemplateTrackerMIInverseCompositional(vpTemplateTrackerWarp *_warp)
  : vpTemplateTrackerMI(_warp), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
    evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(1e-20), p_prec(), G_prec(), KQuasiNewton() //, useAYOptim(false)
{
  useInverse=true;
}

void vpTemplateTrackerMIInverseCompositional::initTemplateRefBspline(unsigned int ptIndex, double &et) //AY : Optim
{
  ptTemplateSupp[ptIndex].BtInit=new double[(1 + nbParam + nbParam*nbParam)*(unsigned int)bspline];

  unsigned int index = 0;
  int endIndex = 1;

  double (*ptBspFct)(double);
  double (*ptdBspFct)(double);
  double (*ptd2BspFct)(double);
  if(bspline == 3){
    if(et>0.5){et=et-1;}
    ptBspFct = &vpTemplateTrackerMIBSpline::Bspline3;
    ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline3;
    ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline3;
  }
  else{
    ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
    ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
    ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
    endIndex = 2;
  }

  for(int it=-1; it<=endIndex; it++)
  {
    ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)((double)(-it)+et);

    for(unsigned int ip=0;ip<nbParam;++ip)
    {
      ptTemplateSupp[ptIndex].BtInit[index++] = (*ptdBspFct)((double)(-it)+et) * ptTemplate[ptIndex].dW[ip] * (-1.0);
      for(unsigned int ip2=0;ip2<nbParam;++ip2)
      {
        ptTemplateSupp[ptIndex].BtInit[index++] = (*ptd2BspFct)((double)(-it)+et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2];
      }
    }
  }
}

void vpTemplateTrackerMIInverseCompositional::initCompInverse(const vpImage<unsigned char> &I)
{
  ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];

  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);

  if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
  {
    vpImageFilter::getGradX(dIx, d2Ix,fgdG,taillef);
    vpImageFilter::getGradY(dIx, d2Ixy,fgdG,taillef);
    vpImageFilter::getGradY(dIy, d2Iy,fgdG,taillef);
  }

  Warp->computeCoeff(p);
  for(unsigned int point=0;point<templateSize;point++)
  {
    int i=ptTemplate[point].y;
    int j=ptTemplate[point].x;

    X1[0]=j;X1[1]=i;
    
    Warp->computeDenom(X1,p);
    ptTemplate[point].dW=new double[nbParam];

    double dx=ptTemplate[point].dx*(Nc-1)/255.;
    double dy=ptTemplate[point].dy*(Nc-1)/255.;

    Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
    double Tij=ptTemplate[point].val;
    int ct=(int)((Tij*(Nc-1))/255.);
    double et=(Tij*(Nc-1))/255.-ct;

    ptTemplateSupp[point].et=et;
    ptTemplateSupp[point].ct=ct;

    // ###### AY Optim
    //        if(useAYOptim)
    //            if(ApproxHessian != HESSIAN_NONSECOND /*&& hessianComputation != vpTemplateTrackerMI::USE_HESSIEN_DESIRE*/)
    //                initTemplateRefBspline(point, et);
    // ###################
  }
  CompoInitialised=true;

}
void vpTemplateTrackerMIInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
  initCompInverse(I);

  //double erreur=0;
  int Nbpoint=0;

  //double Tij;
  double IW;
  int cr,ct;
  double er,et;

  Nbpoint=0;
  //erreur=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);

  zeroProbabilities();
  Warp->computeCoeff(p);

  // AY : Optim
  //    unsigned int totParam = (bspline * bspline)*(1+nbParam+nbParam*nbParam);
  //    unsigned int size = (1 + nbParam + nbParam*nbParam)*bspline;
  //    double *ptb;

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

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

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

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

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

      ct=ptTemplateSupp[point].ct;
      et=ptTemplateSupp[point].et;
      cr=(int)((IW*(Nc-1))/255.);
      er=((double)IW*(Nc-1))/255.-cr;

      //calcul de l'erreur
      //erreur+=(Tij-IW)*(Tij-IW);

      if( ApproxHessian==HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect) )
      {
        vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
      }
      else if ((ApproxHessian==HESSIAN_0||ApproxHessian==HESSIAN_NEW) && (ptTemplateSelect[point] || !useTemplateSelect))
      {
        if(bspline==3){
          vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
          //                    {
          //                        if(et>0.5){ct++;}
          //                        if(er>0.5){cr++;}
          //                        int index = (cr*Nc+ct)*totParam;
          //                        double *ptb = &PrtTout[index];
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
          //                    }
        }
        else{
          vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);

          //                    {
          //                        // ################### AY : Optim
          //                        unsigned int index = (cr*Nc+ct)*totParam;
          //                        ptb = &PrtTout[index];
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline4(ptb, er, ptTemplateSupp[point].BtInit, size);
          //                        // ###################
          //                    }
        }
      }
      else if (ptTemplateSelect[point] || !useTemplateSelect)
        vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc,nbParam, bspline);
    }
  }

  double MI;
  computeProba(Nbpoint);
  computeMI(MI);
  computeHessien(Hdesire);

  lambda=lambdaDep;

  vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);

  HLMdesireInverse=HLMdesire.inverseByLU();
  KQuasiNewton=HLMdesireInverse;
}

void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
  if(!CompoInitialised)
    std::cout<<"Compositionnal tracking no initialised\nUse InitCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
  dW=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);

  lambda=lambdaDep;
  double MI=0,MIprec=-1000;

  vpColVector p_avant_estimation;p_avant_estimation=p;
  MI_preEstimation=-getCost(I,p);
  NMI_preEstimation=-getNormalizedCost(I,p);

  //    std::cout << "MI avant: " << MI_preEstimation << std::endl;
  //    std::cout << "NMI avant: " << NMI_preEstimation << std::endl;

  initPosEvalRMS(p);

  vpColVector dpinv(nbParam);
  double alpha=2.;

  unsigned int iteration=0;

  //unsigned int bspline_ = (unsigned int) bspline;
  //unsigned int totParam = (bspline_ * bspline_)*(1+nbParam+nbParam*nbParam);

  vpMatrix Hnorm(nbParam,nbParam);

  do
  {
    int Nbpoint=0;
    MIprec=MI;
    MI=0;

    zeroProbabilities();

    Warp->computeCoeff(p);

    {
      for(int point=0;point<(int)templateSize;point++)
      {
        vpColVector x1(2),x2(2);
        double i2,j2;

        x1[0]=(double)ptTemplate[point].x;
        x1[1]=(double)ptTemplate[point].y;

        Warp->computeDenom(x1,p); // A modif pour parallelisation mais ne pose pas de pb avec warp utilises dans DECSA
        Warp->warpX(x1,x2,p);

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

        if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
        {
          //if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() == I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() && (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
          {
            Nbpoint++;
            double IW;
            if(!blur)
              IW=(double)I.getValue(i2,j2);
            else
              IW=BI.getValue(i2,j2);

            int ct=ptTemplateSupp[point].ct;
            double et=ptTemplateSupp[point].et;
            double tmp = IW*(((double)Nc)-1.f)/255.f;
            int cr=(int)tmp;
            double er=tmp-(double)cr;

            if( (ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && (ptTemplateSelect[point] || !useTemplateSelect) )
            {
              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
            }
            else if (ptTemplateSelect[point] || !useTemplateSelect)
            {
              if(bspline==3){
                vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
              }
              else{
                vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
              }
            }
            else{
              vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
            }
          }

        }
      }
    }

    if(Nbpoint==0)
    {
      diverge=true;
      MI=0;
      deletePosEvalRMS();
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));

    }
    else
    {
      //            computeProba(Nbpoint);

      unsigned int indd, indd2;
      indd = indd2 = 0;
      unsigned int Ncb_ = (unsigned int)Ncb;
      for(unsigned int i=0;i<Ncb_*Ncb_;i++){
        Prt[i]=Prt[i]/Nbpoint;
        for(unsigned int j=0;j<nbParam;j++){
          dPrt[indd]=dPrt[indd]/Nbpoint;
          indd++;
          for(unsigned int k=0;k<nbParam;k++){
            d2Prt[indd2]=d2Prt[indd2]/Nbpoint;
            indd2++;
          }
        }
      }

      computeMI(MI);

      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE){
        computeHessienNormalized(Hnorm);
        computeHessien(H);
      }
      computeGradient();

      vpMatrix::computeHLM(H,lambda,HLM);

      try
      {
        switch(hessianComputation)
        {
        case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
          dp=gain*HLMdesireInverse*G;
          break;
        case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
          if(HLM.cond()>HLMdesire.cond())
            dp=gain*HLMdesireInverse*G;
          else
            dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        default:
          dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        }
      }
      catch(vpException &e)
      {
        //std::cerr<<"probleme inversion"<<std::endl;
        throw(e);
      }
    }

    switch(minimizationMethod)
    {
    case vpTemplateTrackerMIInverseCompositional::USE_LMA:
    {
      vpColVector dp_test_LMA(nbParam);
      vpColVector dpinv_test_LMA(nbParam);
      vpColVector p_test_LMA(nbParam);
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp_test_LMA=-100000.1*dp;
      else
        dp_test_LMA=1.*dp;
      Warp->getParamInverse(dp_test_LMA,dpinv_test_LMA);
      Warp->pRondp(p,dpinv_test_LMA,p_test_LMA);

      MI=-getCost(I,p);
      double MI_LMA=-getCost(I,p_test_LMA);
      if(MI_LMA>MI)
      {
        dp=dp_test_LMA;
        lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
      }
      else
      {
        dp=0;
        lambda=(lambda*10.<1e6)?1e6:lambda*10.;
      }
    }
      break;
    case vpTemplateTrackerMIInverseCompositional::USE_GRADIENT:
      dp=-gain*0.3*G*20;
      break;

    case vpTemplateTrackerMIInverseCompositional::USE_QUASINEWTON:
    {
      if(iterationGlobale!=0)
      {
        vpColVector s_quasi=p-p_prec;
        vpColVector y_quasi=G-G_prec;
        double s_scal_y=s_quasi.t()*y_quasi;
        //std::cout<<"mise a jour K"<<std::endl;
        /*if(s_scal_y!=0)//BFGS
                    KQuasiNewton=KQuasiNewton+0.01*(-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y);*/
        //if(s_scal_y!=0)//DFP
        if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())//DFP
        {
          KQuasiNewton=KQuasiNewton+0.0001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
          //std::cout<<"mise a jour K"<<std::endl;
        }
      }
      dp=gain*KQuasiNewton*G;
      //std::cout<<KQuasiNewton<<std::endl<<std::endl;
      p_prec=p;
      G_prec=G;
      //p-=1.01*dp;
    }
      break;

    default:
    {
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-1.*dp;

      break;
    }
    }

    Warp->getParamInverse(dp,dpinv);
    Warp->pRondp(p,dpinv,p);

    iteration++;
    iterationGlobale++;

    computeEvalRMS(p);

    //        std::cout << p.t() << std::endl;
  }
  while( (!diverge) && (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  //while( (!diverge) && (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );

  nbIteration=iteration;

  if(diverge)
  {
    if(computeCovariance){
      covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
      covarianceMatrix = -1;
      MI_postEstimation = -1;
      NMI_postEstimation = -1;
    }
    deletePosEvalRMS();

    //        throw(vpTrackingException(vpTrackingException::badValue, "Tracking failed")) ;
  }
  else
  {
    MI_postEstimation=-getCost(I,p);
    NMI_postEstimation=-getNormalizedCost(I,p);
    //        std::cout << "MI apres: " << MI_postEstimation << std::endl;
    //        std::cout << "NMI apres: " << NMI_postEstimation << std::endl;
    if(MI_preEstimation>MI_postEstimation)
    {
      p=p_avant_estimation;
      MI_postEstimation = MI_preEstimation;
      NMI_postEstimation = NMI_preEstimation;
      covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
      covarianceMatrix = -1;
    }

    deletePosEvalRMS();

    if(computeCovariance){
      try{
        covarianceMatrix = (-H).inverseByLU();
        //            covarianceMatrix = (-Hnorm).inverseByLU();
      }
      catch(...){
        covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
        covarianceMatrix = -1;
        MI_postEstimation = -1;
        NMI_postEstimation = -1;
        deletePosEvalRMS();
      }
    }
  }
}

void vpTemplateTrackerMIInverseCompositional::initPosEvalRMS(const vpColVector &pw)
{
  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
  x_pos=new double[nb_corners];
  y_pos=new double[nb_corners];

  Warp->computeCoeff(pw);
  vpTemplateTrackerTriangle triangle;

  for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
  {
    zoneTracked->getTriangle(i, triangle);
    for (unsigned int j=0; j<3; j++) {
      triangle.getCorner(j, X1[0], X1[1]);

      Warp->computeDenom(X1,pw);
      Warp->warpX(X1,X2,p);
      x_pos[i*3+j]=X2[0];
      y_pos[i*3+j]=X2[1];
    }
  }
}

void vpTemplateTrackerMIInverseCompositional::computeEvalRMS(const vpColVector &pw)
{
  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;

  Warp->computeCoeff(pw);
  evolRMS=0;
  vpTemplateTrackerTriangle triangle;

  for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
  {
    zoneTracked->getTriangle(i, triangle);
    for (unsigned int j=0; j<3; j++) {
      triangle.getCorner(j, X1[0], X1[1]);

      Warp->computeDenom(X1,pw);
      Warp->warpX(X1,X2,pw);
      evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
      x_pos[i*3+j]=X2[0];
      y_pos[i*3+j]=X2[1];
    }
  }
  evolRMS=evolRMS/nb_corners;
}

void vpTemplateTrackerMIInverseCompositional::deletePosEvalRMS()
{
  delete[] x_pos;
  delete[] y_pos;
}