Visual Servoing Platform version 3.5.0
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
95void 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
112void 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
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
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:338