コード例 #1
0
void vpTemplateTrackerMIInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
  initCompInverse(I);

  double erreur=0;
  int Nbpoint=0;

  double i2,j2;
  double Tij;
  double IW;
  int cr,ct;
  double er,et;

  int i,j;

  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++)
  {
    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))
    {
      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);

  double lambda=lambdaDep;

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

  HLMdesireInverse=HLMdesire.inverseByLU();
  KQuasiNewton=HLMdesireInverse;
}
コード例 #2
0
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);

  int Nbpoint=0;


  double 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);

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
  int Ncb_ = (int)Ncb;
  int nbParam_ = (int)nbParam;
  int sizePrtBis = Ncb_*Ncb_;
  int sizedPrtBis = Ncb_*Ncb_*nbParam_;
  int sized2PrtBis = Ncb_*Ncb_*nbParam_*nbParam_;
#endif

  vpMatrix Hnorm(nbParam,nbParam);

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

#ifndef USE_OPENMP_MI_INVCOMP
    zeroProbabilities();
#endif

    Warp->computeCoeff(p);

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
#pragma omp parallel
#endif
    {
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
      double *Prtbis = new double[sizePrtBis];
      double *dPrtbis = new double[sizedPrtBis];
      double *d2Prtbis = new double[sized2PrtBis];

      unsigned int indd, indd2;
      indd = indd2 = 0;
      for(int i=0;i<Ncb_*Ncb_;i++){
        Prtbis[i]=0.0;
        Prt[i]=0.0;
        for(int j=0;j<nbParam_;j++){
          dPrtbis[indd]=0.0;
          dPrt[indd]=0.0;
          indd++;
          for(int k=0;k<nbParam_;k++){
            d2Prtbis[indd2]=0.0;
            d2Prt[indd2]=0.0;
            indd2++;
          }
        }
      }

#pragma omp barrier
#pragma omp for reduction(+:Nbpoint)
#endif
      for(int point=0;point<(int)templateSize;point++)
      {
        vpColVector x1(2),x2(2);
        double i2,j2;
        double IW;
        int cr,ct;
        double er,et;

        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++;
            if(!blur)
              IW=(double)I.getValue(i2,j2);
            else
              IW=BI.getValue(i2,j2);

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

            if( (ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && (ptTemplateSelect[point] || !useTemplateSelect) )
            {
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prtbis, dPrtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
#else
              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
#endif
            }
            else if (ptTemplateSelect[point] || !useTemplateSelect)
            {
              if(bspline==3){
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
                vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prtbis, dPrtbis, d2Prtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#else
                vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#endif
                //                        {
                //                            // ################### AY : Optim
                //                            if(et>0.5){ct++;}
                //                            if(er>0.5){cr++;}
                //                            int index = (cr*Nc+ct)*totParam;
                //                            double *ptb = &PrtTout[index];
                //                            #endif
                //                            vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
                //                            // ###################
                //                        }
              }
              else{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
                vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prtbis, dPrtbis, d2Prtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#else
                vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#endif
              }
            }
            else{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
              vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prtbis, cr, er, ct, et, Ncb ,nbParam, bspline);
#else
              vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
#endif
            }
          }

        }
      }

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
#pragma omp critical
      {
        unsigned int indd, indd2;
        indd = indd2 = 0;
        for(int i=0;i<Ncb*Ncb;i++){
          Prt[i]+=Prtbis[i]/(double)Nbpoint;
          for(int j=0;j<nbParam_;j++){
            dPrt[indd]+=dPrtbis[indd]/(double)Nbpoint;
            indd++;
            for(int k=0;k<nbParam_;k++){
              d2Prt[indd2]+=d2Prtbis[indd2]/(double)Nbpoint;
              indd2++;
            }
          }
        }

        delete [] Prtbis;
        delete [] dPrtbis;
        delete [] d2Prtbis;
      }

#pragma omp barrier
#endif
    }

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

    }
    else
    {
      //            computeProba(Nbpoint);

#ifndef USE_OPENMP_MI_INVCOMP
      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++;
          }
        }
      }
