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