Visual Servoing Platform  version 3.2.1 under development (2019-10-21) under development (2019-10-21)
vpTemplateTrackerZNCC.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 #include <visp3/tt/vpTemplateTrackerZNCC.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 IW, Tij;
59  int i, j;
60  double i2, j2;
61  int Nbpoint = 0;
62 
63  Warp->computeCoeff(tp);
64 
65  double moyTij = 0;
66  double moyIW = 0;
67  for (unsigned int point = 0; point < templateSize; point++) {
68  i = ptTemplate[point].y;
69  j = ptTemplate[point].x;
70  X1[0] = j;
71  X1[1] = i;
72  Warp->computeDenom(X1, tp);
73  Warp->warpX(X1, X2, tp);
74 
75  j2 = X2[0];
76  i2 = X2[1];
77  if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) {
78  Tij = ptTemplate[point].val;
79  if (!blur)
80  IW = I.getValue(i2, j2);
81  else
82  IW = BI.getValue(i2, j2);
83  // IW=getSubPixBspline4(I,i2,j2);
84  moyTij += Tij;
85  moyIW += IW;
86  Nbpoint++;
87  }
88  }
89  ratioPixelIn = (double)Nbpoint / (double)templateSize;
90  if (!Nbpoint) {
91  throw(vpException(vpException::divideByZeroError, "Cannot get cost: size = 0"));
92  }
93 
94  moyTij = moyTij / Nbpoint;
95  moyIW = moyIW / Nbpoint;
96 
97  double nom = 0; //,denom=0;
98  double var1 = 0, var2 = 0;
99  for (unsigned int point = 0; point < templateSize; point++) {
100  i = ptTemplate[point].y;
101  j = ptTemplate[point].x;
102  X1[0] = j;
103  X1[1] = i;
104  Warp->computeDenom(X1, tp);
105  Warp->warpX(X1, X2, tp);
106 
107  j2 = X2[0];
108  i2 = X2[1];
109  if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) {
110  Tij = ptTemplate[point].val;
111  if (!blur)
112  IW = I.getValue(i2, j2);
113  else
114  IW = BI.getValue(i2, j2);
115  // IW=getSubPixBspline4(I,i2,j2);
116  nom += (Tij - moyTij) * (IW - moyIW);
117  // denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
118  var1 += (IW - moyIW) * (IW - moyIW);
119  var2 += (Tij - moyTij) * (Tij - moyTij);
120 
121  Nbpoint++;
122  }
123  }
124  // if(Nbpoint==0)return 10e10; // cannot occur
125  // return -nom/sqrt(denom);
126  return -nom / sqrt(var1 * var2);
127 }
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1422
vpTemplateTrackerPoint * ptTemplate
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:305
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpImage< double > BI
vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp)
unsigned int templateSize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpRowVector.h:271
unsigned int getHeight() const
Definition: vpImage.h:186
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpTemplateTrackerWarp * Warp
unsigned int getWidth() const
Definition: vpImage.h:244
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310