Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
vpTemplateTrackerMIForwardAdditional.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Example of template tracking.
33  *
34  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  *
38 *****************************************************************************/
39 
40 #include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
41 
42 #ifdef VISP_HAVE_OPENMP
43 #include <omp.h>
44 #endif
45 
47  : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), p_prec(), G_prec(), KQuasiNewton()
48 {
49  useCompositionnal = false;
50 }
51 
53 {
54  dW = 0;
55 
56  int Nbpoint = 0;
57 
58  if (blur)
62 
63  double Tij;
64  double IW, dx, dy;
65  int cr, ct;
66  double er, et;
67 
68  Nbpoint = 0;
69 
71  Warp->computeCoeff(p);
72  for (unsigned int point = 0; point < templateSize; point++) {
73  int i = ptTemplate[point].y;
74  int j = ptTemplate[point].x;
75  X1[0] = j;
76  X1[1] = i;
77  X2[0] = j;
78  X2[1] = i;
79 
80  Warp->computeDenom(X1, p);
81  Warp->warpX(X1, X2, p);
82 
83  double j2 = X2[0];
84  double i2 = X2[1];
85 
86  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
87  Nbpoint++;
88  Tij = ptTemplate[point].val;
89  if (!blur)
90  IW = I.getValue(i2, j2);
91  else
92  IW = BI.getValue(i2, j2);
93 
94  dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
95  dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
96 
97  ct = static_cast<int>((IW * (Nc - 1)) / 255.);
98  cr = static_cast<int>((Tij * (Nc - 1)) / 255.);
99  et = (IW * (Nc - 1)) / 255. - ct;
100  er = (static_cast<double>(Tij) * (Nc - 1)) / 255. - cr;
101  Warp->dWarp(X1, X2, p, dW);
102 
103  double *tptemp = new double[nbParam];
104  for (unsigned int it = 0; it < nbParam; it++)
105  tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
106 
108  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
110  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
111 
112  delete[] tptemp;
113  }
114  }
115 
116  if (Nbpoint > 0) {
117  double MI;
118  computeProba(Nbpoint);
119  computeMI(MI);
121 
123  try {
125  } catch (const vpException &e) {
126  throw(e);
127  }
128  }
129 }
130 
132 {
133  dW = 0;
134 
135  int Nbpoint = 0;
136  if (blur)
140 
141  double MI = 0, MIprec = -1000;
142 
143  MI_preEstimation = -getCost(I, p);
144 
145  double alpha = 2.;
146 
147  unsigned int iteration = 0;
148 
149  initPosEvalRMS(p);
150  double evolRMS_init = 0;
151  double evolRMS_prec = 0;
152  double evolRMS_delta;
153 
154  do {
155  if (iteration % 5 == 0)
157  Nbpoint = 0;
158  MIprec = MI;
159  MI = 0;
160  // erreur=0;
161 
163 
164  Warp->computeCoeff(p);
165 #ifdef VISP_HAVE_OPENMP
166  int nthreads = omp_get_num_procs();
167  // std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function:
168  // " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
169  omp_set_num_threads(nthreads);
170 #pragma omp parallel for default(shared)
171 #endif
172  for (int point = 0; point < (int)templateSize; point++) {
173  int i = ptTemplate[point].y;
174  int j = ptTemplate[point].x;
175  X1[0] = j;
176  X1[1] = i;
177 
178  Warp->computeDenom(X1, p);
179  Warp->warpX(X1, X2, p);
180 
181  double j2 = X2[0];
182  double i2 = X2[1];
183 
184  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
185  Nbpoint++;
186  double Tij = ptTemplate[point].val;
187  double IW;
188  if (!blur)
189  IW = I.getValue(i2, j2);
190  else
191  IW = BI.getValue(i2, j2);
192 
193  double dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
194  double dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
195 
196  int ct = (int)((IW * (Nc - 1)) / 255.);
197  int cr = (int)((Tij * (Nc - 1)) / 255.);
198  double et = (IW * (Nc - 1)) / 255. - ct;
199  double er = ((double)Tij * (Nc - 1)) / 255. - cr;
200 
201  Warp->dWarp(X1, X2, p, dW);
202 
203  double *tptemp = new double[nbParam];
204  for (unsigned int it = 0; it < nbParam; it++)
205  tptemp[it] = (dW[0][it] * dx + dW[1][it] * dy);
207  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
209  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
210 
211  delete[] tptemp;
212  }
213  }
214 
215  if (Nbpoint == 0) {
216  diverge = true;
217  MI = 0;
218  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
219  } else {
220  computeProba(Nbpoint);
221  computeMI(MI);
222  computeHessien(H);
223  computeGradient();
224 
226  try {
227  switch (hessianComputation) {
229  dp = gain * HLMdesireInverse * G;
230  break;
232  if (HLM.cond() > HLMdesire.cond())
233  dp = gain * HLMdesireInverse * G;
234  else
235  dp = gain * 0.2 * HLM.inverseByLU() * G;
236  break;
237  default:
238  dp = gain * 0.2 * HLM.inverseByLU() * G;
239  break;
240  }
241  } catch (const vpException &e) {
242  throw(e);
243  }
244  }
245 
246  switch (minimizationMethod) {
248  vpColVector p_test_LMA(nbParam);
250  p_test_LMA = p - 100000.1 * dp;
251  else
252  p_test_LMA = p + dp;
253  MI = -getCost(I, p);
254  double MI_LMA = -getCost(I, p_test_LMA);
255  if (MI_LMA > MI) {
256  p = p_test_LMA;
257  lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
258  } else {
259  lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
260  }
261  } break;
263  dp = -gain * 6.0 * G;
264  if (useBrent) {
265  alpha = 2.;
266  computeOptimalBrentGain(I, p, -MI, dp, alpha);
267  dp = alpha * dp;
268  }
269  p += dp;
270  break;
271  }
272 
274  if (iterationGlobale != 0) {
275  vpColVector s_quasi = p - p_prec;
276  vpColVector y_quasi = G - G_prec;
277  double s_scal_y = s_quasi.t() * y_quasi;
278  if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
279  KQuasiNewton = KQuasiNewton + 0.001 * (s_quasi * s_quasi.t() / s_scal_y -
280  KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
281  (y_quasi.t() * KQuasiNewton * y_quasi));
282  }
283  }
284  dp = -KQuasiNewton * G;
285  p_prec = p;
286  G_prec = G;
287  p -= 1.01 * dp;
288  } break;
289 
290  default: {
292  dp = -0.1 * dp;
293  if (useBrent) {
294  alpha = 2.;
295  computeOptimalBrentGain(I, p, -MI, dp, alpha);
296  dp = alpha * dp;
297  }
298 
299  p += dp;
300  break;
301  }
302  }
303 
304  computeEvalRMS(p);
305 
306  if (iteration == 0) {
307  evolRMS_init = evolRMS;
308  }
309  iteration++;
311 
312  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
313  evolRMS_prec = evolRMS;
314 
315  } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
316  (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init) * evolRMS_eps));
317 
318  if (Nbpoint == 0) {
319  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
320  }
321 
322  nbIteration = iteration;
323  MI_postEstimation = -getCost(I, p);
325  MI_postEstimation = -1;
326  }
327 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition: vpException.h:59
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size, const vpImage< bool > *p_mask=nullptr)
static void filter(const vpImage< ImageType > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false, const vpImage< bool > *p_mask=nullptr)
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size, const vpImage< bool > *p_mask=nullptr)
unsigned int getWidth() const
Definition: vpImage.h:249
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1816
unsigned int getHeight() const
Definition: vpImage.h:184
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6346
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6407
void initHessienDesired(const vpImage< unsigned char > &I)
void trackNoPyr(const vpImage< unsigned char > &I)
vpHessienApproximationType ApproxHessian
vpHessienType hessianComputation
void computeMI(double &MI)
void computeProba(int &nbpoint)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp) vp_override
void computeHessien(vpMatrix &H)
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, vpMatrix &dM)=0
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
vpImage< double > dIx
vpImage< double > dIy
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)
unsigned int nbIteration
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
unsigned int iterationGlobale
vpImage< double > BI
unsigned int templateSize
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.