Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpTemplateTrackerMIInverseCompositional.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#include <visp3/core/vpTrackingException.h>
40#include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
41
42#include <memory>
43
45 : vpTemplateTrackerMI(_warp), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
46 p_prec(), G_prec(), KQuasiNewton()
47{
48 useInverse = true;
49}
50
51void vpTemplateTrackerMIInverseCompositional::initTemplateRefBspline(unsigned int ptIndex, double &et) // AY : Optim
52{
53 ptTemplateSupp[ptIndex].BtInit = new double[(1 + nbParam + nbParam * nbParam) * static_cast<unsigned int>(bspline)];
54
55 unsigned int index = 0;
56 int endIndex = 1;
57
58 double (*ptBspFct)(double);
59 double (*ptdBspFct)(double);
60 double (*ptd2BspFct)(double);
61 if (bspline == 3) {
62 if (et > 0.5) {
63 et = et - 1;
64 }
65 ptBspFct = &vpTemplateTrackerMIBSpline::Bspline3;
66 ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline3;
67 ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline3;
68 } else {
69 ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
70 ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
71 ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
72 endIndex = 2;
73 }
74
75 for (int it = -1; it <= endIndex; it++) {
76 ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)(static_cast<double>(-it) + et);
77
78 for (unsigned int ip = 0; ip < nbParam; ++ip) {
79 ptTemplateSupp[ptIndex].BtInit[index++] =
80 (*ptdBspFct)(static_cast<double>(-it) + et) * ptTemplate[ptIndex].dW[ip] * (-1.0);
81 for (unsigned int ip2 = 0; ip2 < nbParam; ++ip2) {
82 ptTemplateSupp[ptIndex].BtInit[index++] =
83 (*ptd2BspFct)(static_cast<double>(-it) + et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2];
84 }
85 }
86 }
87}
88
90{
91 ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
92
95
101 }
102 double Nc_255_ = (Nc - 1) / 255.;
103 Warp->computeCoeff(p);
104 for (unsigned int point = 0; point < templateSize; point++) {
105 int i = ptTemplate[point].y;
106 int j = ptTemplate[point].x;
107
108 ptTemplate[point].dW = new double[nbParam];
109
110 double dx = ptTemplate[point].dx * Nc_255_;
111 double dy = ptTemplate[point].dy * Nc_255_;
112
113 Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
114 double Tij = ptTemplate[point].val;
115 int ct = static_cast<int>(Tij * Nc_255_);
116 double et = (Tij * Nc_255_) - ct;
117 ptTemplateSupp[point].et = et;
118 ptTemplateSupp[point].ct = ct;
119 }
120 CompoInitialised = true;
121}
122
124{
126
127 int Nbpoint = 0;
128
129 double IW;
130 int cr, ct;
131 double er, et;
132
133 Nbpoint = 0;
134
135 if (blur)
137
139 Warp->computeCoeff(p);
140
141 for (unsigned int point = 0; point < templateSize; point++) {
142 int i = ptTemplate[point].y;
143 int j = ptTemplate[point].x;
144 X1[0] = j;
145 X1[1] = i;
146
147 Warp->computeDenom(X1, p);
148 Warp->warpX(X1, X2, p);
149
150 double j2 = X2[0];
151 double i2 = X2[1];
152
153 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
154 Nbpoint++;
155
156 if (blur)
157 IW = BI.getValue(i2, j2);
158 else
159 IW = I.getValue(i2, j2);
160
161 ct = ptTemplateSupp[point].ct;
162 et = ptTemplateSupp[point].et;
163 cr = static_cast<int>((IW * (Nc - 1)) / 255.);
164 er = (IW * (Nc - 1)) / 255. - cr;
165
166 if (ApproxHessian == HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect)) {
167 vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
168 bspline);
169 } else if ((ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW) &&
170 (ptTemplateSelect[point] || !useTemplateSelect)) {
171 if (bspline == 3) {
172 vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
173 } else {
174 vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
175 }
176 } else if (ptTemplateSelect[point] || !useTemplateSelect)
177 vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc, nbParam, bspline);
178 }
179 }
180
181 double MI;
182 computeProba(Nbpoint);
183 computeMI(MI);
185
187
189
191 KQuasiNewton = HLMdesireInverse;
192}
193
195{
196 if (!CompoInitialised) {
197 std::cout << "Compositionnal tracking not initialised.\nUse initCompInverse() function." << std::endl;
198 }
199
200 dW = 0;
201
202 if (blur)
204
206 double MI = 0, MIprec = -1000;
207
208 vpColVector p_avant_estimation;
209 p_avant_estimation = p;
212
214
215 vpColVector dpinv(nbParam);
216 double alpha = 2.;
217
218 unsigned int iteration = 0;
219
220 vpMatrix Hnorm(nbParam, nbParam);
221 double evolRMS_init = 0;
222 double evolRMS_prec = 0;
223 double evolRMS_delta;
224
225 vpColVector dp_test_LMA(nbParam);
226 vpColVector dpinv_test_LMA(nbParam);
227 vpColVector p_test_LMA(nbParam);
228
229 do {
230 int Nbpoint = 0;
231 MIprec = MI;
232 MI = 0;
233
235
236 Warp->computeCoeff(p);
237
238 for (int point = 0; point < static_cast<int>(templateSize); point++) {
239 X1[0] = static_cast<double>(ptTemplate[point].x);
240 X1[1] = static_cast<double>(ptTemplate[point].y);
241
242 Warp->computeDenom(X1, p);
243 Warp->warpX(X1, X2, p);
244
245 double j2 = X2[0];
246 double i2 = X2[1];
247
248 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
249
250 Nbpoint++;
251 double IW;
252 if (!blur)
253 IW = static_cast<double>(I.getValue(i2, j2));
254 else
255 IW = BI.getValue(i2, j2);
256
257 int ct = ptTemplateSupp[point].ct;
258 double et = ptTemplateSupp[point].et;
259 double tmp = IW * (static_cast<double>(Nc) - 1.) / 255.;
260 int cr = static_cast<int>(tmp);
261 double er = tmp - static_cast<double>(cr);
262
264 (ptTemplateSelect[point] || !useTemplateSelect)) {
265 vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
267 } else if (ptTemplateSelect[point] || !useTemplateSelect) {
268 if (bspline == 3) {
269 vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
270 nbParam);
271 } else {
272 vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
273 nbParam);
274 }
275 } else {
276 vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb, nbParam, bspline);
277 }
278 }
279 }
280
281 if (Nbpoint == 0) {
282 diverge = true;
283 MI = 0;
284 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
285
286 } else {
287 unsigned int indd, indd2;
288 indd = indd2 = 0;
289 unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
290 for (unsigned int i = 0; i < Ncb_ * Ncb_; i++) {
291 Prt[i] /= Nbpoint;
292 for (unsigned int j = 0; j < nbParam; j++) {
293 dPrt[indd] /= Nbpoint;
294 indd++;
295 for (unsigned int k = 0; k < nbParam; k++) {
296 d2Prt[indd2] /= Nbpoint;
297 indd2++;
298 }
299 }
300 }
301
302 computeMI(MI);
303
307 }
310
311 try {
312 switch (hessianComputation) {
315 break;
317 if (HLM.cond() > HLMdesire.cond())
319 else
320 dp = gain * 0.2 * HLM.inverseByLU() * G;
321 break;
322 default:
323 dp = gain * 0.2 * HLM.inverseByLU() * G;
324 break;
325 }
326 } catch (const vpException &e) {
327 throw(e);
328 }
329 }
330
331 switch (minimizationMethod) {
334 dp_test_LMA = -100000.1 * dp;
335 else
336 dp_test_LMA = dp;
337 Warp->getParamInverse(dp_test_LMA, dpinv_test_LMA);
338 Warp->pRondp(p, dpinv_test_LMA, p_test_LMA);
339
340 MI = -getCost(I, p);
341 double MI_LMA = -getCost(I, p_test_LMA);
342 if (MI_LMA > MI) {
343 dp = dp_test_LMA;
344 lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
345 } else {
346 dp = 0;
347 lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
348 }
349 } break;
351 dp = -gain * 0.3 * G * 20;
352 break;
353
355 if (iterationGlobale != 0) {
356 vpColVector s_quasi = p - p_prec;
357 vpColVector y_quasi = G - G_prec;
358 double s_scal_y = s_quasi.t() * y_quasi;
359 if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
360 KQuasiNewton = KQuasiNewton + 0.0001 * (s_quasi * s_quasi.t() / s_scal_y -
361 KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
362 (y_quasi.t() * KQuasiNewton * y_quasi));
363 }
364 }
365 dp = gain * KQuasiNewton * G;
366 p_prec = p;
367 G_prec = G;
368 } break;
369
370 default: {
371 if (useBrent) {
372 alpha = 2.;
373 computeOptimalBrentGain(I, p, -MI, dp, alpha);
374 dp = alpha * dp;
375 }
377 dp = -dp;
378
379 break;
380 }
381 }
382
383 Warp->getParamInverse(dp, dpinv);
384 Warp->pRondp(p, dpinv, p);
386
387 if (iteration == 0) {
388 evolRMS_init = evolRMS;
389 }
390 iteration++;
392
393 evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
394 evolRMS_prec = evolRMS;
395
396 } while ((!diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
397 (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init) * evolRMS_eps));
398
399 nbIteration = iteration;
400
401 if (diverge) {
402 if (computeCovariance) {
404 covarianceMatrix = -1;
407 }
408 } else {
411
413 p = p_avant_estimation;
417 covarianceMatrix = -1;
418 }
419
420 if (computeCovariance) {
421 try {
422 covarianceMatrix = (-H).inverseByLU();
423 } catch (...) {
425 covarianceMatrix = -1;
428 }
429 }
430 }
431}
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 getGradX(const vpImage< unsigned char > &I, vpImage< FilterType > &dIx)
static void filter(const vpImage< unsigned char > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false)
static void getGradY(const vpImage< unsigned char > &I, vpImage< FilterType > &dIy)
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
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
double cond(double svThreshold=1e-6) const
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
vpHessienApproximationType ApproxHessian
vpImage< double > d2Ix
void computeHessienNormalized(vpMatrix &H)
vpHessienType hessianComputation
void computeProba(int &nbpoint)
void computeHessien(vpMatrix &H)
vpImage< double > d2Ixy
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
vpImage< double > d2Iy
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
unsigned int getNbParam() const
virtual void getParamInverse(const vpColVector &p, vpColVector &p_inv) const =0
virtual void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)=0
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const =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.