Visual Servoing Platform  version 3.6.1 under development (2024-03-04)
vpAdaptiveGain.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Adaptive gain.
32  */
33 
38 /* --- VISP --- */
39 #include <visp3/core/vpColVector.h>
40 #include <visp3/core/vpDebug.h>
41 #include <visp3/vs/vpAdaptiveGain.h>
42 
43 #include <cmath> // std::fabs
44 #include <iostream>
45 #include <limits> // numeric_limits
46 
47 const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
48 const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666;
49 const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666;
50 
51 vpAdaptiveGain::vpAdaptiveGain() : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
52 {
53  this->initFromVoid();
54 
55  return;
56 }
57 
58 vpAdaptiveGain::vpAdaptiveGain(double c) : coeff_a(), coeff_b(), coeff_c(), lambda(1.) { initFromConstant(c); }
59 
60 vpAdaptiveGain::vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
61  : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
62 {
63  initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
64 }
65 
67 {
68  this->coeff_a = 0;
69  this->coeff_b = 1;
70  this->coeff_c = c;
71  return;
72 }
73 
75 {
78  return;
79 }
80 
81 void vpAdaptiveGain::initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
82 {
83  this->coeff_a = gain_at_zero - gain_at_infinity;
84  // if (0 == this ->coeff_a)
85  if (std::fabs(this->coeff_a) <= std::numeric_limits<double>::epsilon()) {
86  this->coeff_b = 0;
87  }
88  else {
89  this->coeff_b = slope_at_zero / (this->coeff_a);
90  }
91  this->coeff_c = gain_at_infinity;
92 
93  return;
94 }
95 
97 {
98  double res = this->coeff_a + this->coeff_c;
99 
100  this->coeff_a = 0;
101  this->coeff_b = 1;
102  this->coeff_c = res;
103 
104  return res;
105 }
106 
107 double vpAdaptiveGain::value_const(double x) const
108 {
109  double res = this->coeff_a * exp(-this->coeff_b * x) + this->coeff_c;
110 
111  return res;
112 }
113 
115 {
116  double res = this->coeff_c;
117 
118  return res;
119 }
120 
121 double vpAdaptiveGain::value(double x) const
122 {
123  this->lambda = this->value_const(x);
124 
125  return lambda;
126 }
127 
128 double vpAdaptiveGain::limitValue(void) const
129 {
130  this->lambda = this->limitValue_const();
131 
132  return lambda;
133 }
134 
135 double vpAdaptiveGain::operator()(double x) const { return this->value(x); }
136 
137 double vpAdaptiveGain::operator()(void) const { return this->limitValue(); }
138 
139 double vpAdaptiveGain::operator()(const vpColVector &x) const { return this->value(x.infinityNorm()); }
140 
141 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda)
142 {
143  os << "Zero= " << lambda.coeff_a + lambda.coeff_c << "\tInf= " << lambda.coeff_c
144  << "\tSlope= " << lambda.coeff_a * lambda.coeff_b;
145 
146  return os;
147 }
Adaptive gain computation.
double value_const(double x) const
double operator()(void) const
double setConstant(void)
static const double DEFAULT_LAMBDA_ZERO
double limitValue_const(void) const
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
void initFromVoid(void)
static const double DEFAULT_LAMBDA_INFINITY
static const double DEFAULT_LAMBDA_SLOPE
void initFromConstant(double c)
double limitValue(void) const
double value(double x) const
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:523
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double infinityNorm() const