Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
44 {
45  nbParam = 8;
46  dW.resize(2, nbParam);
47 }
48 
49 // get the parameter corresponding to the lower level of a gaussian pyramid
51 {
52  pdown = p;
53  pdown[2] = p[2] * 2.;
54  pdown[5] = p[5] * 2.;
55  pdown[6] = p[6] / 2.;
56  pdown[7] = p[7] / 2.;
57 }
58 
60 {
61  pup = p;
62  pup[2] = p[2] / 2.;
63  pup[5] = p[5] / 2.;
64  pup[6] = p[6] * 2.;
65  pup[7] = p[7] * 2.;
66 }
67 
68 /*calcul de di*dw(x,p0)/dp */
69 void vpTemplateTrackerWarpHomography::getdW0(const int &i, const int &j, const double &dy, const double &dx,
70  double *dIdW)
71 {
72  dIdW[0] = j * dx;
73  dIdW[1] = j * dy;
74  dIdW[2] = -j * j * dx - i * j * dy;
75  dIdW[3] = i * dx;
76  dIdW[4] = i * dy;
77  dIdW[5] = -i * j * dx - i * i * dy;
78  dIdW[6] = dx;
79  dIdW[7] = dy;
80 }
81 /*calcul de dw(x,p0)/dp */
82 void vpTemplateTrackerWarpHomography::getdWdp0(const int &i, const int &j, double *dIdW)
83 {
84  dIdW[0] = j;
85  dIdW[1] = 0;
86  dIdW[2] = -j * j;
87  dIdW[3] = i;
88  dIdW[4] = 0;
89  dIdW[5] = -i * j;
90  dIdW[6] = 1.;
91  dIdW[7] = 0;
92 
93  dIdW[8] = 0;
94  dIdW[9] = j;
95  dIdW[10] = -i * j;
96  dIdW[11] = 0;
97  dIdW[12] = i;
98  dIdW[13] = -i * i;
99  dIdW[14] = 0;
100  dIdW[15] = 1.;
101 }
103 {
104  denom = (1. / (ParamM[2] * vX[0] + ParamM[5] * vX[1] + 1.));
105 }
106 
107 void vpTemplateTrackerWarpHomography::warpX(const int &i, const int &j, double &i2, double &j2,
108  const vpColVector &ParamM)
109 {
110  j2 = ((1. + ParamM[0]) * j + ParamM[3] * i + ParamM[6]) * denom;
111  i2 = (ParamM[1] * j + (1. + ParamM[4]) * i + ParamM[7]) * denom;
112 }
113 
115 {
116  // if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)>0)//si dans le plan image reel
117  if ((denom) > 0) // FS optimisation
118  {
119  vXres[0] = ((1 + ParamM[0]) * vX[0] + ParamM[3] * vX[1] + ParamM[6]) * denom;
120  vXres[1] = (ParamM[1] * vX[0] + (1 + ParamM[4]) * vX[1] + ParamM[7]) * denom;
121  } else
123  "Division by zero in vpTemplateTrackerWarpHomography::warpX()"));
124 }
125 
127  const vpColVector & /*ParamM*/, vpMatrix &dW_)
128 {
129  double j = X1[0];
130  double i = X1[1];
131  dW_ = 0;
132  dW_[0][0] = j * denom;
133  dW_[0][2] = -j * X2[0] * denom;
134  dW_[0][3] = i * denom;
135  dW_[0][5] = -i * X2[0] * denom;
136  dW_[0][6] = denom;
137 
138  dW_[1][1] = j * denom;
139  dW_[1][2] = -j * X2[1] * denom;
140  dW_[1][4] = i * denom;
141  dW_[1][5] = -i * X2[1] * denom;
142  dW_[1][7] = denom;
143 }
144 
145 /*compute dw=dw/dx*dw/dp */
147  const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW_)
148 {
149  double dwdx0, dwdx1;
150  double dwdy0, dwdy1;
151 
152  dwdx0 = ((1. + ParamM[0]) - X2[0] * ParamM[2]) * denom;
153  dwdx1 = (ParamM[1] - X2[1] * ParamM[2]) * denom;
154  dwdy0 = (ParamM[3] - X2[0] * ParamM[5]) * denom;
155  dwdy1 = ((1. + ParamM[4]) - X2[1] * ParamM[5]) * denom;
156  for (unsigned int i = 0; i < nbParam; i++) {
157  dW_[0][i] = dwdx0 * dwdp0[i] + dwdy0 * dwdp0[i + nbParam];
158  dW_[1][i] = dwdx1 * dwdp0[i] + dwdy1 * dwdp0[i + nbParam];
159  }
160 }
161 
163 {
164 
165  if ((ParamM[2] * vX[0] + ParamM[5] * vX[1] + 1) < 0) // si dans le plan image reel
166  {
167  vXres[0] = ((1 + ParamM[0]) * vX[0] + ParamM[3] * vX[1] + ParamM[6]) / (ParamM[2] * vX[0] + ParamM[5] * vX[1] + 1);
168  vXres[1] = (ParamM[1] * vX[0] + (1 + ParamM[4]) * vX[1] + ParamM[7]) / (ParamM[2] * vX[0] + ParamM[5] * vX[1] + 1);
169  } else
170  throw(vpTrackingException(vpTrackingException::fatalError, "Division by zero in "
171  "vpTemplateTrackerWarpHomography::"
172  "warpXSpecialInv()"));
173 }
175 {
176  vpHomography H = getHomography(ParamM);
177  vpHomography Hinv = H.inverse();
178  getParam(Hinv, ParamMinv);
179 }
180 
182 {
183  vpHomography H;
184  for (unsigned int i = 0; i < 3; i++)
185  for (unsigned int j = 0; j < 3; j++) {
186  if (i + 3 * j != 8) {
187  H[i][j] = ParamM[i + 3 * j];
188  if (i == j)
189  H[i][j]++;
190  } else
191  H[i][j] = 1.;
192  }
193 
194  return H;
195 }
197 {
198  par = 0;
199  for (unsigned int i = 0; i < 3; i++)
200  for (unsigned int j = 0; j < 3; j++) {
201  if (i + 3 * j != 8) {
202  par[i + 3 * j] = H[i][j] / H[2][2];
203  if (i == j)
204  par[i + 3 * j]--;
205  }
206  }
207 }
208 
210 {
211  vpHomography H1 = getHomography(p1);
212  vpHomography H2 = getHomography(p2);
213  vpHomography H = H1 * H2;
214  getParam(H, pres);
215 }
void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
Error that can be emited by the vpTracker class and its derivates.
void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
void computeDenom(vpColVector &vX, const vpColVector &ParamM)
void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)
void getdWdp0(const int &i, const int &j, double *dIdW)
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpHomography inverse() const
invert the homography
void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
void getParam(const vpHomography &H, vpColVector &par) const
void getParamPyramidUp(const vpColVector &p, vpColVector &pup)
void warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
vpHomography getHomography(const vpColVector &ParamM) const