Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpTemplateTrackerSSD.cpp
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  *****************************************************************************/
40 
41 #include <visp3/tt/vpTemplateTrackerSSD.h>
42 
44 {
45  dW.resize(2, nbParam);
46  G.resize(nbParam);
49 
51 
52  X1.resize(2);
53  X2.resize(2);
54  DI.resize(2);
55 }
56 
58 {
59  double erreur = 0;
60  double IW;
61  int Nbpoint = 0;
62 
63  Warp->computeCoeff(tp);
64  for (unsigned int point = 0; point < templateSize; point++) {
65  int i = ptTemplate[point].y;
66  int j = ptTemplate[point].x;
67  X1[0] = j;
68  X1[1] = i;
69  Warp->computeDenom(X1, tp);
70  Warp->warpX(X1, X2, tp);
71 
72  double j2 = X2[0];
73  double i2 = X2[1];
74  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
75  double Tij = ptTemplate[point].val;
76  if (!blur)
77  IW = I.getValue(i2, j2);
78  else
79  IW = BI.getValue(i2, j2);
80  // IW=getSubPixBspline4(I,i2,j2);
81  erreur += ((double)Tij - IW) * ((double)Tij - IW);
82  Nbpoint++;
83  }
84  }
85  ratioPixelIn = (double)Nbpoint / (double)templateSize;
86 
87  if (Nbpoint == 0)
88  return 10e10;
89  return erreur / Nbpoint;
90 }
91 
93 {
94  double erreur = 0;
95  double IW;
96  unsigned int Nbpoint = 0;
97 
98  if (pyrInitialised) {
101  }
102 
103  Warp->computeCoeff(tp);
104  for (unsigned int point = 0; point < templateSize; point++) {
105  int i = ptTemplate[point].y;
106  int j = ptTemplate[point].x;
107  X1[0] = j;
108  X1[1] = i;
109  Warp->computeDenom(X1, tp);
110  Warp->warpX(X1, X2, tp);
111 
112  double j2 = X2[0];
113  double i2 = X2[1];
114  if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) {
115  double Tij = ptTemplate[point].val;
116  IW = I.getValue(i2, j2);
117  // IW=getSubPixBspline4(I,i2,j2);
118  erreur += ((double)Tij - IW) * ((double)Tij - IW);
119  Nbpoint++;
120  }
121  }
122  if (Nbpoint == 0)
123  return 10e10;
124  return erreur / Nbpoint;
125 }
unsigned int getWidth() const
Definition: vpImage.h:239
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerPoint ** ptTemplatePyr
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
vpImage< double > BI
unsigned int templateSize
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1420
double getSSD(const vpImage< unsigned char > &I, const vpColVector &tp)
vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpRowVector.h:213
unsigned int * templateSizePyr
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
unsigned int getHeight() const
Definition: vpImage.h:178
vpTemplateTrackerWarp * Warp
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244