Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
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/vpHinkley.h>
46 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
47 #include <visp3/core/vpDebug.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 
73 vpHinkley::vpHinkley() : dmin2(0.1), alpha(0.2), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0) { }
74 
91 vpHinkley::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 
108 void vpHinkley::init(double alpha_val, double delta_val)
109 {
110  init();
111 
112  setAlpha(alpha_val);
113  setDelta(delta_val);
114 }
115 
122 
130 {
131  nsignal = 0;
132  mean = 0.0;
133 
134  Sk = 0;
135  Mk = 0;
136 
137  Tk = 0;
138  Nk = 0;
139 }
140 
149 void vpHinkley::setDelta(double delta) { dmin2 = delta / 2; }
150 
158 void vpHinkley::setAlpha(double alpha_val) { this->alpha = alpha_val; }
159 
171 {
172 
173  vpHinkleyJumpType jump = noJump;
174 
175  nsignal++; // Signal length
176 
177  if (nsignal == 1)
178  mean = signal;
179 
180  // Calcul des variables cumulees
181  computeSk(signal);
182 
183  computeMk();
184 
185  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk;
186 
187  // teste si les variables cumulees excedent le seuil
188  if ((Mk - Sk) > alpha)
189  jump = downwardJump;
190 
191 #ifdef VP_DEBUG
192  if (VP_DEBUG_MODE >= 2) {
193  switch (jump) {
194  case noJump:
195  std::cout << "noJump " << std::endl;
196  break;
197  case downwardJump:
198  std::cout << "downWardJump " << std::endl;
199  break;
200  case upwardJump:
201  std::cout << "upwardJump " << std::endl;
202  break;
203  }
204  }
205 #endif
206 
207  computeMean(signal);
208 
209  if (jump == downwardJump) {
210  vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
211 
212  Sk = 0;
213  Mk = 0;
214  nsignal = 0;
215  }
216 
217  return (jump);
218 }
219 
231 {
232 
233  vpHinkleyJumpType jump = noJump;
234 
235  nsignal++; // Signal length
236 
237  if (nsignal == 1)
238  mean = signal;
239 
240  // Calcul des variables cumulees
241  computeTk(signal);
242 
243  computeNk();
244 
245  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Tk: " << Tk << " Nk: " << Nk;
246 
247  // teste si les variables cumulees excedent le seuil
248  if ((Tk - Nk) > alpha)
249  jump = upwardJump;
250 
251 #ifdef VP_DEBUG
252  if (VP_DEBUG_MODE >= 2) {
253  switch (jump) {
254  case noJump:
255  std::cout << "noJump " << std::endl;
256  break;
257  case downwardJump:
258  std::cout << "downWardJump " << std::endl;
259  break;
260  case upwardJump:
261  std::cout << "upWardJump " << std::endl;
262  break;
263  }
264  }
265 #endif
266  computeMean(signal);
267 
268  if (jump == upwardJump) {
269  vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
270 
271  Tk = 0;
272  Nk = 0;
273  nsignal = 0;
274  }
275 
276  return (jump);
277 }
278 
290 {
291 
292  vpHinkleyJumpType jump = noJump;
293 
294  nsignal++; // Signal length
295 
296  if (nsignal == 1)
297  mean = signal;
298 
299  // Calcul des variables cumulees
300  computeSk(signal);
301  computeTk(signal);
302 
303  computeMk();
304  computeNk();
305 
306  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk
307  << " Tk: " << Tk << " Nk: " << Nk << std::endl;
308 
309 // teste si les variables cumulees excedent le seuil
310  if ((Mk - Sk) > alpha)
311  jump = downwardJump;
312  else if ((Tk - Nk) > alpha)
313  jump = upwardJump;
314 
315 #ifdef VP_DEBUG
316  if (VP_DEBUG_MODE >= 2) {
317  switch (jump) {
318  case noJump:
319  std::cout << "noJump " << std::endl;
320  break;
321  case downwardJump:
322  std::cout << "downWardJump " << std::endl;
323  break;
324  case upwardJump:
325  std::cout << "upwardJump " << std::endl;
326  break;
327  }
328  }
329 #endif
330 
331  computeMean(signal);
332 
333  if ((jump == upwardJump) || (jump == downwardJump)) {
334  vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
335 
336  Sk = 0;
337  Mk = 0;
338  Tk = 0;
339  Nk = 0;
340  nsignal = 0;
341  // Debut modif FS le 03/09/2003
342  mean = signal;
343  // Fin modif FS le 03/09/2003
344  }
345 
346  return (jump);
347 }
348 
357 void vpHinkley::computeMean(double signal)
358 {
359  // Debut modif FS le 03/09/2003
360  // Lors d'une chute ou d'une remontee lente du signal, pariculierement
361  // apres un saut, la moyenne a tendance a "deriver". Pour reduire ces
362  // derives de la moyenne, elle n'est remise a jour avec la valeur
363  // courante du signal que si un debut de saut potentiel n'est pas detecte.
364  // if ( ((Mk-Sk) == 0) && ((Tk-Nk) == 0) )
365  if ((std::fabs(Mk - Sk) <= std::fabs(vpMath::maximum(Mk, Sk)) * std::numeric_limits<double>::epsilon()) &&
366  (std::fabs(Tk - Nk) <= std::fabs(vpMath::maximum(Tk, Nk)) * std::numeric_limits<double>::epsilon()))
367  // Fin modif FS le 03/09/2003
368 
369  // Mise a jour de la moyenne.
370  mean = (mean * (nsignal - 1) + signal) / (nsignal);
371 }
379 void vpHinkley::computeSk(double signal)
380 {
381 
382  // Calcul des variables cumulees
383  Sk += signal - mean + dmin2;
384 }
390 void vpHinkley::computeMk()
391 {
392  if (Sk > Mk)
393  Mk = Sk;
394 }
401 void vpHinkley::computeTk(double signal)
402 {
403 
404  // Calcul des variables cumulees
405  Tk += signal - mean - dmin2;
406 }
412 void vpHinkley::computeNk()
413 {
414  if (Tk < Nk)
415  Nk = Tk;
416 }
417 
419 {
420  switch (jump) {
421  case noJump:
422  std::cout << " No jump detected " << std::endl;
423  break;
424  case downwardJump:
425  std::cout << " Jump downward detected " << std::endl;
426  break;
427  case upwardJump:
428  std::cout << " Jump upward detected " << std::endl;
429  break;
430  default:
431  std::cout << " Jump detected " << std::endl;
432  break;
433  }
434 }
435 #elif !defined(VISP_BUILD_SHARED_LIBS)
436  // Work around to avoid warning: libvisp_core.a(vpHinkley.cpp.o) has no symbols
437 void dummy_vpRHinkley() { };
438 #endif
void init()
Definition: vpHinkley.cpp:129
void setDelta(double delta)
Definition: vpHinkley.cpp:149
vp_deprecated vpHinkley()
Definition: vpHinkley.cpp:73
virtual ~vpHinkley()
Definition: vpHinkley.cpp:121
vpHinkleyJumpType testDownwardJump(double signal)
Definition: vpHinkley.cpp:170
void setAlpha(double alpha)
Definition: vpHinkley.cpp:158
static void print(vpHinkleyJumpType jump)
Definition: vpHinkley.cpp:418
vpHinkleyJumpType testUpwardJump(double signal)
Definition: vpHinkley.cpp:230
vpHinkleyJumpType
Definition: vpHinkley.h:100
@ downwardJump
Definition: vpHinkley.h:102
@ upwardJump
Definition: vpHinkley.h:103
vpHinkleyJumpType testDownUpwardJump(double signal)
Definition: vpHinkley.cpp:289
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
#define vpCDEBUG(level)
Definition: vpDebug.h:497