Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpTemplateTrackerMIForwardAdditional.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 * Example of template tracking.
33 *
34 * Authors:
35 * Amaury Dame
36 * Aurelien Yol
37 *
38*****************************************************************************/
39
40#include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
41
42#ifdef VISP_HAVE_OPENMP
43#include <omp.h>
44#endif
45
47 : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), p_prec(), G_prec(), KQuasiNewton()
48{
49 useCompositionnal = false;
50}
51
53{
54 dW = 0;
55
56 int Nbpoint = 0;
57
58 if (blur)
62
63 double Tij;
64 double IW, dx, dy;
65 int cr, ct;
66 double er, et;
67
68 Nbpoint = 0;
69
71 Warp->computeCoeff(p);
72 for (unsigned int point = 0; point < templateSize; point++) {
73 int i = ptTemplate[point].y;
74 int j = ptTemplate[point].x;
75 X1[0] = j;
76 X1[1] = i;
77 X2[0] = j;
78 X2[1] = i;
79
80 Warp->computeDenom(X1, p);
81 Warp->warpX(X1, X2, p);
82
83 double j2 = X2[0];
84 double i2 = X2[1];
85
86 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
87 Nbpoint++;
88 Tij = ptTemplate[point].val;
89 if (!blur)
90 IW = I.getValue(i2, j2);
91 else
92 IW = BI.getValue(i2, j2);
93
94 dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
95 dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
96
97 ct = static_cast<int>((IW * (Nc - 1)) / 255.);
98 cr = static_cast<int>((Tij * (Nc - 1)) / 255.);
99 et = (IW * (Nc - 1)) / 255. - ct;
100 er = (static_cast<double>(Tij) * (Nc - 1)) / 255. - cr;
101 Warp->dWarp(X1, X2, p, dW);
102
103 double *tptemp = new double[nbParam];
104 for (unsigned int it = 0; it < nbParam; it++)
105 tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
106
108 vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
110 vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
111
112 delete[] tptemp;
113 }
114 }
115
116 if (Nbpoint > 0) {
117 double MI;
118 computeProba(Nbpoint);
119 computeMI(MI);
121
123 try {
125 } catch (const vpException &e) {
126 throw(e);
127 }
128 }
129}
130
132{
133 dW = 0;
134
135 int Nbpoint = 0;
136 if (blur)
140
141 double MI = 0, MIprec = -1000;
142
144
145 double alpha = 2.;
146
147 unsigned int iteration = 0;
148
150 double evolRMS_init = 0;
151 double evolRMS_prec = 0;
152 double evolRMS_delta;
153
154 do {
155 if (iteration % 5 == 0)
157 Nbpoint = 0;
158 MIprec = MI;
159 MI = 0;
160 // erreur=0;
161
163
164 Warp->computeCoeff(p);
165#ifdef VISP_HAVE_OPENMP
166 int nthreads = omp_get_num_procs();
167 // std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function:
168 // " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
169 omp_set_num_threads(nthreads);
170#pragma omp parallel for default(shared)
171#endif
172 for (int point = 0; point < (int)templateSize; point++) {
173 int i = ptTemplate[point].y;
174 int j = ptTemplate[point].x;
175 X1[0] = j;
176 X1[1] = i;
177
178 Warp->computeDenom(X1, p);
179 Warp->warpX(X1, X2, p);
180
181 double j2 = X2[0];
182 double i2 = X2[1];
183
184 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
185 Nbpoint++;
186 double Tij = ptTemplate[point].val;
187 double IW;
188 if (!blur)
189 IW = I.getValue(i2, j2);
190 else
191 IW = BI.getValue(i2, j2);
192
193 double dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
194 double dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
195
196 int ct = (int)((IW * (Nc - 1)) / 255.);
197 int cr = (int)((Tij * (Nc - 1)) / 255.);
198 double et = (IW * (Nc - 1)) / 255. - ct;
199 double er = ((double)Tij * (Nc - 1)) / 255. - cr;
200
201 Warp->dWarp(X1, X2, p, dW);
202
203 double *tptemp = new double[nbParam];
204 for (unsigned int it = 0; it < nbParam; it++)
205 tptemp[it] = (dW[0][it] * dx + dW[1][it] * dy);
207 vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
209 vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
210
211 delete[] tptemp;
212 }
213 }
214
215 if (Nbpoint == 0) {
216 diverge = true;
217 MI = 0;
218 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
219 } else {
220 computeProba(Nbpoint);
221 computeMI(MI);
224
226 try {
227 switch (hessianComputation) {
230 break;
232 if (HLM.cond() > HLMdesire.cond())
234 else
235 dp = gain * 0.2 * HLM.inverseByLU() * G;
236 break;
237 default:
238 dp = gain * 0.2 * HLM.inverseByLU() * G;
239 break;
240 }
241 } catch (const vpException &e) {
242 throw(e);
243 }
244 }
245
246 switch (minimizationMethod) {
248 vpColVector p_test_LMA(nbParam);
250 p_test_LMA = p - 100000.1 * dp;
251 else
252 p_test_LMA = p + dp;
253 MI = -getCost(I, p);
254 double MI_LMA = -getCost(I, p_test_LMA);
255 if (MI_LMA > MI) {
256 p = p_test_LMA;
257 lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
258 } else {
259 lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
260 }
261 } break;
263 dp = -gain * 6.0 * G;
264 if (useBrent) {
265 alpha = 2.;
266 computeOptimalBrentGain(I, p, -MI, dp, alpha);
267 dp = alpha * dp;
268 }
269 p += dp;
270 break;
271 }
272
274 if (iterationGlobale != 0) {
275 vpColVector s_quasi = p - p_prec;
276 vpColVector y_quasi = G - G_prec;
277 double s_scal_y = s_quasi.t() * y_quasi;
278 if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
279 KQuasiNewton = KQuasiNewton + 0.001 * (s_quasi * s_quasi.t() / s_scal_y -
280 KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
281 (y_quasi.t() * KQuasiNewton * y_quasi));
282 }
283 }
284 dp = -KQuasiNewton * G;
285 p_prec = p;
286 G_prec = G;
287 p -= 1.01 * dp;
288 } break;
289
290 default: {
292 dp = -0.1 * dp;
293 if (useBrent) {
294 alpha = 2.;
295 computeOptimalBrentGain(I, p, -MI, dp, alpha);
296 dp = alpha * dp;
297 }
298
299 p += dp;
300 break;
301 }
302 }
303
305
306 if (iteration == 0) {
307 evolRMS_init = evolRMS;
308 }
309 iteration++;
311
312 evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
313 evolRMS_prec = evolRMS;
314
315 } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
316 (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init) * evolRMS_eps));
317
318 if (Nbpoint == 0) {
319 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
320 }
321
322 nbIteration = iteration;
326 }
327}
Implementation of column vector and the associated operations.
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition vpException.h:59
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void filter(const vpImage< unsigned char > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
Type getValue(unsigned int i, unsigned int j) const
Definition vpImage.h:1592
unsigned int getHeight() const
Definition vpImage.h:184
double cond(double svThreshold=1e-6) const
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
void initHessienDesired(const vpImage< unsigned char > &I)
vpHessienApproximationType ApproxHessian
vpHessienType hessianComputation
void computeProba(int &nbpoint)
void computeHessien(vpMatrix &H)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, vpMatrix &dM)=0
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
vpImage< double > dIx
vpImage< double > dIy
void computeEvalRMS(const vpColVector &p)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
unsigned int iterationMax
void initPosEvalRMS(const vpColVector &p)
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
unsigned int iterationGlobale
vpImage< double > BI
unsigned int templateSize
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.