Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpTemplateTrackerSSDInverseCompositional.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/vpImageTools.h>
41 #include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
42 
44  : vpTemplateTrackerSSD(warp), compoInitialised(false), HInv(), HCompInverse(), useTemplateSelect(false), evolRMS(0),
45  x_pos(), y_pos(), threshold_RMS(1e-8)
46 {
47  useInverse = true;
50 }
51 
53 {
54 
55  H = 0;
56  int i, j;
57 
58  for (unsigned int point = 0; point < templateSize; point++) {
59  if ((!useTemplateSelect) || (ptTemplateSelect[point])) {
60  i = ptTemplate[point].y;
61  j = ptTemplate[point].x;
62  X1[0] = j;
63  X1[1] = i;
64  Warp->computeDenom(X1, p);
65  ptTemplate[point].dW = new double[nbParam];
66 
67  Warp->getdW0(i, j, ptTemplate[point].dy, ptTemplate[point].dx, ptTemplate[point].dW);
68 
69  for (unsigned int it = 0; it < nbParam; it++)
70  for (unsigned int jt = 0; jt < nbParam; jt++)
71  H[it][jt] += ptTemplate[point].dW[it] * ptTemplate[point].dW[jt];
72  }
73  }
74  HInv = H;
75  vpMatrix HLMtemp(nbParam, nbParam);
76  vpMatrix::computeHLM(H, lambdaDep, HLMtemp);
77 
79  HCompInverse = HLMtemp.inverseByLU();
80  // std::cout<<Hinverse<<std::endl;
81  vpColVector dWtemp(nbParam);
82  vpColVector HiGtemp(nbParam);
83 
84  for (unsigned int point = 0; point < templateSize; point++) {
85  if ((!useTemplateSelect) || (ptTemplateSelect[point])) {
86  // i=ptTemplate[point].y;
87  // j=ptTemplate[point].x;
88  for (unsigned int it = 0; it < nbParam; it++)
89  dWtemp[it] = ptTemplate[point].dW[it];
90 
91  HiGtemp = -1. * HCompInverse * dWtemp;
92  ptTemplate[point].HiG = new double[nbParam];
93 
94  for (unsigned int it = 0; it < nbParam; it++)
95  ptTemplate[point].HiG[it] = HiGtemp[it];
96  }
97  }
98  compoInitialised = true;
99 }
100 
102 {
103  initCompInverse(I);
104 }
105 
107 {
108  if (blur)
110 
111  vpColVector dpinv(nbParam);
112  double IW;
113  double Tij;
114  unsigned int iteration = 0;
115  int i, j;
116  double i2, j2;
117  double alpha = 2.;
118  // vpTemplateTrackerPointtest *pt;
119  initPosEvalRMS(p);
120 
122  do {
123  unsigned int Nbpoint = 0;
124  double erreur = 0;
125  dp = 0;
126  Warp->computeCoeff(p);
127  for (unsigned int point = 0; point < templateSize; point++) {
128  if ((!useTemplateSelect) || (ptTemplateSelect[point])) {
129  // pt=&ptTemplatetest[point];
130  pt = &ptTemplate[point];
131  i = pt->y;
132  j = pt->x;
133  X1[0] = j;
134  X1[1] = i;
135  Warp->computeDenom(X1, p);
136  Warp->warpX(X1, X2, p);
137  j2 = X2[0];
138  i2 = X2[1];
139 
140  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
141  Tij = pt->val;
142  if (!blur)
143  IW = I.getValue(i2, j2);
144  else
145  IW = BI.getValue(i2, j2);
146  Nbpoint++;
147  double er = (Tij - IW);
148  for (unsigned int it = 0; it < nbParam; it++)
149  dp[it] += er * pt->HiG[it];
150 
151  erreur += er * er;
152  }
153  }
154  }
155  // std::cout << "npoint: " << Nbpoint << std::endl;
156  if (Nbpoint == 0) {
157  // std::cout<<"plus de point dans template suivi"<<std::endl;
159  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
160  }
161  dp = gain * dp;
162  // std::cout<<erreur/Nbpoint<<","<<GetCost(I,p)<<std::endl;
163  if (useBrent) {
164  alpha = 2.;
165  computeOptimalBrentGain(I, p, erreur / Nbpoint, dp, alpha);
166  dp = alpha * dp;
167  }
168  Warp->getParamInverse(dp, dpinv);
169  Warp->pRondp(p, dpinv, p);
170  iteration++;
171 
172  computeEvalRMS(p);
173  // std::cout << "iteration: " << iteration << " max: " << iterationMax <<
174  // std::endl; std::cout << "evolRMS: " << evolRMS << " threshold: " <<
175  // threshold_RMS << std::endl;
176  } while (/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax) && (evolRMS > threshold_RMS));
177 
178  nbIteration = iteration;
180 }
181 
183 {
184  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
185  x_pos.resize(nb_corners);
186  y_pos.resize(nb_corners);
187 
188  Warp->computeCoeff(p_);
189  vpTemplateTrackerTriangle triangle;
190 
191  for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
192  zoneTracked->getTriangle(i, triangle);
193  for (unsigned int j = 0; j < 3; j++) {
194  triangle.getCorner(j, X1[0], X1[1]);
195 
196  Warp->computeDenom(X1, p_);
197  Warp->warpX(X1, X2, p_);
198  x_pos[i * 3 + j] = X2[0];
199  y_pos[i * 3 + j] = X2[1];
200  }
201  }
202 }
203 
205 {
206  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
207 
208  Warp->computeCoeff(p_);
209  evolRMS = 0;
210  vpTemplateTrackerTriangle triangle;
211 
212  for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
213  zoneTracked->getTriangle(i, triangle);
214  for (unsigned int j = 0; j < 3; j++) {
215  triangle.getCorner(j, X1[0], X1[1]);
216 
217  Warp->computeDenom(X1, p_);
218  Warp->warpX(X1, X2, p_);
219  evolRMS += (x_pos[i * 3 + j] - X2[0]) * (x_pos[i * 3 + j] - X2[0]) +
220  (y_pos[i * 3 + j] - X2[1]) * (y_pos[i * 3 + j] - X2[1]);
221  x_pos[i * 3 + j] = X2[0];
222  y_pos[i * 3 + j] = X2[1];
223  }
224  }
225  evolRMS = evolRMS / nb_corners;
226 }
227 
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
unsigned int getWidth() const
Definition: vpImage.h:237
vpTemplateTrackerPoint * ptTemplate
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
vpColVector getCorner(unsigned int i) const
vpImage< double > BI
virtual void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const =0
unsigned int templateSize
unsigned int iterationMax
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const =0
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1402
Error that can be emited by the vpTracker class and its derivates.
unsigned int getNbTriangle() const
virtual void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)=0
unsigned int nbIteration
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpMatrix inverseByLU() const
vpTemplateTrackerZone * zoneTracked
unsigned int getHeight() const
Definition: vpImage.h:179
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5200