Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpTemplateTrackerSSD.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  * Template tracker.
33  *
34  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  *
38 *****************************************************************************/
39 
40 #include <visp3/tt/vpTemplateTrackerSSD.h>
41 
43 {
44  dW.resize(2, nbParam);
45  G.resize(nbParam);
48 
50 
51  X1.resize(2);
52  X2.resize(2);
53  DI.resize(2);
54 }
55 
57 {
58  double erreur = 0;
59  double IW;
60  int Nbpoint = 0;
61 
62  Warp->computeCoeff(tp);
63  for (unsigned int point = 0; point < templateSize; point++) {
64  int i = ptTemplate[point].y;
65  int j = ptTemplate[point].x;
66  X1[0] = j;
67  X1[1] = i;
68  Warp->computeDenom(X1, tp);
69  Warp->warpX(X1, X2, tp);
70 
71  double j2 = X2[0];
72  double i2 = X2[1];
73  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
74  double Tij = ptTemplate[point].val;
75  if (!blur)
76  IW = I.getValue(i2, j2);
77  else
78  IW = BI.getValue(i2, j2);
79  erreur += (Tij - IW) * (Tij - IW);
80  Nbpoint++;
81  }
82  }
83  ratioPixelIn = static_cast<double>(Nbpoint) / static_cast<double>(templateSize);
84 
85  if (Nbpoint == 0)
86  return 10e10;
87  return erreur / Nbpoint;
88 }
89 
91 {
92  double erreur = 0;
93  double IW;
94  unsigned int Nbpoint = 0;
95 
96  if (pyrInitialised) {
99  }
100 
101  Warp->computeCoeff(tp);
102  for (unsigned int point = 0; point < templateSize; point++) {
103  int i = ptTemplate[point].y;
104  int j = ptTemplate[point].x;
105  X1[0] = j;
106  X1[1] = i;
107  Warp->computeDenom(X1, tp);
108  Warp->warpX(X1, X2, tp);
109 
110  double j2 = X2[0];
111  double i2 = X2[1];
112  if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) {
113  double Tij = ptTemplate[point].val;
114  IW = I.getValue(i2, j2);
115  erreur += (Tij - IW) * (Tij - IW);
116  Nbpoint++;
117  }
118  }
119  if (Nbpoint == 0)
120  return 10e10;
121  return erreur / Nbpoint;
122 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
unsigned int getWidth() const
Definition: vpImage.h:242
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1567
unsigned int getHeight() const
Definition: vpImage.h:184
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:259
double getSSD(const vpImage< unsigned char > &I, const vpColVector &tp)
vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
vpTemplateTrackerPoint ** ptTemplatePyr
unsigned int * templateSizePyr
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
vpImage< double > BI
unsigned int templateSize