Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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 
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 }
95 
109 void 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 
150 void vpHinkley::setDelta(double delta) { dmin2 = delta / 2; }
151 
159 void vpHinkley::setAlpha(double alpha_val) { this->alpha = alpha_val; }
160 
172 {
173 
174  vpHinkleyJumpType jump = noJump;
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 
234  vpHinkleyJumpType jump = noJump;
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 
293  vpHinkleyJumpType jump = noJump;
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 
358 void 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 }
380 void vpHinkley::computeSk(double signal)
381 {
382 
383  // Calcul des variables cumulees
384  Sk += signal - mean + dmin2;
385 }
391 void vpHinkley::computeMk()
392 {
393  if (Sk > Mk)
394  Mk = Sk;
395 }
402 void vpHinkley::computeTk(double signal)
403 {
404 
405  // Calcul des variables cumulees
406  Tk += signal - mean - dmin2;
407 }
413 void 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()
Definition: vpHinkley.cpp:130
void setDelta(double delta)
Definition: vpHinkley.cpp:150
virtual ~vpHinkley()
Definition: vpHinkley.cpp:122
vpHinkleyJumpType testDownwardJump(double signal)
Definition: vpHinkley.cpp:171
void setAlpha(double alpha)
Definition: vpHinkley.cpp:159
static void print(vpHinkleyJumpType jump)
Definition: vpHinkley.cpp:419
vpHinkleyJumpType testUpwardJump(double signal)
Definition: vpHinkley.cpp:231
vpHinkleyJumpType
Definition: vpHinkley.h:97
@ downwardJump
Definition: vpHinkley.h:99
@ upwardJump
Definition: vpHinkley.h:100
vpHinkleyJumpType testDownUpwardJump(double signal)
Definition: vpHinkley.cpp:290
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
#define vpCDEBUG(level)
Definition: vpDebug.h:497