bool
vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc,
const vpImagePoint &iP, unsigned char &Ipixelplan)
{
//test si pixel dans zone projetee
bool inside = false;
for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
if(listTriangle[i].inTriangle(iP)){
inside = true;
break;
}
if(!inside) return false;
// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
// return false;
//methoed algebrique
double z;
//calcul de la profondeur de l'intersection
z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
//calcul coordonnees 3D intersection
Xinter_optim[0]=iP.get_u()*z;
Xinter_optim[1]=iP.get_v()*z;
Xinter_optim[2]=z;
//recuperation des coordonnes de l'intersection dans le plan objet
//repere plan object :
// centre = X0_2_optim[i] (premier point definissant le plan)
// base = u:(X[1]-X[0]) et v:(X[3]-X[0])
//ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
double u = 0, v = 0;
for(unsigned int i = 0; i < 3; i++)
{
double diff = (Xinter_optim[i]-X0_2_optim[i]);
u += diff*vbase_u_optim[i];
v += diff*vbase_v_optim[i];
}
u = u/(euclideanNorm_u*euclideanNorm_u);
v = v/(euclideanNorm_v*euclideanNorm_v);
if( u > 0 && v > 0 && u < 1. && v < 1.)
{
double i2,j2;
i2=v*(Isrc.getHeight()-1);
j2=u*(Isrc.getWidth()-1);
if (interp == BILINEAR_INTERPOLATION)
Ipixelplan = Isrc.getValue(i2,j2);
else if (interp == SIMPLE)
Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
return true;
}
else
return false;
}
void vpTemplateTrackerSSDESM::trackNoPyr(const vpImage<unsigned char> &I)
{
double erreur=0;
unsigned 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 IW,dIWx,dIWy;
double Tij;
unsigned int iteration=0;
int i,j;
double i2,j2;
double alpha=2.;
do
{
Nbpoint=0;
erreur=0;
dp=0;
HDir=0;
GDir=0;
GInv=0;
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))
{
//INVERSE
Tij=ptTemplate[point].val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
Nbpoint++;
double er=(Tij-IW);
for(unsigned int it=0;it<nbParam;it++)
GInv[it]+=er*ptTemplate[point].dW[it];
erreur+=er*er;
//DIRECT
//dIWx=dIx.getValue(i2,j2);
//dIWy=dIy.getValue(i2,j2);
dIWx=dIx.getValue(i2,j2)+ptTemplate[point].dx;
dIWy=dIy.getValue(i2,j2)+ptTemplate[point].dy;
//Calcul du Hessien
//Warp->dWarp(X1,X2,p,dW);
Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
double *tempt=new double[nbParam];
for(unsigned int it=0;it<nbParam;it++)
tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
for(unsigned int it=0;it<nbParam;it++)
for(unsigned int jt=0;jt<nbParam;jt++)
HDir[it][jt]+=tempt[it]*tempt[jt];
for(unsigned int it=0;it<nbParam;it++)
GDir[it]+=er*tempt[it];
delete[] tempt;
}
}
if(Nbpoint==0) {
//std::cout<<"plus de point dans template suivi"<<std::endl;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
vpMatrix::computeHLM(HDir,lambdaDep,HLMDir);
try
{
//dp=(HLMInv+HLMDir).inverseByLU()*(GInv+GDir);
//dp=HLMInv.inverseByLU()*GInv+HLMDir.inverseByLU()*GDir;
//dp=HLMInv.inverseByLU()*GInv;
dp=(HLMDir).inverseByLU()*(GDir);
}
catch(vpException &e)
{
//std::cout<<"probleme inversion"<<std::endl;
throw(e);
}
dp=gain*dp;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
dp=alpha*dp;
}
//Warp->pRondp(p,dp,p);
p+=dp;
iteration++;
}
while( (iteration < iterationMax));
nbIteration=iteration;
}
void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
initCompInverse(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 Ic,dIcx=0.,dIcy=0.;
double Iref;
int i,j;
double i2,j2;
int Nbpoint=0;
double moyIref=0;
double moyIc=0;
double denom=0;
moydIrefdp.resize(nbParam); moydIrefdp=0;
vpMatrix moyd2Iref(nbParam,nbParam);moyd2Iref=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))
{
Iref=ptTemplate[point].val;
if(!blur)
Ic=I.getValue(i2,j2);
else
Ic=BI.getValue(i2,j2);
Nbpoint++;
moyIref+=Iref;
moyIc+=Ic;
for(unsigned int it=0;it<nbParam;it++)
moydIrefdp[it]+=ptTemplate[point].dW[it];
Warp->dWarp(X1,X2,p,dW);
double *tempt=new double[nbParam];
for(unsigned int it=0;it<nbParam;it++)
tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
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++)
{
moyd2Iref[it][jt] +=(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));
}
delete[] tempt;
}
}
moyIref=moyIref/Nbpoint;
moydIrefdp=moydIrefdp/Nbpoint;
moyd2Iref=moyd2Iref/Nbpoint;
moyIc=moyIc/Nbpoint;
Hdesire=0;
double covarIref=0,covarIc=0;
double sIcIref=0;
vpColVector sIcdIref(nbParam);sIcdIref=0;
vpMatrix sIcd2Iref(nbParam,nbParam);sIcd2Iref=0;
vpMatrix sdIrefdIref(nbParam,nbParam);sdIrefdIref=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))
{
Iref=ptTemplate[point].val;
if(!blur)
Ic=I.getValue(i2,j2);
else
Ic=BI.getValue(i2,j2);
dIcx=dIx.getValue(i2,j2);
dIcy=dIy.getValue(i2,j2);
Warp->dWarp(X1,X2,p,dW);
double *tempt=new double[nbParam];
for(unsigned int it=0;it<nbParam;it++)
tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
double prodIc=(Ic-moyIc);
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++)
{
sIcd2Iref[it][jt] +=prodIc*(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)-moyd2Iref[it][jt]);
sdIrefdIref[it][jt] +=(ptTemplate[point].dW[it]-moydIrefdp[it])*(ptTemplate[point].dW[jt]-moydIrefdp[jt]);
}
delete[] tempt;
for(unsigned int it=0;it<nbParam;it++)
sIcdIref[it]+=prodIc*(ptTemplate[point].dW[it]-moydIrefdp[it]);
covarIref+=(Iref-moyIref)*(Iref-moyIref);
covarIc+=(Ic-moyIc)*(Ic-moyIc);
sIcIref+=(Iref-moyIref)*(Ic-moyIc);
}
}
covarIref=sqrt(covarIref);
covarIc=sqrt(covarIc);
denom=covarIref*covarIc;
double NCC=sIcIref/denom;
//std::cout<<"NCC = "<<NCC<<std::endl;
vpColVector dcovarIref(nbParam);dcovarIref=-sIcdIref/covarIref;
vpColVector dNCC(nbParam);dNCC=(sIcdIref/denom-NCC*dcovarIref/covarIref);
vpMatrix d2covarIref(nbParam,nbParam);
d2covarIref=-(sIcd2Iref-sdIrefdIref+dcovarIref*dcovarIref.t())/covarIref;
#ifdef APPROX_NCC
Hdesire=sIcd2Iref/denom;
#else
Hdesire=(sIcd2Iref-sdIrefdIref+dcovarIref*dcovarIref.t())/denom;
#endif
vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire);
HLMdesireInverse=HLMdesire.inverseByLU();
//std::cout<<"Hdesire = "<<Hdesire<<std::endl;
}
void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
//double erreur=0;
vpColVector dpinv(nbParam);
double Ic;
double Iref;
unsigned int iteration=0;
int i,j;
double i2,j2;
initPosEvalRMS(p);
do
{
unsigned int Nbpoint=0;
//erreur=0;
G=0;
Warp->computeCoeff(p);
double moyIref=0;
double moyIc=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))
{
Iref=ptTemplate[point].val;
if(!blur)
Ic=I.getValue(i2,j2);
else
Ic=BI.getValue(i2,j2);
Nbpoint++;
moyIref+=Iref;
moyIc+=Ic;
}
}
if(Nbpoint > 0)
{
moyIref=moyIref/Nbpoint;
moyIc=moyIc/Nbpoint;
double sIcIref=0;
double covarIref=0,covarIc=0;
vpColVector sIcdIref(nbParam);sIcdIref=0;
vpColVector sIrefdIref(nbParam);sIrefdIref=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))
{
Iref=ptTemplate[point].val;
if(!blur)
Ic=I.getValue(i2,j2);
else
Ic=BI.getValue(i2,j2);
double prod=(Ic-moyIc);
for(unsigned int it=0;it<nbParam;it++)
sIcdIref[it]+=prod*(ptTemplate[point].dW[it]-moydIrefdp[it]);
for(unsigned int it=0;it<nbParam;it++)
sIrefdIref[it]+=(Iref-moyIref)*(ptTemplate[point].dW[it]-moydIrefdp[it]);
//double er=(Iref-Ic);
//erreur+=(er*er);
//denom+=(Iref-moyIref)*(Iref-moyIref)*(Ic-moyIc)*(Ic-moyIc);
covarIref+=(Iref-moyIref)*(Iref-moyIref);
covarIc+=(Ic-moyIc)*(Ic-moyIc);
sIcIref+=(Iref-moyIref)*(Ic-moyIc);
}
}
covarIref=sqrt(covarIref);
covarIc=sqrt(covarIc);
double denom=covarIref*covarIc;
//if(denom==0.0)
if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
{
diverge=true;
}
else
{
double NCC=sIcIref/denom;
vpColVector dcovarIref(nbParam);dcovarIref=sIrefdIref/covarIref;
G=1.*(sIcdIref/denom-NCC*dcovarIref/covarIref);
try
{
dp=-1.*HLMdesireInverse*G;
}
catch(...)
{
std::cout<<"probleme inversion"<<std::endl;
break;
}
Warp->getParamInverse(dp,dpinv);
Warp->pRondp(p,dpinv,p);
computeEvalRMS(p);
}
}
else
diverge=true;
iteration++;
}
while( (!diverge &&(evolRMS>threshold_RMS) && (iteration < iterationMax)));
//std::cout<<"erreur "<<erreur<<std::endl;
nbIteration=iteration;
deletePosEvalRMS();
}
void vpTemplateTrackerSSDInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
double erreur=0;
unsigned int Nbpoint=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
vpColVector dpinv(nbParam);
double IW;
double Tij;
unsigned int iteration=0;
int i,j;
double i2,j2;
double alpha=2.;
//vpTemplateTrackerPointtest *pt;
initPosEvalRMS(p);
vpTemplateTrackerPoint *pt;
do
{
Nbpoint=0;
erreur=0;
dp=0;
Warp->computeCoeff(p);
for(unsigned int point=0;point<templateSize;point++)
{
if((!useTemplateSelect)||(ptTemplateSelect[point]))
{
//pt=&ptTemplatetest[point];
pt=&ptTemplate[point];
i=pt->y;
j=pt->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=pt->val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
Nbpoint++;
double er=(Tij-IW);
for(unsigned int it=0;it<nbParam;it++)
dp[it]+=er*pt->HiG[it];
erreur+=er*er;
}
}
}
//std::cout << "npoint: " << Nbpoint << std::endl;
if(Nbpoint==0) {
//std::cout<<"plus de point dans template suivi"<<std::endl;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
dp=gain*dp;
//std::cout<<erreur/Nbpoint<<","<<GetCost(I,p)<<std::endl;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
dp=alpha*dp;
}
Warp->getParamInverse(dp,dpinv);
Warp->pRondp(p,dpinv,p);
iteration++;
computeEvalRMS(p);
//std::cout << "iteration: " << iteration << " max: " << iterationMax << std::endl;
//std::cout << "evolRMS: " << evolRMS << " threshold: " << threshold_RMS << std::endl;
}
while(/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax)&&(evolRMS>threshold_RMS));
nbIteration=iteration;
deletePosEvalRMS();
}
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")) ;
}
}
}
}
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 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;
}
double vpTemplateTrackerZNCC::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
{
double IW,Tij;
int i,j;
double i2,j2;
int Nbpoint=0;
Warp->computeCoeff(tp);
double moyTij=0;
double moyIW=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,tp);
Warp->warpX(X1,X2,tp);
j2=X2[0];i2=X2[1];
if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
{
Tij=ptTemplate[point].val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
//IW=getSubPixBspline4(I,i2,j2);
moyTij+=Tij;
moyIW+=IW;
Nbpoint++;
}
}
ratioPixelIn=(double)Nbpoint/(double)templateSize;
if(! Nbpoint) {
throw(vpException(vpException::divideByZeroError,
"Cannot get cost: size = 0")) ;
}
moyTij=moyTij/Nbpoint;
moyIW=moyIW/Nbpoint;
double nom=0;//,denom=0;
double var1=0,var2=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,tp);
Warp->warpX(X1,X2,tp);
j2=X2[0];i2=X2[1];
if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
{
Tij=ptTemplate[point].val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
//IW=getSubPixBspline4(I,i2,j2);
nom+=(Tij-moyTij)*(IW-moyIW);
//denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
var1+=(IW-moyIW)*(IW-moyIW);
var2+=(Tij-moyTij)*(Tij-moyTij);
Nbpoint++;
}
}
// if(Nbpoint==0)return 10e10; // cannot occur
//return -nom/sqrt(denom);
return -nom/sqrt(var1*var2);
}
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 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();
}
