Visual Servoing Platform version 3.5.0
vpTemplateTrackerMIForwardCompositional.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 * Example of template tracking.
33 *
34 * Authors:
35 * Amaury Dame
36 * Aurelien Yol
37 * Fabien Spindler
38 *
39 *****************************************************************************/
40#include <visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h>
41
43 : vpTemplateTrackerMI(_warp), CompoInitialised(false)
44{
45}
46
48{
49 ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
50 for (unsigned int point = 0; point < templateSize; point++) {
51 int i = ptTemplate[point].y;
52 int j = ptTemplate[point].x;
53 X1[0] = j;
54 X1[1] = i;
55 Warp->computeDenom(X1, p);
56 ptTemplate[point].dW = new double[2 * nbParam];
57 Warp->getdWdp0(i, j, ptTemplate[point].dW);
58
59 double Tij = ptTemplate[point].val;
60 int ct = (int)((Tij * (Nc - 1)) / 255.);
61 double et = (Tij * (Nc - 1)) / 255. - ct;
62 ptTemplateSupp[point].et = et;
63 ptTemplateSupp[point].ct = ct;
64 ptTemplateSupp[point].Bt = new double[4];
65 ptTemplateSupp[point].dBt = new double[4];
66 for (char it = -1; it <= 2; it++) {
67 ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
68 ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et);
69 }
70 }
71 CompoInitialised = true;
72}
74{
75 initCompo();
76
77 dW = 0;
78
79 if (blur)
83
84 int Nbpoint = 0;
85
86 double IW, dx, dy;
87 int cr, ct;
88 double er, et;
89
90 Nbpoint = 0;
91
93
94 Warp->computeCoeff(p);
95 for (unsigned int point = 0; point < templateSize; point++) {
96 int i = ptTemplate[point].y;
97 int j = ptTemplate[point].x;
98 X1[0] = j;
99 X1[1] = i;
100
101 Warp->computeDenom(X1, p);
102 Warp->warpX(X1, X2, p);
103
104 double j2 = X2[0];
105 double i2 = X2[1];
106
107 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
108 Nbpoint++;
109 if (!blur)
110 IW = I.getValue(i2, j2);
111 else
112 IW = BI.getValue(i2, j2);
113
114 dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
115 dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
116
117 cr = ptTemplateSupp[point].ct;
118 er = ptTemplateSupp[point].et;
119 ct = (int)((IW * (Nc - 1)) / 255.);
120 et = ((double)IW * (Nc - 1)) / 255. - ct;
121
122 Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
123
124 double *tptemp = new double[nbParam];
125 for (unsigned int it = 0; it < nbParam; it++)
126 tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
127
128 vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
129
130 delete[] tptemp;
131 }
132 }
133 double MI;
134 computeProba(Nbpoint);
135 computeMI(MI);
137
139
142}
143
145{
146 if (!CompoInitialised) {
147 std::cout << "Compositionnal tracking not initialised.\nUse initCompo() function." << std::endl;
148 }
149 dW = 0;
150
151 if (blur) {
153 }
156
158 double MI = 0, MIprec = -1000;
159
161
163
164 double i2, j2;
165 double IW;
166 int cr, ct;
167 double er, et;
168 double dx, dy;
169
170 vpColVector dpinv(nbParam);
171 double alpha = 2.;
172
173 int i, j;
174 unsigned int iteration = 0;
175
176 double evolRMS_init = 0;
177 double evolRMS_prec = 0;
178 double evolRMS_delta;
179
180 do {
181 int Nbpoint = 0;
182 MIprec = MI;
183 MI = 0;
184
186
187 Warp->computeCoeff(p);
188
189 for (unsigned int point = 0; point < templateSize; point++) {
190 i = ptTemplate[point].y;
191 j = ptTemplate[point].x;
192 X1[0] = j;
193 X1[1] = i;
194 Warp->warpX(i, j, i2, j2, p);
195 X2[0] = j2;
196 X2[1] = i2;
197
198 Warp->computeDenom(X1, p);
199 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
200 Nbpoint++;
201 if (!blur)
202 IW = I.getValue(i2, j2);
203 else
204 IW = BI.getValue(i2, j2);
205
206 dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
207 dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
208
209 ct = (int)((IW * (Nc - 1)) / 255.);
210 et = ((double)IW * (Nc - 1)) / 255. - ct;
211 cr = ptTemplateSupp[point].ct;
212 er = ptTemplateSupp[point].et;
213
214 Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
215
216 double *tptemp = new double[nbParam];
217 for (unsigned int it = 0; it < nbParam; it++)
218 tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
219
221 vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
223 vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
224
225 delete[] tptemp;
226 }
227 }
228 if (Nbpoint == 0) {
229 diverge = true;
230 MI = 0;
231 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
232 } else {
233 computeProba(Nbpoint);
234 computeMI(MI);
238
240
241 try {
242 switch (hessianComputation) {
245 break;
247 if (HLM.cond() > HLMdesire.cond())
249 else
250 dp = gain * 0.2 * HLM.inverseByLU() * G;
251 break;
252 default:
253 dp = gain * 0.2 * HLM.inverseByLU() * G;
254 break;
255 }
256 } catch (const vpException &e) {
257 throw(e);
258 }
259 }
260
262 dp = -0.04 * dp;
263// else
264// dp = 1. * dp;
265
266 if (useBrent) {
267 alpha = 2.;
268 computeOptimalBrentGain(I, p, -MI, dp, alpha);
269 dp = alpha * dp;
270 }
271 Warp->pRondp(p, dp, p);
273
274 if (iteration == 0) {
275 evolRMS_init = evolRMS;
276 }
277
278 iteration++;
279
280 evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
281 evolRMS_prec = evolRMS;
282
283 } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
284 (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init)*evolRMS_eps) );
285
286 nbIteration = iteration;
287
291 }
292}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
unsigned int getWidth() const
Definition: vpImage.h:246
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
unsigned int getHeight() const
Definition: vpImage.h:188
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6622
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
void initHessienDesired(const vpImage< unsigned char > &I)
vpHessienApproximationType ApproxHessian
vpHessienType hessianComputation
void computeMI(double &MI)
void computeProba(int &nbpoint)
void computeHessien(vpMatrix &H)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, const double *dwdp0, vpMatrix &dM)=0
virtual void getdWdp0(const int &v, const int &u, 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)
unsigned int nbIteration
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
vpImage< double > BI
unsigned int templateSize
Error that can be emited by the vpTracker class and its derivates.