Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpTemplateTrackerWarpHomography.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/core/vpTrackingException.h>
40#include <visp3/tt/vpTemplateTrackerWarpHomography.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];
59 p_down[4] = p[4];
60 p_down[5] = p[5] * 2.;
61 p_down[6] = p[6] / 2.;
62 p_down[7] = p[7] / 2.;
63}
64
72{
73 p_up[0] = p[0];
74 p_up[1] = p[1];
75 p_up[2] = p[2] / 2.;
76 p_up[3] = p[3];
77 p_up[4] = p[4];
78 p_up[5] = p[5] / 2.;
79 p_up[6] = p[6] * 2.;
80 p_up[7] = p[7] * 2.;
81}
82
91void vpTemplateTrackerWarpHomography::getdW0(const int &v, const int &u, const double &dv, const double &du,
92 double *dIdW)
93{
94 double u_du_ = u * du;
95 double v_dv_ = v * dv;
96 dIdW[0] = u_du_;
97 dIdW[1] = u * dv;
98 dIdW[2] = -u * (u_du_ + v_dv_);
99 dIdW[3] = v * du;
100 dIdW[4] = v_dv_;
101 dIdW[5] = -v * (u_du_ + v_dv_);
102 dIdW[6] = du;
103 dIdW[7] = dv;
104}
105
117void vpTemplateTrackerWarpHomography::getdWdp0(const int &v, const int &u, double *dIdW)
118{
119 double uv_ = u * v;
120 dIdW[0] = u;
121 dIdW[1] = 0;
122 dIdW[2] = -u * u;
123 dIdW[3] = v;
124 dIdW[4] = 0;
125 dIdW[5] = -uv_;
126 dIdW[6] = 1.;
127 dIdW[7] = 0;
128
129 dIdW[8] = 0;
130 dIdW[9] = u;
131 dIdW[10] = -uv_;
132 dIdW[11] = 0;
133 dIdW[12] = v;
134 dIdW[13] = -v * v;
135 dIdW[14] = 0;
136 dIdW[15] = 1.;
137}
138
149{
150 double value = (p[2] * X[0] + p[5] * X[1] + 1.);
151
152 if (std::fabs(value) > std::numeric_limits<double>::epsilon()) {
153 denom = (1. / value);
154 } else {
156 "Division by zero in vpTemplateTrackerWarpHomography::computeDenom()"));
157 }
158}
159
169void vpTemplateTrackerWarpHomography::warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)
170{
171 u2 = ((1. + p[0]) * u1 + p[3] * v1 + p[6]) * denom;
172 v2 = (p[1] * u1 + (1. + p[4]) * v1 + p[7]) * denom;
173}
174
185{
186 X2[0] = ((1 + p[0]) * X1[0] + p[3] * X1[1] + p[6]) * denom;
187 X2[1] = (p[1] * X1[0] + (1 + p[4]) * X1[1] + p[7]) * denom;
188}
189
202 vpMatrix &dM)
203{
204 double u = X[0];
205 double v = X[1];
206 dM[0][0] = u * denom;
207 dM[0][1] = 0;
208 dM[0][2] = -u * X[0] * denom;
209 dM[0][3] = v * denom;
210 dM[0][4] = 0;
211 dM[0][5] = -v * X[0] * denom;
212 dM[0][6] = denom;
213
214 dM[1][1] = u * denom;
215 dM[1][2] = -u * X[1] * denom;
216 dM[1][4] = v * denom;
217 dM[1][5] = -v * X[1] * denom;
218 dM[1][7] = denom;
219}
220
233 const double *dwdp0, vpMatrix &dM)
234{
235 double dwdx0, dwdx1;
236 double dwdy0, dwdy1;
237
238 dwdx0 = ((1. + p[0]) - X[0] * p[2]) * denom;
239 dwdx1 = (p[1] - X[1] * p[2]) * denom;
240 dwdy0 = (p[3] - X[0] * p[5]) * denom;
241 dwdy1 = ((1. + p[4]) - X[1] * p[5]) * denom;
242 for (unsigned int i = 0; i < nbParam; i++) {
243 dM[0][i] = dwdx0 * dwdp0[i] + dwdy0 * dwdp0[i + nbParam];
244 dM[1][i] = dwdx1 * dwdp0[i] + dwdy1 * dwdp0[i + nbParam];
245 }
246}
247
256{
257 double value = (p[2] * X1[0] + p[5] * X1[1] + 1.);
258
259 if (std::fabs(value) > std::numeric_limits<double>::epsilon()) {
260 X2[0] = ((1 + p[0]) * X1[0] + p[3] * X1[1] + p[6]) / value;
261 X2[1] = (p[1] * X1[0] + (1 + p[4]) * X1[1] + p[7]) / value;
262 } else {
263 throw(vpTrackingException(vpTrackingException::fatalError, "Division by zero in "
264 "vpTemplateTrackerWarpHomography::"
265 "warpXInv()"));
266 }
267}
268
276{
277 double h_00 = 1. + p[0];
278 double h_10 = p[1];
279 double h_20 = p[2];
280 double h_01 = p[3];
281 double h_11 = 1. + p[4];
282 double h_21 = p[5];
283 double h_02 = p[6];
284 double h_12 = p[7];
285
286 double h_inv_22 = (h_00 * h_11 - h_01 * h_10);
287
288 if (std::fabs(h_inv_22) < std::numeric_limits<double>::epsilon()) {
289 throw(vpException(vpException::fatalError, "Cannot get homography inverse parameters. Matrix determinant is 0."));
290 }
291
292 p_inv[0] = (h_11 - h_12 * h_21) / h_inv_22 - 1.;
293 p_inv[3] = (h_02 * h_21 - h_01) / h_inv_22;
294 p_inv[6] = (h_01 * h_12 - h_02 * h_11) / h_inv_22;
295
296 p_inv[1] = (h_12 * h_20 - h_10) / h_inv_22;
297 p_inv[4] = (h_00 - h_02 * h_20) / h_inv_22 - 1.;
298 p_inv[7] = (h_02 * h_10 - h_00 * h_12) / h_inv_22;
299
300 p_inv[2] = (h_10 * h_21 - h_11 * h_20) / h_inv_22;
301 p_inv[5] = (h_01 * h_20 - h_00 * h_21) / h_inv_22;
302}
303
311{
312 vpHomography H;
313 H[0][0] = 1. + p[0];
314 H[1][0] = p[1];
315 H[2][0] = p[2];
316 H[0][1] = p[3];
317 H[1][1] = 1. + p[4];
318 H[2][1] = p[5];
319 H[0][2] = p[6];
320 H[1][2] = p[7];
321 H[2][2] = 1.;
322
323 return H;
324}
325
332{
333 p.resize(getNbParam(), false);
334 p[0] = H[0][0] / H[2][2] - 1.;
335 p[1] = H[1][0] / H[2][2];
336 p[2] = H[2][0] / H[2][2];
337 p[3] = H[0][1] / H[2][2];
338 p[4] = H[1][1] / H[2][2] - 1.;
339 p[5] = H[2][1] / H[2][2];
340 p[6] = H[0][2] / H[2][2];
341 p[7] = H[1][2] / H[2][2];
342}
343
350{
351 p.resize(getNbParam(), false);
352 p[0] = H[0][0] / H[2][2] - 1.;
353 p[1] = H[1][0] / H[2][2];
354 p[2] = H[2][0] / H[2][2];
355 p[3] = H[0][1] / H[2][2];
356 p[4] = H[1][1] / H[2][2] - 1.;
357 p[5] = H[2][1] / H[2][2];
358 p[6] = H[0][2] / H[2][2];
359 p[7] = H[1][2] / H[2][2];
360}
361
371{
372 double h1_00 = 1. + p1[0];
373 double h1_10 = p1[1];
374 double h1_20 = p1[2];
375 double h1_01 = p1[3];
376 double h1_11 = 1. + p1[4];
377 double h1_21 = p1[5];
378 double h1_02 = p1[6];
379 double h1_12 = p1[7];
380
381 double h2_00 = 1. + p2[0];
382 double h2_10 = p2[1];
383 double h2_20 = p2[2];
384 double h2_01 = p2[3];
385 double h2_11 = 1. + p2[4];
386 double h2_21 = p2[5];
387 double h2_02 = p2[6];
388 double h2_12 = p2[7];
389
390 double h12_22 = h1_20 * h2_02 + h1_21 * h2_12 + 1.;
391
392 p12[0] = (h1_00 * h2_00 + h1_01 * h2_10 + h1_02 * h2_20) / h12_22 - 1.;
393 p12[3] = (h1_00 * h2_01 + h1_01 * h2_11 + h1_02 * h2_21) / h12_22;
394 p12[6] = (h1_00 * h2_02 + h1_01 * h2_12 + h1_02) / h12_22;
395
396 p12[1] = (h1_10 * h2_00 + h1_11 * h2_10 + h1_12 * h2_20) / h12_22;
397 p12[4] = (h1_10 * h2_01 + h1_11 * h2_11 + h1_12 * h2_21) / h12_22 - 1.;
398 p12[7] = (h1_10 * h2_02 + h1_11 * h2_12 + h1_12) / h12_22;
399
400 p12[2] = (h1_20 * h2_00 + h1_21 * h2_10 + h2_20) / h12_22;
401 p12[5] = (h1_20 * h2_01 + h1_21 * h2_11 + h2_21) / h12_22;
402}
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ fatalError
Fatal error.
Definition vpException.h:84
Implementation of an homography and operations on homographies.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void warpX(const vpColVector &X1, vpColVector &X2, const vpColVector &p)
void getParamPyramidUp(const vpColVector &p, vpColVector &p_up)
vpHomography getHomography(const vpColVector &ParamM) const
void getdWdp0(const int &v, const int &u, double *dIdW)
void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
void warpXInv(const vpColVector &X1, vpColVector &X2, const vpColVector &p)
void getParamInverse(const vpColVector &p, vpColVector &p_inv) const
void dWarpCompo(const vpColVector &X, const vpColVector &, const vpColVector &p, const double *dwdp0, vpMatrix &dW)
void dWarp(const vpColVector &, const vpColVector &X, const vpColVector &, vpMatrix &dW)
void computeDenom(vpColVector &X, const vpColVector &p)
void getParam(const vpHomography &H, vpColVector &p) const
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const
void getParamPyramidDown(const vpColVector &p, vpColVector &p_down)
unsigned int nbParam
Number of parameters used to model warp transformation.
unsigned int getNbParam() const
double denom
Internal value used by homography warp model.
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.