vpAdaptiveGain.cpp

/****************************************************************************
 *
 * $Id: vpAdaptiveGain.cpp 4641 2014-02-05 12:42:03Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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:
 * Adaptive gain.
 *
 * Authors:
 * Nicolas Mansard
 *
 *****************************************************************************/
/* --- VISP --- */
#include <visp/vpColVector.h>
#include <visp/vpDebug.h>
#include <visp/vpAdaptiveGain.h>

#include <iostream>
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits

const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666;
const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE  = 1.666;

/* -------------------------------------------------------------------------- */
/* --- CONSTRUCTION --------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

vpAdaptiveGain::vpAdaptiveGain ()
  :
  coeff_a (),
  coeff_b (),
  coeff_c (),
  lambda(1.)
{
  vpDEBUG_TRACE (10, "# Entree constructeur par default.");
  this ->initFromVoid ();

  vpDEBUG_TRACE (10, "# Sortie constructeur par default.");
  return;
}

vpAdaptiveGain::vpAdaptiveGain (double c)
{
  initFromConstant(c);
}

vpAdaptiveGain::vpAdaptiveGain (double gain_at_zero, double gain_at_infinity, double slope_at_zero)
{
  initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
}

/* -------------------------------------------------------------------------- */
/* --- INIT ----------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

void vpAdaptiveGain::initFromConstant (const double c)
{
    vpDEBUG_TRACE (10, "# Entree.");

    this ->coeff_a = 0;
    this ->coeff_b = 1;
    this ->coeff_c = c;

    vpDEBUG_TRACE (10, "# Sortie.");
    return;
}


void vpAdaptiveGain::initFromVoid (void)
{
  vpDEBUG_TRACE (10, "# Entree.");

  this ->initStandard (vpAdaptiveGain::DEFAULT_LAMBDA_ZERO,
                       vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY,
                       vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE);

  vpDEBUG_TRACE (10, "# Sortie.");
  return;
}


void vpAdaptiveGain::initStandard (const double gain_at_zero,
                                   const double gain_at_infinity,
                                   const double slope_at_zero)
{
  vpDEBUG_TRACE (10, "# Entree.");

  this ->coeff_a = gain_at_zero - gain_at_infinity;
  //if (0 == this ->coeff_a)
  if (std::fabs(this ->coeff_a) <= std::numeric_limits<double>::epsilon())
    {
      this ->coeff_b = 0;
    }
  else
    {
      this ->coeff_b = slope_at_zero / ( this ->coeff_a);
    }
  this ->coeff_c = gain_at_infinity;

  vpDEBUG_TRACE (10, "# Sortie :a,b,c= %.3f,%.3f,%.3f.",
           this ->coeff_a, this ->coeff_b, this ->coeff_c);
  return;
}



/* -------------------------------------------------------------------------- */
/* --- MODIFICATOR ---------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

double vpAdaptiveGain::setConstant (void)
{
  vpDEBUG_TRACE (10, "# Entree.");

  double res = this ->coeff_a + this ->coeff_c;

  this ->coeff_a = 0;
  this ->coeff_b = 1;
  this ->coeff_c = res;

  vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
  return res;
}

/* -------------------------------------------------------------------------- */
/* --- VALEUR --------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

double vpAdaptiveGain::value_const (const double x) const
{
  vpDEBUG_TRACE (10, "# Entree.");

  double res = this ->coeff_a * exp (- this ->coeff_b * x) + this ->coeff_c;

  vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
  return res;
}

double vpAdaptiveGain::limitValue_const (void) const
{
  vpDEBUG_TRACE (10, "# Entree.");

  double res = this ->coeff_c;

  vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
  return res;
}

double vpAdaptiveGain::value (const double x) const
{
  vpDEBUG_TRACE (10, "# Entree.");

  this ->lambda = this ->value_const (x);

  vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
  return lambda;
}


double vpAdaptiveGain:: limitValue (void) const
{
  vpDEBUG_TRACE (10, "# Entree.");

  this ->lambda = this ->limitValue_const ();

  vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
  return lambda;
}

/* -------------------------------------------------------------------------- */
/* --- ACCESSORS ------------------------------------------------------------ */
/* -------------------------------------------------------------------------- */



// double vpAdaptiveGain::
// getLastValue (void) const
// {
//   return this ->lambda;
// }

double vpAdaptiveGain::operator() (const double x) const
{
  return this ->value (x);
}

double vpAdaptiveGain::operator() (void) const
{
  return this ->limitValue ();
}

double vpAdaptiveGain::operator() (const vpColVector & x) const
{
  return this ->value (x .infinityNorm());
}


//   double operator() (double val_e)  const;
//   double operator()  (const CColVector & e) const;

/* -------------------------------------------------------------------------- */
/* --- OUTPUT --------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */



VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpAdaptiveGain& lambda)
{
  os << "Zero= " << lambda .coeff_a + lambda .coeff_c
     << "\tInf= " << lambda .coeff_c
     << "\tDeriv= " << lambda .coeff_a * lambda .coeff_b;

  return os;
}