#endif

      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:
    {
      double s_scal_y;
      if(iterationGlobale!=0)
      {
        vpColVector s_quasi=p-p_prec;
        vpColVector y_quasi=G-G_prec;
        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();
        //            throw(vpMatrixException(vpMatrixException::matrixError, "Covariance not inversible")) ;
      }
    }
  }
}
コード例 #3
0
void vpTemplateTrackerMIESM::initHessienDesired(const vpImage<unsigned char> &I)
{
  initCompInverse();
  std::cout<<"Initialise Hessian at Desired position..."<<std::endl;

  dW=0;

  double erreur=0;
  int Nbpoint=0;

  double i2,j2;
  double Tij;
  double IW,dx,dy;
  //int cr,ct;
  //double er,et;
  int i,j;

  Nbpoint=0;
  erreur=0;

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

  zeroProbabilities();

  Warp->computeCoeff(p);
  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))
    {
      Nbpoint++;
      Tij=ptTemplate[point].val;
      //Tij=Iterateurvecteur->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;

      //            if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
      //                vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
      //            else
      //                vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);

      //            vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,false);
    }
  }


  double MI;
  computeProba(Nbpoint);
  computeMI(MI);
  computeHessien(HdesireInverse);
  //std::cout<<"HdesireInverse"<<std::endl<<HdesireInverse<<std::endl;

  /////////////////////////////////////////////////////////////////////////
  // DIRECT COMPO

  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);
  }

  Nbpoint=0;
  erreur=0;
  zeroProbabilities();

  Warp->computeCoeff(p);
  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())&&(j2<I.getWidth()))
    {
      Nbpoint++;
      Tij=ptTemplate[point].val;
      if(!blur)
        IW=I.getValue(i2,j2);
      else
        IW=BI.getValue(i2,j2);

      dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
      dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;

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

      Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);

      double *tptemp=new double[nbParam];
      for(unsigned int it=0;it<nbParam;it++)
        tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;


      //                vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,
      //                                                         hessianComputation== vpTemplateTrackerMI::USE_HESSIEN_DESIRE);
      //calcul de l'erreur
      erreur+=(Tij-IW)*(Tij-IW);

      //          if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
      //              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam, bspline);
      //          else
      //              vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam, bspline);

      //      vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,false);

      delete[] tptemp;
    }
  }

  computeProba(Nbpoint);
  computeMI(MI);
  computeHessien(HdesireDirect);
  //std::cout<<"HdesireDirect"<<std::endl<<HdesireDirect<<std::endl;

  double lambda=lambdaDep;

  Hdesire=HdesireDirect+HdesireInverse;
  //Hdesire=HdesireDirect;
  //Hdesire=HdesireInverse;

  //std::cout<<"HdesireDirect "<<HdesireDirect<<std::endl;
  //std::cout<<"HdesireInverse "<<HdesireInverse<<std::endl;
  vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
  HLMdesireInverse=HLMdesire.inverseByLU();

  //std::cout<<"\tEnd initialisation..."<<std::endl;

}
コード例 #4
0
void vpTemplateTrackerMIESM::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);
  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)
  {
    getGradX(dIx, d2Ix,fgdG,taillef);
    getGradY(dIx, d2Ixy,fgdG,taillef);
    getGradY(dIy, d2Iy,fgdG,taillef);
  }*/

  double erreur=0;
  int Nbpoint=0;
  int point;

  MI_preEstimation=-getCost(I,p);

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

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

  vpColVector dpinv(nbParam);

  double dx,dy;
  double alpha=2.;

  int i,j;
  unsigned int iteration=0;
  do
  {
    Nbpoint=0;
    //MIprec=MI;
    MI=0;
    erreur=0;

    zeroProbabilities();

    /////////////////////////////////////////////////////////////////////////
    // Inverse
    Warp->computeCoeff(p);
    for(point=0;point<(int)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))
      {
        Nbpoint++;
        Tij=ptTemplate[point].val;
        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);

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

        //                if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) // cas à tester AY
        //                    vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
        //                else
        //                    vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
      }
    }

    if(Nbpoint==0)
    {
      //std::cout<<"plus de point dans template suivi"<<std::endl;
      diverge=true;
      MI=0;
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
    }
    else
    {
      computeProba(Nbpoint);
      computeMI(MI);
      if(hessianComputation!= vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
        computeHessien(HInverse);
      computeGradient();
      GInverse=G;

      /////////////////////////////////////////////////////////////////////////
      // DIRECT

      Nbpoint=0;
      //MIprec=MI;
      MI=0;
      erreur=0;

      zeroProbabilities();

      Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
      int nthreads = omp_get_num_procs() ;
      //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
      omp_set_num_threads(nthreads);
#pragma omp parallel for private(point, Tij,IW,i,j,i2,j2,/*cr,ct,er,et,*/dx,dy) default(shared)
#endif
      for(point=0;point<(int)templateSize;point++)
      {
        i=ptTemplate[point].y;
        j=ptTemplate[point].x;
        X1[0]=j;X1[1]=i;
        Warp->computeDenom(X1,p);
        Warp->warpX(i,j,i2,j2,p);
        X2[0]=j2;X2[1]=i2;

        //Warp->computeDenom(X1,p);
        if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
        {
          Nbpoint++;
          Tij=ptTemplate[point].val;
          //Tij=Iterateurvecteur->val;
          if(!blur)
            IW=I.getValue(i2,j2);
          else
            IW=BI.getValue(i2,j2);

          dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
          dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;

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

          Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);

          double *tptemp=new double[nbParam];
          for(unsigned int it=0;it<nbParam;it++)
            tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;


          //calcul de l'erreur
          erreur+=(Tij-IW)*(Tij-IW);
          //                    if(bspline==3)
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
          //                    else
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
          //					vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,
          //                               hessianComputation==vpHessienType::USE_HESSIEN_DESIRE);

          delete[] tptemp;

        }
      }

      computeProba(Nbpoint);
      computeMI(MI);
      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
        computeHessien(HDirect);
      computeGradient();
      GDirect=G;

      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
      {
        H=HDirect+HInverse;
        vpMatrix::computeHLM(H,lambda,HLM);
      }
      G=GDirect-GInverse;
      //G=GDirect;
      //G=-GInverse;

      try
      {
        if(minimizationMethod==vpTemplateTrackerMIESM::USE_GRADIENT)
          dp=-gain*0.3*G;
        else
        {
          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.3*HLM.inverseByLU()*G;
            break;
          default:
            dp=gain*0.3*HLM.inverseByLU()*G;
            break;
          }
        }
      }
      catch(vpException &e)
      {
        //std::cerr<<"probleme inversion"<<std::endl;
        throw(e);
      }

      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-1.*dp;

      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      p+=dp;

      iteration++;
    }
  }
  while( /*(MI!=MIprec) &&*/(iteration< iterationMax));

  MI_postEstimation=-getCost(I,p);
  if(MI_preEstimation>MI_postEstimation)
  {
    MI_postEstimation = -1;
  }

  nbIteration=iteration;
}
コード例 #5
0
ファイル: genepair.c プロジェクト: bschiffthaler/genepair
int main(int argc, char *argv[]){

  printf("Reading the options; %d\n",argc);
  int ntype = atoi(argv[1]);
  if (ntype == 1){//compute MIs
    char *nmicrf = argv[2];
    char *nmif = argv[3];
    size_t ngenes = atoi(argv[4]);
    int nexp = atoi(argv[5]);
    int gene1 = atoi(argv[6]);
    int gene2 = atoi(argv[7]);
    int numbins = atoi(argv[8]);
    int splineord = atoi(argv[9]);
    computeMI(ngenes, nexp, numbins, splineord, gene1, gene2, nmif, nmicrf);
  }

  if (ntype == 2){//compute clr
    char *nvalf = argv[2];
    size_t ngenes = atoi(argv[3]);
    char* nclrf = argv[4];
    int inputftype = atoi(argv[5]);
    computeCLR(nvalf,ngenes,nclrf,inputftype);
  }

	if (ntype == 3){//compute MI line
		char *nmilinef = argv[2];
		char *nmif = argv[3];
    size_t ngenes = atoi(argv[4]);
    int nexp = atoi(argv[5]);
    int numbins = atoi(argv[6]);
    int splineord = atoi(argv[7]);
		int linei = atoi(argv[8]);
		getMIline(nmilinef, nmif, ngenes, nexp, numbins, splineord, linei);
	}

  if (ntype == 4){//compute MIs in paralel
    char *nmicrf = argv[2];
    char *nmif = argv[3];
    size_t ngenes = atoi(argv[4]);
    int nexp = atoi(argv[5]);
    int numbins = atoi(argv[6]);
    int splineord = atoi(argv[7]);
    int geneR1 = atoi(argv[8]);
    int geneR2 = atoi(argv[9]);
    int geneC1 = atoi(argv[10]);
    int geneC2 = atoi(argv[11]);
    computeMIparalel(ngenes, nexp, numbins, splineord, nmif, nmicrf, geneR1, geneR2, geneC1, geneC2);
  }

//void getMIrandomness(const char* nmif, int npairs, int nexp, double limit, int numbins, int splineord){
  if (ntype == 5){//compute MIs for a number of random pairs between two numbers
    char *nmif = argv[2];
    int npairs = atoi(argv[3]);
    int nexp = atoi(argv[4]);
    double limit = (double) atof(argv[5]);
    int numbins = atoi(argv[6]);
    int splineord = atoi(argv[7]);
    getMIrandomness(nmif, npairs, nexp, limit, numbins, splineord);
  }

  if (ntype == 6){//compute MIs
    char *nmicrf = argv[2];
    char *nmif = argv[3];
    size_t ngenes = atoi(argv[4]);
    int nexp = atoi(argv[5]);
    int gene1 = atoi(argv[6]);
    int gene2 = atoi(argv[7]);
    int numbins = atoi(argv[8]);
    int splineord = atoi(argv[9]);
    computeMIundef(ngenes, nexp, numbins, splineord, gene1, gene2, nmif, nmicrf);
  }

//printf("test X reading: %f %e\n",X[0],X[5*nexp+2]);//X[10*nexp+0]);
//float m = getMIpair(X, ngenes, nexp, ngene1, ngene2, numbins, splineord);
//printf("mi pair: %f\n",m);
//getMIline(X, ngenes, nexp, numbins, splineord, ngene1);
//float z = getCLRpair(X, ngenes, nexp, ngene1, ngene2, numbins, splineord);
//printf("zscore: %f\n",z);
//getMIfile(X, ngenes, nexp, numbins, splineord);
//printf("CLR launched\n");
//computeCLR(ngenes);
//free(X);


}
コード例 #6
0
void vpTemplateTrackerMIForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
{
  //std::cout<<"Initialise Hessian at Desired position..."<<std::endl;

  dW=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);

  double i2,j2;
  double Tij;
  double IW,dx,dy;
  int cr,ct;
  double er,et;

  int i,j;

  Nbpoint=0;

  zeroProbabilities();
  Warp->computeCoeff(p);
  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);
    Warp->warpX(X1,X2,p);

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

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
    {
      Nbpoint++;
      Tij=ptTemplate[point].val;
      if(!blur)
        IW=I.getValue(i2,j2);
      else
        IW=BI.getValue(i2,j2);

      dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
      dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;

      ct=(int)((IW*(Nc-1))/255.);
      cr=(int)((Tij*(Nc-1))/255.);
      et=(IW*(Nc-1))/255.-ct;
      er=((double)Tij*(Nc-1))/255.-cr;
      //std::cout<<"test"<<std::endl;
      Warp->dWarp(X1,X2,p,dW);

      double *tptemp=new double[nbParam];
      for(unsigned int it=0;it<nbParam;it++)
        tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;

      if(ApproxHessian==HESSIAN_NONSECOND)
        vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
      else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
        vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);

      delete[] tptemp;
    }
  }

  if(Nbpoint>0)
  {
    double MI;
    computeProba(Nbpoint);
    computeMI(MI);
    computeHessien(Hdesire);


    //	double conditionnement=GetConditionnement(Hdesire);
    //	std::cout<<"conditionnement : "<<conditionnement<<std::endl;
    vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
    try
    {
      HLMdesireInverse=HLMdesire.inverseByLU();
    }
    catch(vpException &e)
    {
      //std::cerr<<"probleme inversion"<<std::endl;
      throw(e);
    }
    //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
    //std::cout<<"\tEnd initialisation..."<<std::endl;
  }

}
コード例 #7
0
void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
  dW=0;

  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);

  double MI=0,MIprec=-1000;

  MI_preEstimation=-getCost(I,p);

  double i2,j2;
  double Tij;
  double IW,dx,dy;
  //unsigned
  int cr,ct;
  double er,et;
  double alpha=2.;

  int i,j;
  unsigned int iteration=0;

  initPosEvalRMS(p);
  do
  {
    if(iteration%5==0)
      initHessienDesired(I);
    Nbpoint=0;
    MIprec=MI;
    MI=0;
    erreur=0;

    zeroProbabilities();

    Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
    int nthreads = omp_get_num_procs() ;
    //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
    omp_set_num_threads(nthreads);
#pragma omp parallel for private(Tij,IW,i,j,i2,j2,cr,ct,er,et,dx,dy) default(shared)
#endif
    for(int point=0;point<(int)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))
      {
        Nbpoint++;
        Tij=ptTemplate[point].val;
        //Tij=Iterateurvecteur->val;
        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);

        dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
        dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;

        ct=(int)((IW*(Nc-1))/255.);
        cr=(int)((Tij*(Nc-1))/255.);
        et=(IW*(Nc-1))/255.-ct;
        er=((double)Tij*(Nc-1))/255.-cr;

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

        //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
        Warp->dWarp(X1,X2,p,dW);

        //double *tptemp=temp;
        double *tptemp=new double[nbParam];;
        for(unsigned int it=0;it<nbParam;it++)
          tptemp[it] =(dW[0][it]*dx+dW[1][it]*dy);
        //*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy;
        //std::cout<<cr<<"   "<<ct<<"  ; ";
        if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
          vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
        else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
          vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);

        delete[] tptemp;
      }
    }

    if(Nbpoint==0)
    {
      //std::cout<<"plus de point dans template suivi"<<std::endl;
      diverge=true;
      MI=0;
      deletePosEvalRMS();
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
    }
    else
    {
      computeProba(Nbpoint);
      computeMI(MI);
      //std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
      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;
        deletePosEvalRMS();
        throw(e);
      }
    }

    switch(minimizationMethod)
    {
    case vpTemplateTrackerMIForwardAdditional::USE_LMA:
    {
      vpColVector p_test_LMA(nbParam);
      if(ApproxHessian==HESSIAN_NONSECOND)
        p_test_LMA=p-100000.1*dp;
      else
        p_test_LMA=p+1.*dp;
      MI=-getCost(I,p);
      double MI_LMA=-getCost(I,p_test_LMA);
      if(MI_LMA>MI)
      {
        p=p_test_LMA;
        lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
      }
      else
      {
        lambda=(lambda*10.<1e6)?1e6:lambda*10.;
      }
    }
      break;
    case vpTemplateTrackerMIForwardAdditional::USE_GRADIENT:
    {
      dp=-gain*6.0*G;
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      p+=1.*dp;
      break;
    }

    case vpTemplateTrackerMIForwardAdditional::USE_QUASINEWTON:
    {
      double s_scal_y;
      if(iterationGlobale!=0)
      {
        vpColVector s_quasi=p-p_prec;
        vpColVector y_quasi=G-G_prec;
        s_scal_y=s_quasi.t()*y_quasi;
        //if(s_scal_y!=0)//BFGS
        //	KQuasiNewton=KQuasiNewton-(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())
          KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
      }
      dp=-KQuasiNewton*G;
      p_prec=p;
      G_prec=G;
      p-=1.01*dp;
    }
      break;

    default:
    {
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-0.1*dp;
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        //std::cout<<alpha<<std::endl;
        dp=alpha*dp;
      }

      p+=1.*dp;
      break;
    }
    }

    computeEvalRMS(p);
    iteration++;
    iterationGlobale++;

  }
  while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  //while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  if(Nbpoint==0) {
    //std::cout<<"plus de point dans template suivi"<<std::endl;
    deletePosEvalRMS();
    throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
  }

  nbIteration=iteration;
  MI_postEstimation=-getCost(I,p);
  if(MI_preEstimation>MI_postEstimation)
  {
    MI_postEstimation = -1;
  }
  deletePosEvalRMS();
}