 |
Visual Servoing Platform
version 3.2.0
|
41 #include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
43 #ifdef VISP_HAVE_OPENMP
48 :
vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(0),
49 p_prec(), G_prec(), KQuasiNewton()
52 threshold_RMS = 1e-20;
76 Warp->computeCoeff(
p);
77 for (
unsigned int point = 0; point <
templateSize; point++) {
91 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
102 ct = (int)((IW * (
Nc - 1)) / 255.);
103 cr = (int)((Tij * (
Nc - 1)) / 255.);
104 et = (IW * (
Nc - 1)) / 255. - ct;
105 er = ((double)Tij * (
Nc - 1)) / 255. - cr;
109 double *tptemp =
new double[
nbParam];
110 for (
unsigned int it = 0; it <
nbParam; it++)
111 tptemp[it] =
dW[0][it] * dx +
dW[1][it] * dy;
153 double MI = 0, MIprec = -1000;
159 unsigned int iteration = 0;
163 if (iteration % 5 == 0)
172 Warp->computeCoeff(
p);
173 #ifdef VISP_HAVE_OPENMP
174 int nthreads = omp_get_num_procs();
177 omp_set_num_threads(nthreads);
178 #pragma omp parallel for default(shared)
180 for (
int point = 0; point < (int)
templateSize; point++) {
192 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
204 int ct = (int)((IW * (
Nc - 1)) / 255.);
205 int cr = (int)((Tij * (
Nc - 1)) / 255.);
206 double et = (IW * (
Nc - 1)) / 255. - ct;
207 double er = ((double)Tij * (
Nc - 1)) / 255. - cr;
217 double *tptemp =
new double[
nbParam];
219 for (
unsigned int it = 0; it <
nbParam; it++)
220 tptemp[it] = (
dW[0][it] * dx +
dW[1][it] * dy);
268 switch (minimizationMethod) {
272 p_test_LMA =
p - 100000.1 *
dp;
274 p_test_LMA =
p + 1. *
dp;
276 double MI_LMA = -
getCost(I, p_test_LMA);
299 double s_scal_y = s_quasi.
t() * y_quasi;
303 if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
304 KQuasiNewton = KQuasiNewton + 0.001 * (s_quasi * s_quasi.
t() / s_scal_y -
305 KQuasiNewton * y_quasi * y_quasi.
t() * KQuasiNewton /
306 (y_quasi.
t() * KQuasiNewton * y_quasi));
308 dp = -KQuasiNewton *
G;
333 }
while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
334 (iteration <
iterationMax) && (evolRMS > threshold_RMS));
354 x_pos =
new double[nb_corners];
355 y_pos =
new double[nb_corners];
357 Warp->computeCoeff(pw);
362 for (
unsigned int j = 0; j < 3; j++) {
365 Warp->computeDenom(
X1, pw);
367 x_pos[i * 3 + j] =
X2[0];
368 y_pos[i * 3 + j] =
X2[1];
377 Warp->computeCoeff(pw);
383 for (
unsigned int j = 0; j < 3; j++) {
386 Warp->computeDenom(
X1, pw);
388 evolRMS += (x_pos[i * 3 + j] -
X2[0]) * (x_pos[i * 3 + j] -
X2[0]) +
389 (y_pos[i * 3 + j] -
X2[1]) * (y_pos[i * 3 + j] -
X2[1]);
390 x_pos[i * 3 + j] =
X2[0];
391 y_pos[i * 3 + j] =
X2[1];
394 evolRMS = evolRMS / nb_corners;
Error that can be emited by the vpTracker class and its derivates.
Type getValue(unsigned int i, unsigned int j) const
void initHessienDesired(const vpImage< unsigned char > &I)
vpTemplateTrackerPoint * ptTemplate
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
void computeEvalRMS(const vpColVector &p)
void computeProba(int &nbpoint)
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
void initPosEvalRMS(const vpColVector &p)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
void trackNoPyr(const vpImage< unsigned char > &I)
vpColVector getCorner(unsigned int i) const
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)=0
void computeMI(double &MI)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
unsigned int getNbTriangle() const
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
vpMatrix HLMdesireInverse
unsigned int getHeight() const
vpTemplateTrackerMIForwardAdditional()
Default constructor.
vpMatrix inverseByLU() const
vpHessienApproximationType ApproxHessian
unsigned int iterationGlobale
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
vpHessienType hessianComputation
vpTemplateTrackerZone * zoneTracked
error that can be emited by ViSP classes.
void computeHessien(vpMatrix &H)
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)