Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpTemplateTrackerWarp.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/vpTemplateTrackerWarp.h>
40 
42 {
43  if (p.size() < 2) {
44  vpCTRACE << "Bad template tracker warp parameters dimension. Should never occur. " << std::endl;
45  throw(vpException(vpException::dimensionError, "Bad template tracker warp parameters dimension"));
46  }
47  vpColVector S1(2),S2(2),S3(2);
48  vpColVector rS1(2),rS2(2),rS3(2);
49  in.getCorners(S1,S2,S3);
50  computeDenom(S1,p);
51  warpX(S1,rS1,p);
52  computeDenom(S2,p);
53  warpX(S2,rS2,p);
54  computeDenom(S3,p);
55  warpX(S3,rS3,p);
56  out.init(rS1,rS2,rS3);
57 }
59 {
61  out.clear();
62  for(unsigned int i=0;i<in.getNbTriangle();i++)
63  {
64  in.getTriangle(i,TR);
65  warpTriangle(TR,p,TT);
66  out.add(TT);
67  }
68 }
69 
71 {
72  unsigned int nb_corners = Z.getNbTriangle() * 3;
73  computeCoeff(p);
74  vpColVector X1(2),X2(2);
75 
76  double res=0;
78  for(unsigned int i=0;i<Z.getNbTriangle();i++)
79  {
80  Z.getTriangle(i, triangle);
81  for (unsigned int j=0; j<3; j++) {
82  triangle.getCorner(j, X1[0], X1[1]);
83 
84  computeDenom(X1,p);
85  warpX(X1,X2,p);
86  res+=sqrt((X2[0]-X1[0])*(X2[0]-X1[0])+(X2[1]-X1[1])*(X2[1]-X1[1]));
87  }
88  }
89 
90  return res/nb_corners;
91 }
92 
93 void vpTemplateTrackerWarp::warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v)
94 {
95  computeCoeff(p);
96  vpColVector X1(2),X2(2);
97  for(int i=0;i<nb_pt;i++)
98  {
99  X1[0]=ut0[i];
100  X1[1]=vt0[i];
101  computeDenom(X1,p);
102  warpX(X1,X2,p);
103  u[i]=X2[0];
104  v[i]=X2[1];
105  //std::cout<<"warp "<<X2[0]<<","<<X2[1]<<std::endl;
106  }
107 }
108 
109 #ifndef DOXYGEN_SHOULD_SKIP_THIS
110 void vpTemplateTrackerWarp::findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p)
111 {
112  vpMatrix dW_(2,nbParam);
113  vpMatrix dX(2,1);
115  vpMatrix G(nbParam,1);
116 
117  int cpt=0;
118  vpColVector X1(2);
119  vpColVector fX1(2);
120  vpColVector X2(2);
121  double erreur=0;
122  double erreur_prec;
123  double lambda=0.01;
124  do
125  {
126  erreur_prec=erreur;
127  H=0;
128  G=0;
129  erreur=0;
130  computeCoeff(p);
131  for(int i=0;i<nb_pt;i++)
132  {
133  X1[0]=ut0[i];
134  X1[1]=vt0[i];
135  computeDenom(X1,p);
136  warpX(X1,fX1,p);
137  dWarp(X1,fX1,p,dW_);
138  H+=dW_.AtA();
139 
140  X2[0]=u[i];
141  X2[1]=v[i];
142 
143  dX=X2-fX1;
144  G+=dW_.t()*dX;
145 
146  erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
147 
148  }
149 
150  vpMatrix::computeHLM(H, lambda, HLM);
151  try{
152  p+=(vpColVector)(HLM.inverseByLU()*G, 0u);
153  }
154  catch(vpException &e) {
155  //std::cout<<"Cannot inverse the matrix by LU " << std::endl;
156  throw(e);
157  }
158  cpt++;
159  }
160  while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
161  //std::cout<<"erreur apres transformation="<<erreur<<std::endl;
162 }
163 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
164 
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
void warpTriangle(const vpTemplateTrackerTriangle &in, const vpColVector &p, vpTemplateTrackerTriangle &out)
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
double getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p)
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpColVector getCorner(unsigned int i) const
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:156
void getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) const
void add(const vpTemplateTrackerTriangle &t)
unsigned int getNbTriangle() const
void warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out)
void init(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3)
#define vpCTRACE
Definition: vpDebug.h:337
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:3525
void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v)
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)=0