Visual Servoing Platform version 3.5.0
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{
48 nbParam = 8;
49}
50
58{
59 p_down[0] = p[0];
60 p_down[1] = p[1];
61 p_down[2] = p[2] * 2.;
62 p_down[3] = p[3];
63 p_down[4] = p[4];
64 p_down[5] = p[5] * 2.;
65 p_down[6] = p[6] / 2.;
66 p_down[7] = p[7] / 2.;
67}
68
76{
77 p_up[0] = p[0];
78 p_up[1] = p[1];
79 p_up[2] = p[2] / 2.;
80 p_up[3] = p[3];
81 p_up[4] = p[4];
82 p_up[5] = p[5] / 2.;
83 p_up[6] = p[6] * 2.;
84 p_up[7] = p[7] * 2.;
85}
86
95void vpTemplateTrackerWarpHomography::getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
96{
97 double u_du_ = u * du;
98 double v_dv_ = v * dv;
99 dIdW[0] = u_du_;
100 dIdW[1] = u * dv;
101 dIdW[2] = -u * (u_du_ + v_dv_);
102 dIdW[3] = v * du;
103 dIdW[4] = v_dv_;
104 dIdW[5] = -v * (u_du_ + v_dv_);
105 dIdW[6] = du;
106 dIdW[7] = dv;
107}
108
120void vpTemplateTrackerWarpHomography::getdWdp0(const int &v, const int &u, double *dIdW)
121{
122 double uv_ = u * v;
123 dIdW[0] = u;
124 dIdW[1] = 0;
125 dIdW[2] = -u * u;
126 dIdW[3] = v;
127 dIdW[4] = 0;
128 dIdW[5] = - uv_;
129 dIdW[6] = 1.;
130 dIdW[7] = 0;
131
132 dIdW[8] = 0;
133 dIdW[9] = u;
134 dIdW[10] = - uv_;
135 dIdW[11] = 0;
136 dIdW[12] = v;
137 dIdW[13] = -v * v;
138 dIdW[14] = 0;
139 dIdW[15] = 1.;
140}
141
152{
153 double value = (p[2] * X[0] + p[5] * X[1] + 1.);
154
155 if (std::fabs(value) > std::numeric_limits<double>::epsilon()) {
156 denom = (1. / value);
157 } else {
159 "Division by zero in vpTemplateTrackerWarpHomography::computeDenom()"));
160 }
161}
162
172void vpTemplateTrackerWarpHomography::warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)
173{
174 u2 = ((1. + p[0]) * u1 + p[3] * v1 + p[6]) * denom;
175 v2 = (p[1] * u1 + (1. + p[4]) * v1 + p[7]) * denom;
176}
177
188{
189 X2[0] = ((1 + p[0]) * X1[0] + p[3] * X1[1] + p[6]) * denom;
190 X2[1] = (p[1] * X1[0] + (1 + p[4]) * X1[1] + p[7]) * denom;
191}
192
205 vpMatrix &dM)
206{
207 double u = X[0];
208 double v = X[1];
209 dM[0][0] = u * denom;
210 dM[0][1] = 0;
211 dM[0][2] = -u * X[0] * denom;
212 dM[0][3] = v * denom;
213 dM[0][4] = 0;
214 dM[0][5] = -v * X[0] * denom;
215 dM[0][6] = denom;
216
217 dM[1][1] = u * denom;
218 dM[1][2] = -u * X[1] * denom;
219 dM[1][4] = v * denom;
220 dM[1][5] = -v * X[1] * denom;
221 dM[1][7] = denom;
222}
223
236 const vpColVector &p, const double *dwdp0, vpMatrix &dM)
237{
238 double dwdx0, dwdx1;
239 double dwdy0, dwdy1;
240
241 dwdx0 = ((1. + p[0]) - X[0] * p[2]) * denom;
242 dwdx1 = (p[1] - X[1] * p[2]) * denom;
243 dwdy0 = (p[3] - X[0] * p[5]) * denom;
244 dwdy1 = ((1. + p[4]) - X[1] * p[5]) * denom;
245 for (unsigned int i = 0; i < nbParam; i++) {
246 dM[0][i] = dwdx0 * dwdp0[i] + dwdy0 * dwdp0[i + nbParam];
247 dM[1][i] = dwdx1 * dwdp0[i] + dwdy1 * dwdp0[i + nbParam];
248 }
249}
250
259{
260 double value = (p[2] * X1[0] + p[5] * X1[1] + 1.);
261
262 if (std::fabs(value) > std::numeric_limits<double>::epsilon()) {
263 X2[0] = ((1 + p[0]) * X1[0] + p[3] * X1[1] + p[6]) / value;
264 X2[1] = (p[1] * X1[0] + (1 + p[4]) * X1[1] + p[7]) / value;
265 } else {
266 throw(vpTrackingException(vpTrackingException::fatalError, "Division by zero in "
267 "vpTemplateTrackerWarpHomography::"
268 "warpXInv()"));
269 }
270}
271
279{
280 double h_00 = 1. + p[0];
281 double h_10 = p[1];
282 double h_20 = p[2];
283 double h_01 = p[3];
284 double h_11 = 1. + p[4];
285 double h_21 = p[5];
286 double h_02 = p[6];
287 double h_12 = p[7];
288
289 double h_inv_22 = (h_00 * h_11 - h_01 * h_10);
290
291 if (std::fabs(h_inv_22) < std::numeric_limits<double>::epsilon()) {
292 throw(vpException(vpException::fatalError, "Cannot get homography inverse parameters. Matrix determinant is 0."));
293 }
294
295 p_inv[0] = (h_11 - h_12 * h_21) / h_inv_22 - 1.;
296 p_inv[3] = (h_02 * h_21 - h_01) / h_inv_22;
297 p_inv[6] = (h_01 * h_12 - h_02 * h_11) / h_inv_22;
298
299 p_inv[1] = (h_12 * h_20 - h_10) / h_inv_22;
300 p_inv[4] = (h_00 - h_02 * h_20) / h_inv_22 - 1.;
301 p_inv[7] = (h_02 * h_10 - h_00 * h_12) / h_inv_22;
302
303 p_inv[2] = (h_10 * h_21 - h_11 * h_20) / h_inv_22;
304 p_inv[5] = (h_01 * h_20 - h_00 * h_21) / h_inv_22;
305}
306
314{
315 vpHomography H;
316 H[0][0] = 1. + p[0];
317 H[1][0] = p[1];
318 H[2][0] = p[2];
319 H[0][1] = p[3];
320 H[1][1] = 1. + p[4];
321 H[2][1] = p[5];
322 H[0][2] = p[6];
323 H[1][2] = p[7];
324 H[2][2] = 1.;
325
326 return H;
327}
328
335{
336 p.resize(getNbParam(), false);
337 p[0] = H[0][0] / H[2][2] - 1.;
338 p[1] = H[1][0] / H[2][2];
339 p[2] = H[2][0] / H[2][2];
340 p[3] = H[0][1] / H[2][2];
341 p[4] = H[1][1] / H[2][2] - 1.;
342 p[5] = H[2][1] / H[2][2];
343 p[6] = H[0][2] / H[2][2];
344 p[7] = H[1][2] / H[2][2];
345}
346
353{
354 p.resize(getNbParam(), false);
355 p[0] = H[0][0] / H[2][2] - 1.;
356 p[1] = H[1][0] / H[2][2];
357 p[2] = H[2][0] / H[2][2];
358 p[3] = H[0][1] / H[2][2];
359 p[4] = H[1][1] / H[2][2] - 1.;
360 p[5] = H[2][1] / H[2][2];
361 p[6] = H[0][2] / H[2][2];
362 p[7] = H[1][2] / H[2][2];
363}
364
374{
375 double h1_00 = 1. + p1[0];
376 double h1_10 = p1[1];
377 double h1_20 = p1[2];
378 double h1_01 = p1[3];
379 double h1_11 = 1. + p1[4];
380 double h1_21 = p1[5];
381 double h1_02 = p1[6];
382 double h1_12 = p1[7];
383
384 double h2_00 = 1. + p2[0];
385 double h2_10 = p2[1];
386 double h2_20 = p2[2];
387 double h2_01 = p2[3];
388 double h2_11 = 1. + p2[4];
389 double h2_21 = p2[5];
390 double h2_02 = p2[6];
391 double h2_12 = p2[7];
392
393 double h12_22 = h1_20 * h2_02 + h1_21 * h2_12 + 1.;
394
395 p12[0] = (h1_00 * h2_00 + h1_01 * h2_10 + h1_02 * h2_20) / h12_22 - 1.;
396 p12[3] = (h1_00 * h2_01 + h1_01 * h2_11 + h1_02 * h2_21) / h12_22;
397 p12[6] = (h1_00 * h2_02 + h1_01 * h2_12 + h1_02) / h12_22;
398
399 p12[1] = (h1_10 * h2_00 + h1_11 * h2_10 + h1_12 * h2_20) / h12_22;
400 p12[4] = (h1_10 * h2_01 + h1_11 * h2_11 + h1_12 * h2_21) / h12_22 - 1.;
401 p12[7] = (h1_10 * h2_02 + h1_11 * h2_12 + h1_12) / h12_22;
402
403 p12[2] = (h1_20 * h2_00 + h1_21 * h2_10 + h2_20) / h12_22;
404 p12[5] = (h1_20 * h2_01 + h1_21 * h2_11 + h2_21) / h12_22;
405}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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.