Visual Servoing Platform  version 3.4.1 under development (2021-04-20)
vpHinkley.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  * Hinkley's cumulative sum test implementation.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpHinkley.h>
50 //#include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpMath.h>
52 
53 #include <cmath> // std::fabs
54 #include <iostream>
55 #include <limits> // numeric_limits
56 #include <stdio.h>
57 #include <stdlib.h>
58 
59 /* VP_DEBUG_MODE fixed by configure:
60  1:
61  2:
62  3: Print data
63 */
64 
76 vpHinkley::vpHinkley() : dmin2(0.1), alpha(0.2), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0) {}
77 
94 vpHinkley::vpHinkley(double alpha_val, double delta_val)
95  : dmin2(delta_val / 2.), alpha(alpha_val), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0)
96 {
97 }
98 
112 void vpHinkley::init(double alpha_val, double delta_val)
113 {
114  init();
115 
116  setAlpha(alpha_val);
117  setDelta(delta_val);
118 }
119 
126 
134 {
135  nsignal = 0;
136  mean = 0.0;
137 
138  Sk = 0;
139  Mk = 0;
140 
141  Tk = 0;
142  Nk = 0;
143 }
144 
153 void vpHinkley::setDelta(double delta) { dmin2 = delta / 2; }
154 
162 void vpHinkley::setAlpha(double alpha_val) { this->alpha = alpha_val; }
163 
175 {
176 
177  vpHinkleyJumpType jump = noJump;
178 
179  nsignal++; // Signal length
180 
181  if (nsignal == 1)
182  mean = signal;
183 
184  // Calcul des variables cumulees
185  computeSk(signal);
186 
187  computeMk();
188 
189  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk;
190 
191  // teste si les variables cumulees excedent le seuil
192  if ((Mk - Sk) > alpha)
193  jump = downwardJump;
194 
195 #ifdef VP_DEBUG
196  if (VP_DEBUG_MODE >= 2) {
197  switch (jump) {
198  case noJump:
199  std::cout << "noJump " << std::endl;
200  break;
201  case downwardJump:
202  std::cout << "downWardJump " << std::endl;
203  break;
204  case upwardJump:
205  std::cout << "upwardJump " << std::endl;
206  break;
207  }
208  }
209 #endif
210 
211  computeMean(signal);
212 
213  if (jump == downwardJump) {
214  vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
215 
216  Sk = 0;
217  Mk = 0;
218  nsignal = 0;
219  }
220 
221  return (jump);
222 }
223 
235 {
236 
237  vpHinkleyJumpType jump = noJump;
238 
239  nsignal++; // Signal length
240 
241  if (nsignal == 1)
242  mean = signal;
243 
244  // Calcul des variables cumulees
245  computeTk(signal);
246 
247  computeNk();
248 
249  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Tk: " << Tk << " Nk: " << Nk;
250 
251  // teste si les variables cumulees excedent le seuil
252  if ((Tk - Nk) > alpha)
253  jump = upwardJump;
254 
255 #ifdef VP_DEBUG
256  if (VP_DEBUG_MODE >= 2) {
257  switch (jump) {
258  case noJump:
259  std::cout << "noJump " << std::endl;
260  break;
261  case downwardJump:
262  std::cout << "downWardJump " << std::endl;
263  break;
264  case upwardJump:
265  std::cout << "upWardJump " << std::endl;
266  break;
267  }
268  }
269 #endif
270  computeMean(signal);
271 
272  if (jump == upwardJump) {
273  vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
274 
275  Tk = 0;
276  Nk = 0;
277  nsignal = 0;
278  }
279 
280  return (jump);
281 }
282 
294 {
295 
296  vpHinkleyJumpType jump = noJump;
297 
298  nsignal++; // Signal length
299 
300  if (nsignal == 1)
301  mean = signal;
302 
303  // Calcul des variables cumulees
304  computeSk(signal);
305  computeTk(signal);
306 
307  computeMk();
308  computeNk();
309 
310  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk
311  << " Tk: " << Tk << " Nk: " << Nk << std::endl;
312 
313  // teste si les variables cumulees excedent le seuil
314  if ((Mk - Sk) > alpha)
315  jump = downwardJump;
316  else if ((Tk - Nk) > alpha)
317  jump = upwardJump;
318 
319 #ifdef VP_DEBUG
320  if (VP_DEBUG_MODE >= 2) {
321  switch (jump) {
322  case noJump:
323  std::cout << "noJump " << std::endl;
324  break;
325  case downwardJump:
326  std::cout << "downWardJump " << std::endl;
327  break;
328  case upwardJump:
329  std::cout << "upwardJump " << std::endl;
330  break;
331  }
332  }
333 #endif
334 
335  computeMean(signal);
336 
337  if ((jump == upwardJump) || (jump == downwardJump)) {
338  vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
339 
340  Sk = 0;
341  Mk = 0;
342  Tk = 0;
343  Nk = 0;
344  nsignal = 0;
345  // Debut modif FS le 03/09/2003
346  mean = signal;
347  // Fin modif FS le 03/09/2003
348  }
349 
350  return (jump);
351 }
352 
361 void vpHinkley::computeMean(double signal)
362 {
363  // Debut modif FS le 03/09/2003
364  // Lors d'une chute ou d'une remontee lente du signal, pariculierement
365  // apres un saut, la moyenne a tendance a "deriver". Pour reduire ces
366  // derives de la moyenne, elle n'est remise a jour avec la valeur
367  // courante du signal que si un debut de saut potentiel n'est pas detecte.
368  // if ( ((Mk-Sk) == 0) && ((Tk-Nk) == 0) )
369  if ((std::fabs(Mk - Sk) <= std::fabs(vpMath::maximum(Mk, Sk)) * std::numeric_limits<double>::epsilon()) &&
370  (std::fabs(Tk - Nk) <= std::fabs(vpMath::maximum(Tk, Nk)) * std::numeric_limits<double>::epsilon()))
371  // Fin modif FS le 03/09/2003
372 
373  // Mise a jour de la moyenne.
374  mean = (mean * (nsignal - 1) + signal) / (nsignal);
375 }
383 void vpHinkley::computeSk(double signal)
384 {
385 
386  // Calcul des variables cumulees
387  Sk += signal - mean + dmin2;
388 }
394 void vpHinkley::computeMk()
395 {
396  if (Sk > Mk)
397  Mk = Sk;
398 }
405 void vpHinkley::computeTk(double signal)
406 {
407 
408  // Calcul des variables cumulees
409  Tk += signal - mean - dmin2;
410 }
416 void vpHinkley::computeNk()
417 {
418  if (Tk < Nk)
419  Nk = Tk;
420 }
421 
423 {
424  switch (jump) {
425  case noJump:
426  std::cout << " No jump detected " << std::endl;
427  break;
428  case downwardJump:
429  std::cout << " Jump downward detected " << std::endl;
430  break;
431  case upwardJump:
432  std::cout << " Jump upward detected " << std::endl;
433  break;
434  default:
435  std::cout << " Jump detected " << std::endl;
436  break;
437  }
438 }
vpHinkleyJumpType testDownUpwardJump(double signal)
Definition: vpHinkley.cpp:293
vpHinkleyJumpType
Definition: vpHinkley.h:102
vpHinkleyJumpType testUpwardJump(double signal)
Definition: vpHinkley.cpp:234
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
#define vpCDEBUG(level)
Definition: vpDebug.h:511
static void print(vpHinkleyJumpType jump)
Definition: vpHinkley.cpp:422
void init()
Definition: vpHinkley.cpp:133
vpHinkleyJumpType testDownwardJump(double signal)
Definition: vpHinkley.cpp:174
void init(double alpha, double delta)
Definition: vpHinkley.cpp:112
virtual ~vpHinkley()
Definition: vpHinkley.cpp:125
void setAlpha(double alpha)
Definition: vpHinkley.cpp:162
void setDelta(double delta)
Definition: vpHinkley.cpp:153