Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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 
91 void 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 
117 void 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 
169 void 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.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
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.
Definition: vpHomography.h:168
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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.