Visual Servoing Platform version 3.5.0
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
76vpHinkley::vpHinkley() : dmin2(0.1), alpha(0.2), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0) {}
77
94vpHinkley::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
112void 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
153void vpHinkley::setDelta(double delta) { dmin2 = delta / 2; }
154
162void vpHinkley::setAlpha(double alpha_val) { this->alpha = alpha_val; }
163
175{
176
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
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
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
361void 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}
383void vpHinkley::computeSk(double signal)
384{
385
386 // Calcul des variables cumulees
387 Sk += signal - mean + dmin2;
388}
394void vpHinkley::computeMk()
395{
396 if (Sk > Mk)
397 Mk = Sk;
398}
405void vpHinkley::computeTk(double signal)
406{
407
408 // Calcul des variables cumulees
409 Tk += signal - mean - dmin2;
410}
416void 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}
void init()
Definition: vpHinkley.cpp:133
void setDelta(double delta)
Definition: vpHinkley.cpp:153
virtual ~vpHinkley()
Definition: vpHinkley.cpp:125
void init(double alpha, double delta)
Definition: vpHinkley.cpp:112
vpHinkleyJumpType testDownwardJump(double signal)
Definition: vpHinkley.cpp:174
void setAlpha(double alpha)
Definition: vpHinkley.cpp:162
static void print(vpHinkleyJumpType jump)
Definition: vpHinkley.cpp:422
vpHinkleyJumpType testUpwardJump(double signal)
Definition: vpHinkley.cpp:234
vpHinkleyJumpType
Definition: vpHinkley.h:102
@ downwardJump
Definition: vpHinkley.h:104
@ upwardJump
Definition: vpHinkley.h:105
vpHinkleyJumpType testDownUpwardJump(double signal)
Definition: vpHinkley.cpp:293
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
#define vpCDEBUG(level)
Definition: vpDebug.h:511