Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
vpTemplateTrackerWarp.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  *
38 *****************************************************************************/
39 #include <visp3/tt/vpTemplateTrackerWarp.h>
40 
43 {
44  if (p.size() < 2) {
45  vpCTRACE << "Bad template tracker warp parameters dimension. Should "
46  "never occur. "
47  << std::endl;
48  throw(vpException(vpException::dimensionError, "Bad template tracker warp parameters dimension"));
49  }
50  vpColVector S1(2), S2(2), S3(2);
51  vpColVector rS1(2), rS2(2), rS3(2);
52  in.getCorners(S1, S2, S3);
53  computeDenom(S1, p);
54  warpX(S1, rS1, p);
55  computeDenom(S2, p);
56  warpX(S2, rS2, p);
57  computeDenom(S3, p);
58  warpX(S3, rS3, p);
59  out.init(rS1, rS2, rS3);
60 }
62 {
64  out.clear();
65  for (unsigned int i = 0; i < in.getNbTriangle(); i++) {
66  in.getTriangle(i, TR);
67  warpTriangle(TR, p, TT);
68  out.add(TT);
69  }
70 }
71 
73 {
74  unsigned int nb_corners = Z.getNbTriangle() * 3;
75  computeCoeff(p);
76  vpColVector X1(2), X2(2);
77 
78  double res = 0;
80  for (unsigned int i = 0; i < Z.getNbTriangle(); i++) {
81  Z.getTriangle(i, triangle);
82  for (unsigned int j = 0; j < 3; j++) {
83  triangle.getCorner(j, X1[0], X1[1]);
84 
85  computeDenom(X1, p);
86  warpX(X1, X2, p);
87  res += sqrt((X2[0] - X1[0]) * (X2[0] - X1[0]) + (X2[1] - X1[1]) * (X2[1] - X1[1]));
88  }
89  }
90 
91  return res / nb_corners;
92 }
93 
94 void vpTemplateTrackerWarp::warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u,
95  double *v)
96 {
97  computeCoeff(p);
98  vpColVector X1(2), X2(2);
99  for (int i = 0; i < nb_pt; i++) {
100  X1[0] = ut0[i];
101  X1[1] = vt0[i];
102  computeDenom(X1, p);
103  warpX(X1, X2, p);
104  u[i] = X2[0];
105  v[i] = X2[1];
106  // std::cout<<"warp "<<X2[0]<<","<<X2[1]<<std::endl;
107  }
108 }
109 
110 #ifndef DOXYGEN_SHOULD_SKIP_THIS
111 void vpTemplateTrackerWarp::findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt,
112  vpColVector &p)
113 {
114  vpMatrix dW_(2, nbParam);
115  vpMatrix dX(2, 1);
117  vpMatrix G(nbParam, 1);
118 
119  int cpt = 0;
120  vpColVector X1(2);
121  vpColVector fX1(2);
122  vpColVector X2(2);
123  double erreur = 0;
124  double erreur_prec;
125  double lambda = 0.01;
126  do {
127  erreur_prec = erreur;
128  H = 0;
129  G = 0;
130  erreur = 0;
131  computeCoeff(p);
132  for (int i = 0; i < nb_pt; i++) {
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  vpMatrix::computeHLM(H, lambda, HLM);
150  try {
151  p += (vpColVector)(HLM.inverseByLU() * G, 0u);
152  } catch (const vpException &e) {
153  // std::cout<<"Cannot inverse the matrix by LU " << std::endl;
154  throw(e);
155  }
156  cpt++;
157  } while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
158  // std::cout<<"erreur apres transformation="<<erreur<<std::endl;
159 }
160 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6407
vpColVector getCorner(unsigned int i) const
void init(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3)
void getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) const
unsigned int nbParam
Number of parameters used to model warp transformation.
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, vpMatrix &dM)=0
void warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out)
double getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p)
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
void warpTriangle(const vpTemplateTrackerTriangle &in, const vpColVector &p, vpTemplateTrackerTriangle &out)
void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v)
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
unsigned int getNbTriangle() const
void add(const vpTemplateTrackerTriangle &t)
#define vpCTRACE
Definition: vpDebug.h:329