40 #include <visp3/core/vpTrackingException.h>
41 #include <visp3/tt/vpTemplateTrackerWarpHomography.h>
58 p_down[2] = p[2] * 2.;
61 p_down[5] = p[5] * 2.;
62 p_down[6] = p[6] / 2.;
63 p_down[7] = p[7] / 2.;
95 double u_du_ = u * du;
96 double v_dv_ = v * dv;
99 dIdW[2] = -u * (u_du_ + v_dv_);
102 dIdW[5] = -v * (u_du_ + v_dv_);
151 double value = (p[2] * X[0] + p[5] * X[1] + 1.);
153 if (std::fabs(value) > std::numeric_limits<double>::epsilon()) {
154 denom = (1. / value);
157 "Division by zero in vpTemplateTrackerWarpHomography::computeDenom()"));
172 u2 = ((1. + p[0]) * u1 + p[3] * v1 + p[6]) *
denom;
173 v2 = (p[1] * u1 + (1. + p[4]) * v1 + p[7]) *
denom;
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;
207 dM[0][0] = u *
denom;
209 dM[0][2] = -u * X[0] *
denom;
210 dM[0][3] = v *
denom;
212 dM[0][5] = -v * X[0] *
denom;
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;
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];
258 double value = (p[2] * X1[0] + p[5] * X1[1] + 1.);
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;
265 "vpTemplateTrackerWarpHomography::"
278 double h_00 = 1. + p[0];
282 double h_11 = 1. + p[4];
287 double h_inv_22 = (h_00 * h_11 - h_01 * h_10);
289 if (std::fabs(h_inv_22) < std::numeric_limits<double>::epsilon()) {
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;
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;
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;
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];
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];
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];
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];
391 double h12_22 = h1_20 * h2_02 + h1_21 * h2_12 + 1.;
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;
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;
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;
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emited by ViSP classes.
Implementation of an homography and operations on homographies.
Implementation of a matrix and operations on matrices.
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)
vpTemplateTrackerWarpHomography()
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.