Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpTemplateTrackerWarpHomography.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Template tracker.
32  *
33  * Authors:
34  * Amaury Dame
35  * Aurelien Yol
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 #include <visp3/tt/vpTemplateTrackerWarpHomography.h>
40 #include <visp3/core/vpTrackingException.h>
41 
43 {
44  nbParam = 8 ;
45  dW.resize(2,nbParam);
46 }
47 
48 //get the parameter corresponding to the lower level of a gaussian pyramid
50 {
51  pdown=p;
52  pdown[2]=p[2]*2.;
53  pdown[5]=p[5]*2.;
54  pdown[6]=p[6]/2.;
55  pdown[7]=p[7]/2.;
56 }
57 
59 {
60  pup=p;
61  pup[2]=p[2]/2.;
62  pup[5]=p[5]/2.;
63  pup[6]=p[6]*2.;
64  pup[7]=p[7]*2.;
65 }
66 
67 /*calcul de di*dw(x,p0)/dp */
68 void vpTemplateTrackerWarpHomography::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
69 {
70  dIdW[0]=j*dx;
71  dIdW[1]=j*dy;
72  dIdW[2]=-j*j*dx-i*j*dy;
73  dIdW[3]=i*dx;
74  dIdW[4]=i*dy;
75  dIdW[5]=-i*j*dx-i*i*dy;
76  dIdW[6]=dx;
77  dIdW[7]=dy;
78 }
79 /*calcul de dw(x,p0)/dp */
80 void vpTemplateTrackerWarpHomography::getdWdp0(const int &i,const int &j,double *dIdW)
81 {
82  dIdW[0]=j;
83  dIdW[1]=0;
84  dIdW[2]=-j*j;
85  dIdW[3]=i;
86  dIdW[4]=0;
87  dIdW[5]=-i*j;
88  dIdW[6]=1.;
89  dIdW[7]=0;
90 
91  dIdW[8]=0;
92  dIdW[9]=j;
93  dIdW[10]=-i*j;
94  dIdW[11]=0;
95  dIdW[12]=i;
96  dIdW[13]=-i*i;
97  dIdW[14]=0;
98  dIdW[15]=1.;
99 }
101 {
102  denom=(1./(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1.));
103 }
104 
105 void vpTemplateTrackerWarpHomography::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
106 {
107  j2=((1.+ParamM[0])*j+ParamM[3]*i+ParamM[6])*denom;
108  i2=(ParamM[1]*j+(1.+ParamM[4])*i+ParamM[7])*denom;
109 }
110 
111 
113 {
114  //if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)>0)//si dans le plan image reel
115  if((denom)>0)// FS optimisation
116  {
117  vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])*denom;
118  vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])*denom;
119  }
120  else
121  throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpX()"));
122 }
123 
125 {
126  double j=X1[0];
127  double i=X1[1];
128  dW_=0;
129  dW_[0][0]=j*denom;
130  dW_[0][2]=-j*X2[0]*denom;
131  dW_[0][3]=i*denom;
132  dW_[0][5]=-i*X2[0]*denom;
133  dW_[0][6]=denom;
134 
135  dW_[1][1]=j*denom;
136  dW_[1][2]=-j*X2[1]*denom;
137  dW_[1][4]=i*denom;
138  dW_[1][5]=-i*X2[1]*denom;
139  dW_[1][7]=denom;
140 }
141 
142 /*compute dw=dw/dx*dw/dp */
144  const double *dwdp0,vpMatrix &dW_)
145 {
146  double dwdx0,dwdx1;
147  double dwdy0,dwdy1;
148 
149  dwdx0=((1.+ParamM[0])-X2[0]*ParamM[2])*denom;
150  dwdx1=(ParamM[1]-X2[1]*ParamM[2])*denom;
151  dwdy0=(ParamM[3]-X2[0]*ParamM[5])*denom;
152  dwdy1=((1.+ParamM[4])-X2[1]*ParamM[5])*denom;
153  for(unsigned int i=0;i<nbParam;i++)
154  {
155  dW_[0][i]=dwdx0*dwdp0[i]+dwdy0*dwdp0[i+nbParam];
156  dW_[1][i]=dwdx1*dwdp0[i]+dwdy1*dwdp0[i+nbParam];
157  }
158 }
159 
161 {
162 
163  if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)<0)//si dans le plan image reel
164  {
165  vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
166  vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
167  }
168  else
169  throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpXSpecialInv()")) ;
170 }
172 {
173  vpHomography H = getHomography(ParamM);
174  vpHomography Hinv = H.inverse();
175  getParam(Hinv, ParamMinv);
176 }
177 
178 
180 {
181  vpHomography H;
182  for(unsigned int i=0;i<3;i++)
183  for(unsigned int j=0;j<3;j++)
184  {
185  if(i+3*j!=8)
186  {
187  H[i][j]=ParamM[i+3*j];
188  if(i==j)H[i][j]++;
189  }
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  {
202  if(i+3*j!=8)
203  {
204  par[i+3*j]=H[i][j]/H[2][2];
205  if(i==j)par[i+3*j]--;
206  }
207  }
208 }
209 
210 
211 
213 {
214  vpHomography H1 = getHomography(p1);
215  vpHomography H2 = getHomography(p2);
216  vpHomography H = H1*H2;
217  getParam(H,pres);
218 }
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:97
void getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
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:179
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