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