vpHinkley.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Hinkley's cumulative sum test implementation.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHinkley.h>
//#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>

#include <cmath> // std::fabs
#include <iostream>
#include <limits> // numeric_limits
#include <stdio.h>
#include <stdlib.h>

/* VP_DEBUG_MODE fixed by configure:
   1:
   2:
   3: Print data
*/

vpHinkley::vpHinkley() : dmin2(0.1), alpha(0.2), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0) {}

vpHinkley::vpHinkley(double alpha_val, double delta_val)
  : dmin2(delta_val / 2.), alpha(alpha_val), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0)
{
}

void vpHinkley::init(double alpha_val, double delta_val)
{
  init();

  setAlpha(alpha_val);
  setDelta(delta_val);
}

vpHinkley::~vpHinkley() {}

void vpHinkley::init()
{
  nsignal = 0;
  mean = 0.0;

  Sk = 0;
  Mk = 0;

  Tk = 0;
  Nk = 0;
}

void vpHinkley::setDelta(double delta) { dmin2 = delta / 2; }

void vpHinkley::setAlpha(double alpha_val) { this->alpha = alpha_val; }

vpHinkley::vpHinkleyJumpType vpHinkley::testDownwardJump(double signal)
{

  vpHinkleyJumpType jump = noJump;

  nsignal++; // Signal length

  if (nsignal == 1)
    mean = signal;

  // Calcul des variables cumulees
  computeSk(signal);

  computeMk();

  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk;

  // teste si les variables cumulees excedent le seuil
  if ((Mk - Sk) > alpha)
    jump = downwardJump;

#ifdef VP_DEBUG
  if (VP_DEBUG_MODE >= 2) {
    switch (jump) {
    case noJump:
      std::cout << "noJump " << std::endl;
      break;
    case downwardJump:
      std::cout << "downWardJump " << std::endl;
      break;
    case upwardJump:
      std::cout << "upwardJump " << std::endl;
      break;
    }
  }
#endif

  computeMean(signal);

  if (jump == downwardJump) {
    vpCDEBUG(2) << "\n*** Reset the Hinkley test  ***\n";

    Sk = 0;
    Mk = 0;
    nsignal = 0;
  }

  return (jump);
}

vpHinkley::vpHinkleyJumpType vpHinkley::testUpwardJump(double signal)
{

  vpHinkleyJumpType jump = noJump;

  nsignal++; // Signal length

  if (nsignal == 1)
    mean = signal;

  // Calcul des variables cumulees
  computeTk(signal);

  computeNk();

  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Tk: " << Tk << " Nk: " << Nk;

  // teste si les variables cumulees excedent le seuil
  if ((Tk - Nk) > alpha)
    jump = upwardJump;

#ifdef VP_DEBUG
  if (VP_DEBUG_MODE >= 2) {
    switch (jump) {
    case noJump:
      std::cout << "noJump " << std::endl;
      break;
    case downwardJump:
      std::cout << "downWardJump " << std::endl;
      break;
    case upwardJump:
      std::cout << "upWardJump " << std::endl;
      break;
    }
  }
#endif
  computeMean(signal);

  if (jump == upwardJump) {
    vpCDEBUG(2) << "\n*** Reset the Hinkley test  ***\n";

    Tk = 0;
    Nk = 0;
    nsignal = 0;
  }

  return (jump);
}

vpHinkley::vpHinkleyJumpType vpHinkley::testDownUpwardJump(double signal)
{

  vpHinkleyJumpType jump = noJump;

  nsignal++; // Signal length

  if (nsignal == 1)
    mean = signal;

  // Calcul des variables cumulees
  computeSk(signal);
  computeTk(signal);

  computeMk();
  computeNk();

  vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk
              << " Tk: " << Tk << " Nk: " << Nk << std::endl;

  // teste si les variables cumulees excedent le seuil
  if ((Mk - Sk) > alpha)
    jump = downwardJump;
  else if ((Tk - Nk) > alpha)
    jump = upwardJump;

#ifdef VP_DEBUG
  if (VP_DEBUG_MODE >= 2) {
    switch (jump) {
    case noJump:
      std::cout << "noJump " << std::endl;
      break;
    case downwardJump:
      std::cout << "downWardJump " << std::endl;
      break;
    case upwardJump:
      std::cout << "upwardJump " << std::endl;
      break;
    }
  }
#endif

  computeMean(signal);

  if ((jump == upwardJump) || (jump == downwardJump)) {
    vpCDEBUG(2) << "\n*** Reset the Hinkley test  ***\n";

    Sk = 0;
    Mk = 0;
    Tk = 0;
    Nk = 0;
    nsignal = 0;
    // Debut modif FS le 03/09/2003
    mean = signal;
    // Fin modif FS le 03/09/2003
  }

  return (jump);
}

void vpHinkley::computeMean(double signal)
{
  // Debut modif FS le 03/09/2003
  // Lors d'une chute ou d'une remontee lente du signal, pariculierement
  // apres un saut, la moyenne a tendance a "deriver". Pour reduire ces
  // derives de la moyenne, elle n'est remise a jour avec la valeur
  // courante du signal que si un debut de saut potentiel n'est pas detecte.
  // if ( ((Mk-Sk) == 0) && ((Tk-Nk) == 0) )
  if ((std::fabs(Mk - Sk) <= std::fabs(vpMath::maximum(Mk, Sk)) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(Tk - Nk) <= std::fabs(vpMath::maximum(Tk, Nk)) * std::numeric_limits<double>::epsilon()))
    // Fin modif FS le 03/09/2003

    // Mise a jour de la moyenne.
    mean = (mean * (nsignal - 1) + signal) / (nsignal);
}
void vpHinkley::computeSk(double signal)
{

  // Calcul des variables cumulees
  Sk += signal - mean + dmin2;
}
void vpHinkley::computeMk()
{
  if (Sk > Mk)
    Mk = Sk;
}
void vpHinkley::computeTk(double signal)
{

  // Calcul des variables cumulees
  Tk += signal - mean - dmin2;
}
void vpHinkley::computeNk()
{
  if (Tk < Nk)
    Nk = Tk;
}

void vpHinkley::print(vpHinkley::vpHinkleyJumpType jump)
{
  switch (jump) {
  case noJump:
    std::cout << " No jump detected " << std::endl;
    break;
  case downwardJump:
    std::cout << " Jump downward detected " << std::endl;
    break;
  case upwardJump:
    std::cout << " Jump upward detected " << std::endl;
    break;
  default:
    std::cout << " Jump  detected " << std::endl;
    break;
  }
}