 |
Visual Servoing Platform
version 3.3.0
|
43 #include <visp3/core/vpImageTools.h>
44 #include <visp3/tt/vpTemplateTrackerSSDForwardAdditional.h>
62 double IW, dIWx, dIWy;
64 unsigned int iteration = 0;
71 double evolRMS_init = 0;
72 double evolRMS_prec = 0;
76 unsigned int Nbpoint = 0;
80 Warp->computeCoeff(
p);
81 for (
unsigned int point = 0; point <
templateSize; point++) {
92 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
105 double *tempt =
new double[
nbParam];
106 for (
unsigned int it = 0; it <
nbParam; it++)
107 tempt[it] =
dW[0][it] * dIWx +
dW[1][it] * dIWy;
109 for (
unsigned int it = 0; it <
nbParam; it++)
110 for (
unsigned int jt = 0; jt <
nbParam; jt++)
111 H[it][jt] += tempt[it] * tempt[jt];
113 double er = (Tij - IW);
114 for (
unsigned int it = 0; it <
nbParam; it++)
115 G[it] += er * tempt[it];
132 switch (minimizationMethod) {
137 double erreur_LMA = -
getCost(I, p_test_LMA);
138 if (erreur_LMA < erreur) {
140 lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
142 lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
160 double s_scal_y = s_quasi.
t() * y_quasi;
161 if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
162 KQuasiNewton = KQuasiNewton + 0.001 * (s_quasi * s_quasi.
t() / s_scal_y -
163 KQuasiNewton * y_quasi * y_quasi.
t() * KQuasiNewton /
164 (y_quasi.
t() * KQuasiNewton * y_quasi));
166 dp = -KQuasiNewton *
G;
187 if (iteration == 0) {
194 evolRMS_delta = std::fabs(
evolRMS - evolRMS_prec);
Error that can be emited by the vpTracker class and its derivates.
void initPosEvalRMS(const vpColVector &p)
vpTemplateTrackerPoint * ptTemplate
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
unsigned int getHeight() const
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)=0
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
unsigned int templateSize
void computeEvalRMS(const vpColVector &p)
Implementation of column vector and the associated operations.
unsigned int getWidth() const
unsigned int iterationMax
vpMatrix inverseByLU() const
unsigned int iterationGlobale
Type getValue(unsigned int i, unsigned int j) const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
error that can be emited by ViSP classes.
vpTemplateTrackerWarp * Warp
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)