39 #include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
64 double **dW_ddp0 =
new double *[(
unsigned int)nb_pt];
65 for (
int i = 0; i < nb_pt; i++) {
67 dW_ddp0[i] =
new double[2 *
nbParam];
79 double lambda = 0.00001;
86 for (
int i = 0; i < nb_pt; i++) {
93 for (
unsigned int ip = 0; ip <
nbParam; ip++) {
94 dW_[0][ip] = dW_ddp0[i][ip];
95 dW_[1][ip] = dW_ddp0[i][ip +
nbParam];
106 erreur += ((u[i] - fX1[0]) * (u[i] - fX1[0]) + (v[i] - fX1[1]) * (v[i] - fX1[1]));
123 while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
126 for (
int i = 0; i < nb_pt; i++)
141 for (
unsigned int i = 0; i < 8; i++) {
182 warp(u, v, 4, p, u2, v2);
184 for (
int i = 0; i < 4; i++) {
231 warp(u, v, 4, p, u2, v2);
234 for (
int i = 0; i < 4; i++) {
262 denom = X[0] *
G[2][0] + X[1] *
G[2][1] +
G[2][2];
278 pA[1][1] = -p[4] - p[5];
298 double u = X1[0], v = X1[1];
299 X2[0] = (u *
G[0][0] + v *
G[0][1] +
G[0][2]) /
denom;
300 X2[1] = (u *
G[1][0] + v *
G[1][1] +
G[1][2]) /
denom;
316 u2 = (u1 *
G[0][0] + v1 *
G[0][1] +
G[0][2]) /
denom;
317 v2 = (u1 *
G[1][0] + v1 *
G[1][1] +
G[1][2]) /
denom;
327 for (
unsigned int i = 0; i < 3; i++)
328 for (
unsigned int j = 0; j < 3; j++)
351 dhdx[0][0] = 1. /
denom;
352 dhdx[1][1] = 1. /
denom;
353 dhdx[0][2] = -X2[0] / (
denom);
354 dhdx[1][2] = -X2[1] / (
denom);
356 for (
unsigned int i = 0; i < 3; i++) {
359 dGx[i][2] =
G[i][0] * X1[1];
360 dGx[i][3] =
G[i][1] * X1[0];
361 dGx[i][4] =
G[i][0] * X1[0] -
G[i][1] * X1[1];
362 dGx[i][5] =
G[i][2] -
G[i][1] * X1[1];
363 dGx[i][6] =
G[i][2] * X1[0];
364 dGx[i][7] =
G[i][2] * X1[1];
384 dhdx[0][2] = -u * du - v * dv;
388 for (
unsigned int par = 0; par < 3; par++) {
389 dGx[par][0] =
G[par][0];
390 dGx[par][1] =
G[par][1];
391 dGx[par][2] =
G[par][0] * v;
392 dGx[par][3] =
G[par][1] * u;
393 dGx[par][4] =
G[par][0] * u -
G[par][1] * v;
394 dGx[par][5] =
G[par][2] -
G[par][1] * v;
395 dGx[par][6] =
G[par][2] * u;
396 dGx[par][7] =
G[par][2] * v;
399 for (
unsigned int par = 0; par <
nbParam; par++) {
401 for (
unsigned int par2 = 0; par2 < 3; par2++)
402 res += dhdx[0][par2] *
dGx[par2][par];
428 for (
unsigned int par = 0; par < 3; par++) {
429 dGx[par][0] =
G[par][0];
430 dGx[par][1] =
G[par][1];
431 dGx[par][2] =
G[par][0] * v;
432 dGx[par][3] =
G[par][1] * u;
433 dGx[par][4] =
G[par][0] * u -
G[par][1] * v;
434 dGx[par][5] =
G[par][2] -
G[par][1] * v;
435 dGx[par][6] =
G[par][2] * u;
436 dGx[par][7] =
G[par][2] * v;
439 dIdW_temp = dhdx *
dGx;
441 for (
unsigned int par = 0; par <
nbParam; par++) {
442 dIdW[par] = dIdW_temp[0][par];
443 dIdW[par +
nbParam] = dIdW_temp[1][par];
469 for (
unsigned int par = 0; par < 3; par++) {
470 dGx[par][0] =
G[par][0];
471 dGx[par][1] =
G[par][1];
472 dGx[par][2] =
G[par][0] * v;
473 dGx[par][3] =
G[par][1] * u;
474 dGx[par][4] =
G[par][0] * u -
G[par][1] * v;
475 dGx[par][5] =
G[par][2] -
G[par][1] * v;
476 dGx[par][6] =
G[par][2] * u;
477 dGx[par][7] =
G[par][2] * v;
480 dIdW_temp = dhdx *
dGx;
482 for (
unsigned int par = 0; par <
nbParam; par++) {
483 dIdW[par] = dIdW_temp[0][par];
484 dIdW[par +
nbParam] = dIdW_temp[1][par];
502 for (
unsigned int i = 0; i <
nbParam; i++) {
503 dM[0][i] =
denom * ((
G[0][0] - X[0] *
G[2][0]) * dwdp0[i] + (
G[0][1] - X[0] *
G[2][1]) * dwdp0[i +
nbParam]);
504 dM[1][i] =
denom * ((
G[1][0] - X[1] *
G[2][0]) * dwdp0[i] + (
G[1][1] - X[1] *
G[2][1]) * dwdp0[i +
nbParam]);
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Implementation of an homography and operations on homographies.
Implementation of a matrix and operations on matrices.
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
vpTemplateTrackerWarpHomographySL3()
void computeDenom(vpColVector &X, const vpColVector &)
void findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt, vpColVector &p)
vpHomography getHomography() const
void warpX(const vpColVector &X1, vpColVector &X2, const vpColVector &)
void computeCoeff(const vpColVector &p)
std::vector< vpMatrix > A
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const
void dWarpCompo(const vpColVector &, const vpColVector &X, const vpColVector &, const double *dwdp0, vpMatrix &dW)
void getParamPyramidDown(const vpColVector &p, vpColVector &p_down)
void getdWdp0(const int &v, const int &u, double *dIdW)
void getParamInverse(const vpColVector &p, vpColVector &p_inv) const
void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &, vpMatrix &dW)
void getParamPyramidUp(const vpColVector &p, vpColVector &p_up)
unsigned int nbParam
Number of parameters used to model warp transformation.
double denom
Internal value used by homography warp model.
void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v)