39 #include <visp3/core/vpTrackingException.h>
40 #include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
45 :
vpTemplateTrackerMI(_warp), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
46 p_prec(), G_prec(), KQuasiNewton()
55 unsigned int index = 0;
58 double (*ptBspFct)(double);
59 double (*ptdBspFct)(double);
60 double (*ptd2BspFct)(double);
65 ptBspFct = &vpTemplateTrackerMIBSpline::Bspline3;
66 ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline3;
67 ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline3;
69 ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
70 ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
71 ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
75 for (
int it = -1; it <= endIndex; it++) {
76 ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)(
static_cast<double>(-it) + et);
78 for (
unsigned int ip = 0; ip <
nbParam; ++ip) {
79 ptTemplateSupp[ptIndex].BtInit[index++] =
80 (*ptdBspFct)(
static_cast<double>(-it) + et) *
ptTemplate[ptIndex].
dW[ip] * (-1.0);
81 for (
unsigned int ip2 = 0; ip2 <
nbParam; ++ip2) {
82 ptTemplateSupp[ptIndex].BtInit[index++] =
91 ptTemplateSupp =
new vpTemplateTrackerPointSuppMIInv[
templateSize];
102 double Nc_255_ = (
Nc - 1) / 255.;
103 Warp->computeCoeff(
p);
104 for (
unsigned int point = 0; point <
templateSize; point++) {
115 int ct =
static_cast<int>(Tij * Nc_255_);
116 double et = (Tij * Nc_255_) - ct;
117 ptTemplateSupp[point].et = et;
118 ptTemplateSupp[point].ct = ct;
120 CompoInitialised =
true;
139 Warp->computeCoeff(
p);
141 for (
unsigned int point = 0; point <
templateSize; point++) {
153 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
161 ct = ptTemplateSupp[point].ct;
162 et = ptTemplateSupp[point].et;
163 cr =
static_cast<int>((IW * (
Nc - 1)) / 255.);
164 er = (IW * (
Nc - 1)) / 255. - cr;
196 if (!CompoInitialised) {
197 std::cout <<
"Compositionnal tracking not initialised.\nUse initCompInverse() function." << std::endl;
206 double MI = 0, MIprec = -1000;
209 p_avant_estimation =
p;
218 unsigned int iteration = 0;
221 double evolRMS_init = 0;
222 double evolRMS_prec = 0;
223 double evolRMS_delta;
236 Warp->computeCoeff(
p);
238 for (
int point = 0; point < static_cast<int>(
templateSize); point++) {
248 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
253 IW =
static_cast<double>(I.
getValue(i2, j2));
257 int ct = ptTemplateSupp[point].ct;
258 double et = ptTemplateSupp[point].et;
259 double tmp = IW * (
static_cast<double>(
Nc) - 1.) / 255.;
260 int cr =
static_cast<int>(tmp);
261 double er = tmp -
static_cast<double>(cr);
287 unsigned int indd, indd2;
289 unsigned int Ncb_ =
static_cast<unsigned int>(
Ncb);
290 for (
unsigned int i = 0; i < Ncb_ * Ncb_; i++) {
292 for (
unsigned int j = 0; j <
nbParam; j++) {
293 dPrt[indd] /= Nbpoint;
295 for (
unsigned int k = 0; k <
nbParam; k++) {
296 d2Prt[indd2] /= Nbpoint;
331 switch (minimizationMethod) {
334 dp_test_LMA = -100000.1 *
dp;
341 double MI_LMA = -
getCost(I, p_test_LMA);
358 double s_scal_y = s_quasi.
t() * y_quasi;
359 if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
360 KQuasiNewton = KQuasiNewton + 0.0001 * (s_quasi * s_quasi.
t() / s_scal_y -
361 KQuasiNewton * y_quasi * y_quasi.
t() * KQuasiNewton /
362 (y_quasi.
t() * KQuasiNewton * y_quasi));
387 if (iteration == 0) {
393 evolRMS_delta = std::fabs(
evolRMS - evolRMS_prec);
396 }
while ((!
diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
413 p = p_avant_estimation;
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void filter(const vpImage< ImageType > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false)
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void getGradX(const vpImage< unsigned char > &I, vpImage< FilterType > &dIx)
static void getGradY(const vpImage< unsigned char > &I, vpImage< FilterType > &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 getCost(const vpImage< unsigned char > &I, const vpColVector &tp) override
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
vpMatrix covarianceMatrix
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 emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.