40 #include <visp3/core/vpTrackingException.h>
41 #include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
46 :
vpTemplateTrackerMI(_warp), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
47 p_prec(), G_prec(), KQuasiNewton()
56 unsigned int index = 0;
59 double (*ptBspFct)(double);
60 double (*ptdBspFct)(double);
61 double (*ptd2BspFct)(double);
66 ptBspFct = &vpTemplateTrackerMIBSpline::Bspline3;
67 ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline3;
68 ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline3;
70 ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
71 ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
72 ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
76 for (
int it = -1; it <= endIndex; it++) {
77 ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)(
static_cast<double>(-it) + et);
79 for (
unsigned int ip = 0; ip <
nbParam; ++ip) {
80 ptTemplateSupp[ptIndex].BtInit[index++] =
81 (*ptdBspFct)(
static_cast<double>(-it) + et) *
ptTemplate[ptIndex].
dW[ip] * (-1.0);
82 for (
unsigned int ip2 = 0; ip2 <
nbParam; ++ip2) {
83 ptTemplateSupp[ptIndex].BtInit[index++] =
92 ptTemplateSupp =
new vpTemplateTrackerPointSuppMIInv[
templateSize];
103 double Nc_255_ = (
Nc - 1) / 255.;
104 Warp->computeCoeff(
p);
105 for (
unsigned int point = 0; point <
templateSize; point++) {
116 int ct =
static_cast<int>(Tij * Nc_255_);
117 double et = (Tij * Nc_255_) - ct;
118 ptTemplateSupp[point].et = et;
119 ptTemplateSupp[point].ct = ct;
121 CompoInitialised =
true;
140 Warp->computeCoeff(
p);
142 for (
unsigned int point = 0; point <
templateSize; point++) {
154 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
162 ct = ptTemplateSupp[point].ct;
163 et = ptTemplateSupp[point].et;
164 cr =
static_cast<int>((IW * (
Nc - 1)) / 255.);
165 er = (IW * (
Nc - 1)) / 255. - cr;
197 if (!CompoInitialised) {
198 std::cout <<
"Compositionnal tracking not initialised.\nUse initCompInverse() function." << std::endl;
207 double MI = 0, MIprec = -1000;
210 p_avant_estimation =
p;
219 unsigned int iteration = 0;
222 double evolRMS_init = 0;
223 double evolRMS_prec = 0;
224 double evolRMS_delta;
237 Warp->computeCoeff(
p);
239 for (
int point = 0; point < static_cast<int>(
templateSize); point++) {
249 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
254 IW =
static_cast<double>(I.
getValue(i2, j2));
258 int ct = ptTemplateSupp[point].ct;
259 double et = ptTemplateSupp[point].et;
260 double tmp = IW * (
static_cast<double>(
Nc) - 1.) / 255.;
261 int cr =
static_cast<int>(tmp);
262 double er = tmp -
static_cast<double>(cr);
288 unsigned int indd, indd2;
290 unsigned int Ncb_ =
static_cast<unsigned int>(
Ncb);
291 for (
unsigned int i = 0; i < Ncb_ * Ncb_; i++) {
293 for (
unsigned int j = 0; j <
nbParam; j++) {
294 dPrt[indd] /= Nbpoint;
296 for (
unsigned int k = 0; k <
nbParam; k++) {
297 d2Prt[indd2] /= Nbpoint;
332 switch (minimizationMethod) {
335 dp_test_LMA = -100000.1 *
dp;
342 double MI_LMA = -
getCost(I, p_test_LMA);
359 double s_scal_y = s_quasi.
t() * y_quasi;
360 if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
361 KQuasiNewton = KQuasiNewton + 0.0001 * (s_quasi * s_quasi.
t() / s_scal_y -
362 KQuasiNewton * y_quasi * y_quasi.
t() * KQuasiNewton /
363 (y_quasi.
t() * KQuasiNewton * y_quasi));
388 if (iteration == 0) {
394 evolRMS_delta = std::fabs(
evolRMS - evolRMS_prec);
397 }
while ((!
diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
414 p = p_avant_estimation;
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
unsigned int getWidth() const
Type getValue(unsigned int i, unsigned int j) const
unsigned int getHeight() const
Implementation of a matrix and operations on matrices.
double cond(double svThreshold=1e-6) const
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
void initCompInverse(const vpImage< unsigned char > &I)
void initHessienDesired(const vpImage< unsigned char > &I)
vpTemplateTrackerMIInverseCompositional()
Default constructor.
void trackNoPyr(const vpImage< unsigned char > &I)
void initTemplateRefBspline(unsigned int ptIndex, double &et)
vpHessienApproximationType ApproxHessian
void computeHessienNormalized(vpMatrix &H)
vpHessienType hessianComputation
void computeMI(double &MI)
void computeProba(int &nbpoint)
void computeHessien(vpMatrix &H)
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
vpMatrix covarianceMatrix
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
double NMI_postEstimation
unsigned int getNbParam() const
virtual void getParamInverse(const vpColVector &p, vpColVector &p_inv) const =0
virtual void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)=0
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const =0
vpMatrix HLMdesireInverse
void computeEvalRMS(const vpColVector &p)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
unsigned int iterationMax
void initPosEvalRMS(const vpColVector &p)
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
unsigned int iterationGlobale
unsigned int templateSize
Error that can be emited by the vpTracker class and its derivates.