 |
Visual Servoing Platform
version 3.2.0
|
43 #include <visp3/core/vpImageTools.h>
44 #include <visp3/tt/vpTemplateTrackerSSDForwardAdditional.h>
62 double IW, dIWx, dIWy;
64 unsigned int iteration = 0;
69 unsigned int Nbpoint = 0;
73 Warp->computeCoeff(
p);
74 for (
unsigned int point = 0; point <
templateSize; point++) {
85 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
98 double *tempt =
new double[
nbParam];
99 for (
unsigned int it = 0; it <
nbParam; it++)
100 tempt[it] =
dW[0][it] * dIWx +
dW[1][it] * dIWy;
102 for (
unsigned int it = 0; it <
nbParam; it++)
103 for (
unsigned int jt = 0; jt <
nbParam; jt++)
104 H[it][jt] += tempt[it] * tempt[jt];
106 double er = (Tij - IW);
107 for (
unsigned int it = 0; it <
nbParam; it++)
108 G[it] += er * tempt[it];
126 switch (minimizationMethod) {
129 p_test_LMA =
p + 1. *
dp;
131 double erreur_LMA = -
getCost(I, p_test_LMA);
132 if (erreur_LMA < erreur) {
134 lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
136 lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
154 double s_scal_y = s_quasi.
t() * y_quasi;
158 if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
159 KQuasiNewton = KQuasiNewton + 0.001 * (s_quasi * s_quasi.
t() / s_scal_y -
160 KQuasiNewton * y_quasi * y_quasi.
t() * KQuasiNewton /
161 (y_quasi.
t() * KQuasiNewton * y_quasi));
163 dp = -KQuasiNewton *
G;
Error that can be emited by the vpTracker class and its derivates.
Type getValue(unsigned int i, unsigned int j) const
vpTemplateTrackerPoint * ptTemplate
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
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
Implementation of column vector and the associated operations.
unsigned int iterationMax
unsigned int getHeight() const
vpMatrix inverseByLU() const
unsigned int iterationGlobale
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
error that can be emited by ViSP classes.
vpTemplateTrackerWarp * Warp
unsigned int getWidth() const
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)