void vpTemplateTrackerSSDESM::initHessienDesired(const vpImage<unsigned char> &I)
{
initCompInverse(I);
}
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 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 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;
}
