Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpHinkley.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 * Hinkley's cumulative sum test implementation.
33 *
34*****************************************************************************/
35
45#include <visp3/core/vpDebug.h>
46#include <visp3/core/vpHinkley.h>
47//#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpMath.h>
49
50#include <cmath> // std::fabs
51#include <iostream>
52#include <limits> // numeric_limits
53#include <stdio.h>
54#include <stdlib.h>
55
56/* VP_DEBUG_MODE fixed by configure:
57 1:
58 2:
59 3: Print data
60*/
61
73vpHinkley::vpHinkley() : dmin2(0.1), alpha(0.2), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0) {}
74
91vpHinkley::vpHinkley(double alpha_val, double delta_val)
92 : dmin2(delta_val / 2.), alpha(alpha_val), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0)
93{
94}
95
109void vpHinkley::init(double alpha_val, double delta_val)
110{
111 init();
112
113 setAlpha(alpha_val);
114 setDelta(delta_val);
115}
116
123
131{
132 nsignal = 0;
133 mean = 0.0;
134
135 Sk = 0;
136 Mk = 0;
137
138 Tk = 0;
139 Nk = 0;
140}
141
150void vpHinkley::setDelta(double delta) { dmin2 = delta / 2; }
151
159void vpHinkley::setAlpha(double alpha_val) { this->alpha = alpha_val; }
160
172{
173
175
176 nsignal++; // Signal length
177
178 if (nsignal == 1)
179 mean = signal;
180
181 // Calcul des variables cumulees
182 computeSk(signal);
183
184 computeMk();
185
186 vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk;
187
188 // teste si les variables cumulees excedent le seuil
189 if ((Mk - Sk) > alpha)
190 jump = downwardJump;
191
192#ifdef VP_DEBUG
193 if (VP_DEBUG_MODE >= 2) {
194 switch (jump) {
195 case noJump:
196 std::cout << "noJump " << std::endl;
197 break;
198 case downwardJump:
199 std::cout << "downWardJump " << std::endl;
200 break;
201 case upwardJump:
202 std::cout << "upwardJump " << std::endl;
203 break;
204 }
205 }
206#endif
207
208 computeMean(signal);
209
210 if (jump == downwardJump) {
211 vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
212
213 Sk = 0;
214 Mk = 0;
215 nsignal = 0;
216 }
217
218 return (jump);
219}
220
232{
233
235
236 nsignal++; // Signal length
237
238 if (nsignal == 1)
239 mean = signal;
240
241 // Calcul des variables cumulees
242 computeTk(signal);
243
244 computeNk();
245
246 vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Tk: " << Tk << " Nk: " << Nk;
247
248 // teste si les variables cumulees excedent le seuil
249 if ((Tk - Nk) > alpha)
250 jump = upwardJump;
251
252#ifdef VP_DEBUG
253 if (VP_DEBUG_MODE >= 2) {
254 switch (jump) {
255 case noJump:
256 std::cout << "noJump " << std::endl;
257 break;
258 case downwardJump:
259 std::cout << "downWardJump " << std::endl;
260 break;
261 case upwardJump:
262 std::cout << "upWardJump " << std::endl;
263 break;
264 }
265 }
266#endif
267 computeMean(signal);
268
269 if (jump == upwardJump) {
270 vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
271
272 Tk = 0;
273 Nk = 0;
274 nsignal = 0;
275 }
276
277 return (jump);
278}
279
291{
292
294
295 nsignal++; // Signal length
296
297 if (nsignal == 1)
298 mean = signal;
299
300 // Calcul des variables cumulees
301 computeSk(signal);
302 computeTk(signal);
303
304 computeMk();
305 computeNk();
306
307 vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk
308 << " Tk: " << Tk << " Nk: " << Nk << std::endl;
309
310 // teste si les variables cumulees excedent le seuil
311 if ((Mk - Sk) > alpha)
312 jump = downwardJump;
313 else if ((Tk - Nk) > alpha)
314 jump = upwardJump;
315
316#ifdef VP_DEBUG
317 if (VP_DEBUG_MODE >= 2) {
318 switch (jump) {
319 case noJump:
320 std::cout << "noJump " << std::endl;
321 break;
322 case downwardJump:
323 std::cout << "downWardJump " << std::endl;
324 break;
325 case upwardJump:
326 std::cout << "upwardJump " << std::endl;
327 break;
328 }
329 }
330#endif
331
332 computeMean(signal);
333
334 if ((jump == upwardJump) || (jump == downwardJump)) {
335 vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
336
337 Sk = 0;
338 Mk = 0;
339 Tk = 0;
340 Nk = 0;
341 nsignal = 0;
342 // Debut modif FS le 03/09/2003
343 mean = signal;
344 // Fin modif FS le 03/09/2003
345 }
346
347 return (jump);
348}
349
358void vpHinkley::computeMean(double signal)
359{
360 // Debut modif FS le 03/09/2003
361 // Lors d'une chute ou d'une remontee lente du signal, pariculierement
362 // apres un saut, la moyenne a tendance a "deriver". Pour reduire ces
363 // derives de la moyenne, elle n'est remise a jour avec la valeur
364 // courante du signal que si un debut de saut potentiel n'est pas detecte.
365 // if ( ((Mk-Sk) == 0) && ((Tk-Nk) == 0) )
366 if ((std::fabs(Mk - Sk) <= std::fabs(vpMath::maximum(Mk, Sk)) * std::numeric_limits<double>::epsilon()) &&
367 (std::fabs(Tk - Nk) <= std::fabs(vpMath::maximum(Tk, Nk)) * std::numeric_limits<double>::epsilon()))
368 // Fin modif FS le 03/09/2003
369
370 // Mise a jour de la moyenne.
371 mean = (mean * (nsignal - 1) + signal) / (nsignal);
372}
380void vpHinkley::computeSk(double signal)
381{
382
383 // Calcul des variables cumulees
384 Sk += signal - mean + dmin2;
385}
391void vpHinkley::computeMk()
392{
393 if (Sk > Mk)
394 Mk = Sk;
395}
402void vpHinkley::computeTk(double signal)
403{
404
405 // Calcul des variables cumulees
406 Tk += signal - mean - dmin2;
407}
413void vpHinkley::computeNk()
414{
415 if (Tk < Nk)
416 Nk = Tk;
417}
418
420{
421 switch (jump) {
422 case noJump:
423 std::cout << " No jump detected " << std::endl;
424 break;
425 case downwardJump:
426 std::cout << " Jump downward detected " << std::endl;
427 break;
428 case upwardJump:
429 std::cout << " Jump upward detected " << std::endl;
430 break;
431 default:
432 std::cout << " Jump detected " << std::endl;
433 break;
434 }
435}
void init()
void setDelta(double delta)
virtual ~vpHinkley()
vpHinkleyJumpType testDownwardJump(double signal)
void setAlpha(double alpha)
static void print(vpHinkleyJumpType jump)
vpHinkleyJumpType testUpwardJump(double signal)
vpHinkleyJumpType
Definition vpHinkley.h:97
@ downwardJump
Definition vpHinkley.h:99
vpHinkleyJumpType testDownUpwardJump(double signal)
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
#define vpCDEBUG(level)
Definition vpDebug.h:506