ViSP  2.10.0
vpTemplateTrackerSSDForwardCompositional.cpp
1 /****************************************************************************
2  *
3  * $Id: vpTemplateTrackerSSDForwardCompositional.cpp 5264 2015-02-04 13:49:55Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Template tracker.
35  *
36  * Authors:
37  * Amaury Dame
38  * Aurelien Yol
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 #include <visp/vpTemplateTrackerSSDForwardCompositional.h>
43 #include <visp/vpImageFilter.h>
44 
46  : vpTemplateTrackerSSD(warp), compoInitialised(false)
47 {
48 }
49 
51 {
52  // std::cout<<"Initialise precomputed value of Compositionnal Direct"<<std::endl;
53  int i,j;
54  for(unsigned int point=0;point<templateSize;point++)
55  {
56  i=ptTemplate[point].y;
57  j=ptTemplate[point].x;
58  ptTemplate[point].dW=new double[2*nbParam];
59  X1[0]=j;X1[1]=i;
60  Warp->computeDenom(X1,p);
61  Warp->getdWdp0(i,j,ptTemplate[point].dW);
62 
63  }
64  compoInitialised=true;
65 }
66 
68 {
69  initCompo(I);
70 }
71 
73 {
74  if(!compoInitialised)
75  std::cout<<"Compositionnal tracking no initialised\nUse InitCompo(vpImage<unsigned char> &I) function"<<std::endl;
76  double erreur=0;
77  unsigned int Nbpoint=0;
78 
79  if(blur)
83 
84  dW=0;
85 
86  double lambda=lambdaDep;
87  double IW,dIWx,dIWy;
88  double Tij;
89  unsigned int iteration=0;
90  int i,j;
91  double i2,j2;
92  double alpha=2.;
93  do
94  {
95  Nbpoint=0;
96  erreur=0;
97  G=0;
98  H=0 ;
99  Warp->computeCoeff(p);
100  for(unsigned int point=0;point<templateSize;point++)
101  {
102  i=ptTemplate[point].y;
103  j=ptTemplate[point].x;
104  X1[0]=j;X1[1]=i;
105 
106  Warp->computeDenom(X1,p);
107  Warp->warpX(X1,X2,p);
108 
109  j2=X2[0];i2=X2[1];
110  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
111  {
112  Tij=ptTemplate[point].val;
113  if(!blur)
114  IW=I.getValue(i2,j2);
115  else
116  IW=BI.getValue(i2,j2);
117  dIWx=dIx.getValue(i2,j2);
118  dIWy=dIy.getValue(i2,j2);
119  Nbpoint++;
120  //Calcul du Hessien
121  /*Warp->dWarp(X1,X2,p,dW);
122  double *tempt=new double[nbParam];
123  for(int it=0;it<nbParam;it++)
124  tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;*/
125 
126  Warp->dWarpCompo(X1,X2,p,ptTemplate[point].dW,dW);
127 
128  double *tempt=new double[nbParam];
129  for(unsigned int it=0;it<nbParam;it++)
130  tempt[it] =dW[0][it]*dIWx+dW[1][it]*dIWy;
131 
132  for(unsigned int it=0;it<nbParam;it++)
133  for(unsigned int jt=0;jt<nbParam;jt++)
134  H[it][jt]+=tempt[it]*tempt[jt];
135 
136  double er=(Tij-IW);
137  for(unsigned int it=0;it<nbParam;it++)
138  G[it]+=er*tempt[it];
139 
140  erreur+=(er*er);
141  delete[] tempt;
142  }
143 
144 
145  }
146  if(Nbpoint==0) {
147  //std::cout<<"plus de point dans template suivi"<<std::endl;
148  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
149  }
150 
151  vpMatrix::computeHLM(H,lambda,HLM);
152 
153  try
154  {
155  dp=1.*HLM.inverseByLU()*G;
156  }
157  catch(vpException &e)
158  {
159  //std::cout<<"probleme inversion"<<std::endl;
160  throw(e);
161  }
162 
163  dp=gain*dp;
164  if(useBrent)
165  {
166  alpha=2.;
167  computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
168  dp=alpha*dp;
169  }
170  Warp->pRondp(p,dp,p);
171  //p+=Gain*dp;
172  iteration++;
173  }
174  while( /*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax));
175 
176  nbIteration=iteration;
177 }
178 
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW)=0
unsigned int getWidth() const
Definition: vpImage.h:161
vpTemplateTrackerPoint * ptTemplate
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
error that can be emited by ViSP classes.
Definition: vpException.h:76
Type getValue(double i, double j) const
Definition: vpImage.h:1068
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
vpImage< double > BI
unsigned int templateSize
unsigned int iterationMax
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const =0
Error that can be emited by the vpTracker class and its derivates.
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M)
vpImage< double > dIx
vpImage< double > dIy
unsigned int nbIteration
vpMatrix inverseByLU() const
unsigned int getHeight() const
Definition: vpImage.h:152
virtual void getdWdp0(const int &i, const int &j, double *dIdW)=0
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:4320