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