Visual Servoing Platform  version 3.2.0
vpTemplateTracker.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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  * Template tracker.
33  *
34  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  * Fabien Spindler
38  *
39  *****************************************************************************/
45 #ifndef vpTemplateTracker_hh
46 #define vpTemplateTracker_hh
47 
48 #include <math.h>
49 
50 #include <visp3/core/vpImageFilter.h>
51 #include <visp3/tt/vpTemplateTrackerHeader.h>
52 #include <visp3/tt/vpTemplateTrackerWarp.h>
53 #include <visp3/tt/vpTemplateTrackerZone.h>
54 
62 class VISP_EXPORT vpTemplateTracker
63 {
64 protected:
65  // traitement pyramidal
66  unsigned int nbLvlPyr; // If = 1, disable pyramidal usage
67  unsigned int l0Pyr;
68  bool pyrInitialised;
69 
71  vpTemplateTrackerPoint **ptTemplatePyr;
72  bool ptTemplateInit;
73  unsigned int templateSize;
74  unsigned int *templateSizePyr;
75  bool *ptTemplateSelect;
76  bool **ptTemplateSelectPyr;
77  bool ptTemplateSelectInit;
78  unsigned int templateSelectSize;
79 
80 #ifndef DOXYGEN_SHOULD_SKIP_THIS
81  vpTemplateTrackerPointSuppMIInv *ptTemplateSupp; // pour inverse et compo
82  vpTemplateTrackerPointSuppMIInv **ptTemplateSuppPyr; // pour inverse et
83  // compo
84 #endif
85 
86  vpTemplateTrackerPointCompo *ptTemplateCompo; // pour ESM
87  vpTemplateTrackerPointCompo **ptTemplateCompoPyr; // pour ESM
88  vpTemplateTrackerZone *zoneTracked;
89  vpTemplateTrackerZone *zoneTrackedPyr;
90 
91  vpImage<unsigned char> *pyr_IDes;
92 
94  vpMatrix Hdesire;
95  vpMatrix *HdesirePyr;
96  vpMatrix HLM;
97  vpMatrix HLMdesire;
98  vpMatrix *HLMdesirePyr;
99  vpMatrix HLMdesireInverse;
100  vpMatrix *HLMdesireInversePyr;
101  vpColVector G;
102 
103  double gain;
104  double thresholdGradient;
105  bool costFunctionVerification;
106  bool blur;
107  bool useBrent;
108  unsigned int nbIterBrent;
109  unsigned int taillef;
110  double *fgG;
111  double *fgdG;
112  double ratioPixelIn;
113  int mod_i;
114  int mod_j; // variable de sampling de zone de reference
115  unsigned int nbParam;
116  double lambdaDep;
117  unsigned int iterationMax;
118  // pour BFGS
119  unsigned int iterationGlobale;
120  // diverge is set to true if there is no more point in the tracked area
121  bool diverge;
122  unsigned int nbIteration;
123  bool useCompositionnal;
124  bool useInverse;
125 
126  vpTemplateTrackerWarp *Warp;
127  // Parametre de deplacement
129  vpColVector dp;
130 
131  // temporary values for warping
133  vpColVector X2;
134  // temporary derivative matrix
135  vpMatrix dW;
136 
140  vpTemplateTrackerZone zoneRef_; // Reference zone
141 
142  // private:
143  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
144  // vpTemplateTracker(const vpTemplateTracker &)
145  // : nbLvlPyr(0), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL),
146  // ptTemplatePyr(NULL),
147  // ptTemplateInit(false), templateSize(0), templateSizePyr(NULL),
148  // ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL),
149  // ptTemplateSelectInit(false), templateSelectSize(0),
150  // ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
151  // ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL),
152  // zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(),
153  // Hdesire(), HdesirePyr(NULL), HLM(), HLMdesire(),
154  // HLMdesirePyr(NULL), HLMdesireInverse(), HLMdesireInversePyr(NULL),
155  // G(), gain(0), thresholdGradient(0),
156  // costFunctionVerification(false), blur(false), useBrent(false),
157  // nbIterBrent(0), taillef(0), fgG(NULL), fgdG(NULL),
158  // ratioPixelIn(0), mod_i(0), mod_j(0), nbParam(), lambdaDep(0),
159  // iterationMax(0), iterationGlobale(0), diverge(false),
160  // nbIteration(0), useCompositionnal(false), useInverse(false),
161  // Warp(NULL), p(), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(),
162  // zoneRef_()
163  // {
164  // throw vpException(vpException::functionNotImplementedError, "Not
165  // implemented!");
166  // }
167  // vpTemplateTracker &operator=(const vpTemplateTracker &){
168  // throw vpException(vpException::functionNotImplementedError, "Not
169  // implemented!"); return *this;
170  // }
171  //#endif
172 
173 public:
176  : nbLvlPyr(0), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL), ptTemplateInit(false),
177  templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL),
178  ptTemplateSelectInit(false), templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
179  ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(),
180  Hdesire(), HdesirePyr(NULL), HLM(), HLMdesire(), HLMdesirePyr(NULL), HLMdesireInverse(),
181  HLMdesireInversePyr(NULL), G(), gain(0), thresholdGradient(0), costFunctionVerification(false), blur(false),
182  useBrent(false), nbIterBrent(0), taillef(0), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(0), mod_j(0),
183  nbParam(), lambdaDep(0), iterationMax(0), iterationGlobale(0), diverge(false), nbIteration(0),
184  useCompositionnal(false), useInverse(false), Warp(NULL), p(), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(),
185  zoneRef_()
186  {
187  }
188  explicit vpTemplateTracker(vpTemplateTrackerWarp *_warp);
189  virtual ~vpTemplateTracker();
190 
191  void display(const vpImage<unsigned char> &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3);
192  void display(const vpImage<vpRGBa> &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3);
193 
194  bool getDiverge() const { return diverge; }
195  vpColVector getdp() { return dp; }
196  vpColVector getG() const { return G; }
197  vpMatrix getH() const { return H; }
198  unsigned int getNbParam() const { return nbParam; }
199  unsigned int getNbIteration() const { return nbIteration; }
200  vpColVector getp() const { return p; }
201  double getRatioPixelIn() const { return ratioPixelIn; }
202 
207  vpTemplateTrackerWarp *getWarp() const { return Warp; }
208 
212  vpTemplateTrackerZone getZoneRef() const { return zoneRef_; }
213 
214  void initClick(const vpImage<unsigned char> &I, bool delaunay = false);
215  void initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &v_ip, bool delaunay = false);
216  void initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone &zone);
217 
218  void resetTracker();
219 
220  void setBlur(bool b) { blur = b; }
221  void setCostFunctionVerification(bool b) { costFunctionVerification = b; }
222  void setGain(double g) { gain = g; }
223  void setGaussianFilterSize(unsigned int new_taill);
224  void setHDes(vpMatrix &tH)
225  {
226  Hdesire = tH;
227  vpMatrix::computeHLM(Hdesire, lambdaDep, HLMdesire);
228  HLMdesireInverse = HLMdesire.inverseByLU();
229  }
235  void setIterationMax(const unsigned int &n) { iterationMax = n; }
240  void setLambda(double l) { lambdaDep = l; }
241  void setNbIterBrent(const unsigned int &b) { nbIterBrent = b; }
242  void setp(const vpColVector &tp)
243  {
244  p = tp;
245  diverge = false;
246  iterationGlobale = 0;
247  }
256  void setPyramidal(unsigned int nlevels = 2, unsigned int level_to_stop = 1)
257  {
258  nbLvlPyr = nlevels;
259  l0Pyr = level_to_stop;
260  if (l0Pyr >= nlevels) {
261  std::cout << "Warning: level_to_stop: " << level_to_stop << " higher than level_to_start: " << nlevels - 1
262  << " (nlevels-1)" << std::endl;
263  std::cout << "Level to stop put to: " << nlevels - 1 << std::endl;
264  l0Pyr = nlevels - 1;
265  }
266  }
275  void setSampling(int sample_i, int sample_j)
276  {
277  mod_i = sample_i;
278  mod_j = sample_j;
279  }
280  void setThresholdGradient(double threshold) { thresholdGradient = threshold; }
282  void setUseBrent(bool b) { useBrent = b; }
283 
284  void track(const vpImage<unsigned char> &I);
285  void trackRobust(const vpImage<unsigned char> &I);
286 
287 protected:
288  void computeOptimalBrentGain(const vpImage<unsigned char> &I, vpColVector &tp, double tMI, vpColVector &direction,
289  double &alpha);
290  virtual double getCost(const vpImage<unsigned char> &I, const vpColVector &tp) = 0;
291  void getGaussianBluredImage(const vpImage<unsigned char> &I) { vpImageFilter::filter(I, BI, fgG, taillef); }
292  virtual void initHessienDesired(const vpImage<unsigned char> &I) = 0;
293  virtual void initHessienDesiredPyr(const vpImage<unsigned char> &I);
294  virtual void initPyramidal(unsigned int nbLvl, unsigned int l0);
295  void initTracking(const vpImage<unsigned char> &I, vpTemplateTrackerZone &zone);
296  virtual void initTrackingPyr(const vpImage<unsigned char> &I, vpTemplateTrackerZone &zone);
297  virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
298  virtual void trackPyr(const vpImage<unsigned char> &I);
299 };
300 #endif
vpTemplateTrackerZone
Definition: vpTemplateTrackerZone.h:61
vpTemplateTrackerWarp
Definition: vpTemplateTrackerWarp.h:57
vpImageFilter::filter
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
Definition: vpImageFilter.cpp:125
vpTemplateTrackerPoint
Definition: vpTemplateTrackerHeader.h:71
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:71
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:103
vpColor::green
static const vpColor green
Definition: vpColor.h:182
vpMatrix::inverseByLU
vpMatrix inverseByLU() const
Definition: vpMatrix_lu.cpp:129
vpTemplateTrackerPointCompo
Definition: vpTemplateTrackerHeader.h:84
vpMatrix::computeHLM
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5070
vpImage< unsigned char >
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
vpTemplateTracker
Definition: vpTemplateTracker.h:61