Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpTemplateTrackerZNCC.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Template tracker.
32  *
33  * Authors:
34  * Amaury Dame
35  * Aurelien Yol
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 #include <visp3/tt/vpTemplateTrackerZNCC.h>
40 
42  : vpTemplateTracker(warp), DI(), temp()
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 
56 
58 {
59  double IW,Tij;
60  int i,j;
61  double i2,j2;
62  int Nbpoint=0;
63 
64  Warp->computeCoeff(tp);
65 
66  double moyTij=0;
67  double moyIW=0;
68  for(unsigned int point=0;point<templateSize;point++)
69  {
70  i=ptTemplate[point].y;
71  j=ptTemplate[point].x;
72  X1[0]=j;X1[1]=i;
73  Warp->computeDenom(X1,tp);
74  Warp->warpX(X1,X2,tp);
75 
76  j2=X2[0];i2=X2[1];
77  if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
78  {
79  Tij=ptTemplate[point].val;
80  if(!blur)
81  IW=I.getValue(i2,j2);
82  else
83  IW=BI.getValue(i2,j2);
84  //IW=getSubPixBspline4(I,i2,j2);
85  moyTij+=Tij;
86  moyIW+=IW;
87  Nbpoint++;
88  }
89  }
90  ratioPixelIn=(double)Nbpoint/(double)templateSize;
91  if(! Nbpoint) {
93  "Cannot get cost: size = 0")) ;
94  }
95 
96  moyTij=moyTij/Nbpoint;
97  moyIW=moyIW/Nbpoint;
98 
99  double nom=0;//,denom=0;
100  double var1=0,var2=0;
101  for(unsigned int point=0;point<templateSize;point++)
102  {
103  i=ptTemplate[point].y;
104  j=ptTemplate[point].x;
105  X1[0]=j;X1[1]=i;
106  Warp->computeDenom(X1,tp);
107  Warp->warpX(X1,X2,tp);
108 
109  j2=X2[0];i2=X2[1];
110  if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
111  {
112  Tij=ptTemplate[point].val;
113  if(!blur)
114  IW=I.getValue(i2,j2);
115  else
116  IW=BI.getValue(i2,j2);
117  //IW=getSubPixBspline4(I,i2,j2);
118  nom+=(Tij-moyTij)*(IW-moyIW);
119  //denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
120  var1+=(IW-moyIW)*(IW-moyIW);
121  var2+=(Tij-moyTij)*(Tij-moyTij);
122 
123  Nbpoint++;
124  }
125  }
126  // if(Nbpoint==0)return 10e10; // cannot occur
127  //return -nom/sqrt(denom);
128  return -nom/sqrt(var1*var2);
129 }
130 
131 
unsigned int getWidth() const
Definition: vpImage.h:226
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
vpTemplateTrackerPoint * ptTemplate
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
error that can be emited by ViSP classes.
Definition: vpException.h:73
Type getValue(double i, double j) const
Definition: vpImage.h:1477
vpImage< double > BI
vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp)
unsigned int templateSize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpRowVector.h:205
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
unsigned int getHeight() const
Definition: vpImage.h:175
vpTemplateTrackerWarp * Warp
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225