Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
94void 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
111void 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:292
Implementation of column vector and the associated operations.
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:152
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
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:333