40#include <visp3/core/vpTrackingException.h>
41#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
61 p_down[2] = p[2] * 2.;
64 p_down[5] = p[5] * 2.;
65 p_down[6] = p[6] / 2.;
66 p_down[7] = p[7] / 2.;
97 double u_du_ = u * du;
98 double v_dv_ = v * dv;
101 dIdW[2] = -u * (u_du_ + v_dv_);
104 dIdW[5] = -v * (u_du_ + v_dv_);
153 double value = (p[2] * X[0] + p[5] * X[1] + 1.);
155 if (std::fabs(value) > std::numeric_limits<double>::epsilon()) {
156 denom = (1. / value);
159 "Division by zero in vpTemplateTrackerWarpHomography::computeDenom()"));
174 u2 = ((1. + p[0]) * u1 + p[3] * v1 + p[6]) *
denom;
175 v2 = (p[1] * u1 + (1. + p[4]) * v1 + p[7]) *
denom;
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;
209 dM[0][0] = u *
denom;
211 dM[0][2] = -u * X[0] *
denom;
212 dM[0][3] = v *
denom;
214 dM[0][5] = -v * X[0] *
denom;
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;
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];
260 double value = (p[2] * X1[0] + p[5] * X1[1] + 1.);
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;
267 "vpTemplateTrackerWarpHomography::"
280 double h_00 = 1. + p[0];
284 double h_11 = 1. + p[4];
289 double h_inv_22 = (h_00 * h_11 - h_01 * h_10);
291 if (std::fabs(h_inv_22) < std::numeric_limits<double>::epsilon()) {
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;
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;
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;
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];
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];
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];
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];
393 double h12_22 = h1_20 * h2_02 + h1_21 * h2_12 + 1.;
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;
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;
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;
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.