Visual Servoing Platform  version 3.5.1 under development (2022-08-17)
vpTemplateTrackerWarpSRT.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/vpTemplateTrackerWarpSRT.h>
41 
46 
54 {
55  p_down[0] = p[0];
56  p_down[1] = p[1];
57  p_down[2] = p[2] / 2.;
58  p_down[3] = p[3] / 2.;
59 }
60 
68 {
69  p_up[0] = p[0];
70  p_up[1] = p[1];
71  p_up[2] = p[2] * 2.;
72  p_up[3] = p[3] * 2.;
73 }
74 
83 void vpTemplateTrackerWarpSRT::getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
84 {
85  dIdW[0] = u * du + v * dv;
86  dIdW[1] = -v * du + u * dv;
87  dIdW[2] = du;
88  dIdW[3] = dv;
89 }
90 
102 void vpTemplateTrackerWarpSRT::getdWdp0(const int &v, const int &u, double *dIdW)
103 {
104  dIdW[0] = u;
105  dIdW[1] = -v;
106  dIdW[2] = 1.;
107  dIdW[3] = 0;
108 
109  dIdW[4] = v;
110  dIdW[5] = u;
111  dIdW[6] = 0;
112  dIdW[7] = 1.;
113 }
114 
124 void vpTemplateTrackerWarpSRT::warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)
125 {
126  double c = cos(p[1]);
127  double s = sin(p[1]);
128  double scale = 1.0 + p[0];
129 
130  u2 = scale * (c * u1 - s * v1) + p[2];
131  v2 = scale * (s * u1 + c * v1) + p[3];
132 }
133 
142 {
143  double c = cos(p[1]);
144  double s = sin(p[1]);
145  double scale = 1.0 + p[0];
146 
147  X2[0] = scale * (c * X1[0] - s * X1[1]) + p[2];
148  X2[1] = scale * (s * X1[0] + c * X1[1]) + p[3];
149 }
150 
162 {
163  double u = X[0];
164  double v = X[1];
165  double c = cos(p[1]);
166  double s = sin(p[1]);
167  double scale = 1.0 + p[0];
168  double c_u_s_v = c * u - s * v;
169  double s_u_c_v = s * u + c * v;
170 
171  dM[0][0] = c_u_s_v;
172  dM[0][1] = -scale * s_u_c_v;
173  dM[0][2] = 1;
174  dM[0][3] = 0;
175 
176  dM[1][0] = s_u_c_v;
177  dM[1][1] = scale * c_u_s_v;
178  dM[1][2] = 0;
179  dM[1][3] = 1;
180 }
181 
190  const double *dwdp0, vpMatrix &dM)
191 {
192  double c = cos(p[1]);
193  double s = sin(p[1]);
194  double scale = 1.0 + p[0];
195 
196  for (unsigned int i = 0; i < nbParam; i++) {
197  dM[0][i] = scale * (c * dwdp0[i] - s * dwdp0[i + nbParam]);
198  dM[1][i] = scale * (s * dwdp0[i] + c * dwdp0[i + nbParam]);
199  }
200 }
201 
210 {
211  double c = cos(p[1]);
212  double s = sin(p[1]);
213  double scale = 1.0 + p[0];
214 
215  X2[0] = scale * (c * X1[0] - s * X1[1]) + p[2];
216  X2[1] = scale * (s * X1[0] + c * X1[1]) + p[3];
217 }
218 
226 {
227  double c = cos(p[1]);
228  double s = sin(p[1]);
229  double scale = 1.0 + p[0];
230  double u = p[2];
231  double v = p[3];
232 
233  p_inv[0] = 1.0 / scale - 1.0;
234  p_inv[1] = atan2(-s, c);
235  p_inv[2] = -(c * u + s * v) / scale;
236  p_inv[3] = (s * u - c * v) / scale;
237 }
238 
248 {
249  double c1 = cos(p1[1]);
250  double s1 = sin(p1[1]);
251  double c2 = cos(p2[1]);
252  double s2 = sin(p2[1]);
253  double scale1 = 1.0 + p1[0];
254  double scale2 = 1.0 + p2[0];
255  double u1 = p1[2];
256  double v1 = p1[3];
257  double u2 = p2[2];
258  double v2 = p2[3];
259 
260  p12[0] = scale1 * scale2 - 1.0;
261  p12[1] = atan2(s1 * c2 + c1 * s2, c1 * c2 - s1 * s2);
262  p12[2] = scale1 * (c1 * u2 - s1 * v2) + u1;
263  p12[3] = scale1 * (s1 * u2 + c1 * v2) + v1;
264 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void getdWdp0(const int &v, const int &u, double *dIdW)
void getParamPyramidDown(const vpColVector &p, vpColVector &p_down)
void warpX(const vpColVector &X1, vpColVector &X2, const vpColVector &p)
void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
void dWarp(const vpColVector &X, const vpColVector &, const vpColVector &p, vpMatrix &dM)
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const
void warpXInv(const vpColVector &X1, vpColVector &X2, const vpColVector &p)
void getParamInverse(const vpColVector &p, vpColVector &p_inv) const
void dWarpCompo(const vpColVector &, const vpColVector &, const vpColVector &p, const double *dwdp0, vpMatrix &dM)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void getParamPyramidUp(const vpColVector &p, vpColVector &p_up)
unsigned int nbParam
Number of parameters used to model warp transformation.