39 #include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
40 #include <visp3/core/vpTrackingException.h>
45 :
vpTemplateTrackerMI(_warp), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
46 evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(1e-20), p_prec(), G_prec(), KQuasiNewton()
55 unsigned int index = 0;
58 double (*ptBspFct)(double);
59 double (*ptdBspFct)(double);
60 double (*ptd2BspFct)(double);
63 ptBspFct = &vpTemplateTrackerMIBSpline::Bspline3;
64 ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline3;
65 ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline3;
68 ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
69 ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
70 ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
74 for(
int it=-1; it<=endIndex; it++)
76 ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)((double)(-it)+et);
78 for(
unsigned int ip=0;ip<
nbParam;++ip)
80 ptTemplateSupp[ptIndex].BtInit[index++] = (*ptdBspFct)((double)(-it)+et) *
ptTemplate[ptIndex].
dW[ip] * (-1.0);
81 for(
unsigned int ip2=0;ip2<
nbParam;++ip2)
83 ptTemplateSupp[ptIndex].BtInit[index++] = (*ptd2BspFct)((double)(-it)+et) *
ptTemplate[ptIndex].
dW[ip] *
ptTemplate[ptIndex].
dW[ip2];
91 ptTemplateSupp=
new vpTemplateTrackerPointSuppMIInv[
templateSize];
107 Warp->computeCoeff(
p);
123 ct=(int)((Tij*(
Nc-1))/255.);
124 et=(Tij*(
Nc-1))/255.-ct;
126 ptTemplateSupp[point].et=et;
127 ptTemplateSupp[point].ct=ct;
135 CompoInitialised=
true;
160 Warp->computeCoeff(
p);
188 ct=ptTemplateSupp[point].ct;
189 et=ptTemplateSupp[point].et;
190 cr=(int)((IW*(
Nc-1))/255.);
191 er=((double)IW*(
Nc-1))/255.-cr;
194 erreur+=(Tij-IW)*(Tij-IW);
244 if(!CompoInitialised)
245 std::cout<<
"Compositionnal tracking no initialised\nUse InitCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
254 double MI=0,MIprec=-1000;
268 unsigned int iteration=0;
283 Warp->computeCoeff(
p);
297 Warp->computeDenom(x1,
p);
314 ct=ptTemplateSupp[point].ct;
315 et=ptTemplateSupp[point].et;
316 double tmp = IW*(((double)
Nc)-1.f)/255.f;
354 unsigned int indd, indd2;
356 unsigned int Ncb_ = (
unsigned int)
Ncb;
357 for(
unsigned int i=0;i<Ncb_*Ncb_;i++){
359 for(
unsigned int j=0;j<
nbParam;j++){
362 for(
unsigned int k=0;k<
nbParam;k++){
404 switch(minimizationMethod)
412 dp_test_LMA=-100000.1*
dp;
419 double MI_LMA=-
getCost(I,p_test_LMA);
443 s_scal_y=s_quasi.
t()*y_quasi;
448 if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
450 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));
487 while( (!
diverge) && (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration<
iterationMax)&&(evolRMS>threshold_RMS) );
512 p=p_avant_estimation;
540 x_pos=
new double[nb_corners];
541 y_pos=
new double[nb_corners];
543 Warp->computeCoeff(pw);
549 for (
unsigned int j=0; j<3; j++) {
552 Warp->computeDenom(
X1,pw);
564 Warp->computeCoeff(pw);
571 for (
unsigned int j=0; j<3; j++) {
574 Warp->computeDenom(
X1,pw);
576 evolRMS+=(x_pos[i*3+j]-
X2[0])*(x_pos[i*3+j]-
X2[0])+(y_pos[i*3+j]-
X2[1])*(y_pos[i*3+j]-
X2[1]);
581 evolRMS=evolRMS/nb_corners;
Implementation of a matrix and operations on matrices.
void initTemplateRefBspline(unsigned int ptIndex, double &et)
double NMI_postEstimation
void computeHessien(vpMatrix &H)
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
unsigned int getWidth() const
vpMatrix covarianceMatrix
static void getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
vpTemplateTrackerPoint * ptTemplate
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
error that can be emited by ViSP classes.
Type getValue(double i, double j) const
vpColVector getCorner(unsigned int i) const
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
vpHessienApproximationType ApproxHessian
void trackNoPyr(const vpImage< unsigned char > &I)
virtual void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const =0
void initPosEvalRMS(const vpColVector &p)
unsigned int templateSize
unsigned int iterationMax
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const =0
void initHessienDesired(const vpImage< unsigned char > &I)
Error that can be emited by the vpTracker class and its derivates.
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
unsigned int getNbTriangle() const
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M)
void computeMI(double &MI)
void computeEvalRMS(const vpColVector &p)
unsigned int iterationGlobale
unsigned int getNbParam() const
virtual void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)=0
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
vpTemplateTrackerMIInverseCompositional()
Default constructor.
void initCompInverse(const vpImage< unsigned char > &I)
vpMatrix HLMdesireInverse
Implementation of column vector and the associated operations.
vpMatrix inverseByLU() const
vpTemplateTrackerZone * zoneTracked
unsigned int getHeight() const
void computeHessienNormalized(vpMatrix &H)
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
void computeProba(int &nbpoint)
vpHessienType hessianComputation