40 #include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h> 56 double **dW_ddp0 =
new double *[(
unsigned int)nb_pt];
57 for (
int i = 0; i < nb_pt; i++) {
59 dW_ddp0[i] =
new double[2 *
nbParam];
71 double lambda = 0.00001;
78 for (
int i = 0; i < nb_pt; i++) {
85 for (
unsigned int ip = 0; ip <
nbParam; ip++) {
86 dW_[0][ip] = dW_ddp0[i][ip];
87 dW_[1][ip] = dW_ddp0[i][ip +
nbParam];
98 erreur += ((u[i] - fX1[0]) * (u[i] - fX1[0]) + (v[i] - fX1[1]) * (v[i] - fX1[1]));
114 while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
117 for (
int i = 0; i < nb_pt; i++)
129 for (
unsigned int i = 0; i < 8; i++) {
166 warp(u, v, 4, p, u2, v2);
168 for (
int i = 0; i < 4; i++) {
209 warp(u, v, 4, p, u2, v2);
212 for (
int i = 0; i < 4; i++) {
232 denom = vX[0] *
G[2][0] + vX[1] *
G[2][1] +
G[2][2];
243 pA[1][1] = -p[4] - p[5];
256 double i = vX[1], j = vX[0];
257 vXres[0] = (j *
G[0][0] + i *
G[0][1] +
G[0][2]) /
denom;
258 vXres[1] = (j *
G[1][0] + i *
G[1][1] +
G[1][2]) /
denom;
263 j2 = (j *
G[0][0] + i *
G[0][1] +
G[0][2]) /
denom;
264 i2 = (j *
G[1][0] + i *
G[1][1] +
G[1][2]) /
denom;
270 for (
unsigned int i = 0; i < 3; i++)
271 for (
unsigned int j = 0; j < 3; j++)
281 dhdx[0][0] = 1. /
denom;
282 dhdx[1][1] = 1. /
denom;
283 dhdx[0][2] = -X2[0] / (
denom);
284 dhdx[1][2] = -X2[1] / (
denom);
286 for (
unsigned int i = 0; i < 3; i++) {
289 dGx[i][2] =
G[i][0] * X1[1];
290 dGx[i][3] =
G[i][1] * X1[0];
291 dGx[i][4] =
G[i][0] * X1[0] -
G[i][1] * X1[1];
292 dGx[i][5] =
G[i][2] -
G[i][1] * X1[1];
293 dGx[i][6] =
G[i][2] * X1[0];
294 dGx[i][7] =
G[i][2] * X1[1];
308 dhdx[0][2] = -j * dx - i * dy;
312 for (
unsigned int par = 0; par < 3; par++) {
313 dGx[par][0] =
G[par][0];
314 dGx[par][1] =
G[par][1];
315 dGx[par][2] =
G[par][0] * i;
316 dGx[par][3] =
G[par][1] * j;
317 dGx[par][4] =
G[par][0] * j -
G[par][1] * i;
318 dGx[par][5] =
G[par][2] -
G[par][1] * i;
319 dGx[par][6] =
G[par][2] * j;
320 dGx[par][7] =
G[par][2] * i;
323 for (
unsigned int par = 0; par <
nbParam; par++) {
325 for (
unsigned int par2 = 0; par2 < 3; par2++)
326 res += dhdx[0][par2] *
dGx[par2][par];
344 for (
unsigned int par = 0; par < 3; par++) {
345 dGx[par][0] =
G[par][0];
346 dGx[par][1] =
G[par][1];
347 dGx[par][2] =
G[par][0] * i;
348 dGx[par][3] =
G[par][1] * j;
349 dGx[par][4] =
G[par][0] * j -
G[par][1] * i;
350 dGx[par][5] =
G[par][2] -
G[par][1] * i;
351 dGx[par][6] =
G[par][2] * j;
352 dGx[par][7] =
G[par][2] * i;
355 dIdW_temp = dhdx *
dGx;
357 for (
unsigned int par = 0; par <
nbParam; par++) {
358 dIdW[par] = dIdW_temp[0][par];
359 dIdW[par +
nbParam] = dIdW_temp[1][par];
373 for (
unsigned int par = 0; par < 3; par++) {
374 dGx[par][0] =
G[par][0];
375 dGx[par][1] =
G[par][1];
376 dGx[par][2] =
G[par][0] * i;
377 dGx[par][3] =
G[par][1] * j;
378 dGx[par][4] =
G[par][0] * j -
G[par][1] * i;
379 dGx[par][5] =
G[par][2] -
G[par][1] * i;
380 dGx[par][6] =
G[par][2] * j;
381 dGx[par][7] =
G[par][2] * i;
384 dIdW_temp = dhdx *
dGx;
386 for (
unsigned int par = 0; par <
nbParam; par++) {
387 dIdW[par] = dIdW_temp[0][par];
388 dIdW[par +
nbParam] = dIdW_temp[1][par];
397 for (
unsigned int i = 0; i <
nbParam; i++) {
398 dW_[0][i] =
denom * ((
G[0][0] - X2[0] *
G[2][0]) * dwdp0[i] + (
G[0][1] - X2[0] *
G[2][1]) * dwdp0[i +
nbParam]);
399 dW_[1][i] =
denom * ((G[1][0] - X2[1] * G[2][0]) * dwdp0[i] + (G[1][1] - X2[1] * G[2][1]) * dwdp0[i +
nbParam]);
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
Implementation of a matrix and operations on matrices.
void computeDenom(vpColVector &vX, const vpColVector &ParamM)
void getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
void computeCoeff(const vpColVector &p)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)
void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW)
error that can be emited by ViSP classes.
std::vector< vpMatrix > A
void findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt, vpColVector &p)
vpHomography getHomography() const
void getdWdp0(const int &i, const int &j, double *dIdW)
Implementation of an homography and operations on homographies.
void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
virtual ~vpTemplateTrackerWarpHomographySL3()
void getParamPyramidUp(const vpColVector &p, vpColVector &pup)
Implementation of column vector and the associated operations.
vpTemplateTrackerWarpHomographySL3()
vpMatrix inverseByLU() const
void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v)