Visual Servoing Platform  version 3.6.1 under development (2024-07-15)
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 
48 const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
49 const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666;
50 const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666;
51 
52 vpAdaptiveGain::vpAdaptiveGain() : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
53 {
54  this->initFromVoid();
55 
56  return;
57 }
58 
59 vpAdaptiveGain::vpAdaptiveGain(double c) : coeff_a(), coeff_b(), coeff_c(), lambda(1.) { initFromConstant(c); }
60 
61 vpAdaptiveGain::vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
62  : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
63 {
64  initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
65 }
66 
68 {
69  this->coeff_a = 0;
70  this->coeff_b = 1;
71  this->coeff_c = c;
72  return;
73 }
74 
76 {
79  return;
80 }
81 
82 void vpAdaptiveGain::initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
83 {
84  this->coeff_a = gain_at_zero - gain_at_infinity;
85  // if (0 == this ->coeff_a)
86  if (std::fabs(this->coeff_a) <= std::numeric_limits<double>::epsilon()) {
87  this->coeff_b = 0;
88  }
89  else {
90  this->coeff_b = slope_at_zero / (this->coeff_a);
91  }
92  this->coeff_c = gain_at_infinity;
93 
94  return;
95 }
96 
98 {
99  double res = this->coeff_a + this->coeff_c;
100 
101  this->coeff_a = 0;
102  this->coeff_b = 1;
103  this->coeff_c = res;
104 
105  return res;
106 }
107 
108 double vpAdaptiveGain::value_const(double x) const
109 {
110  double res = this->coeff_a * exp(-this->coeff_b * x) + this->coeff_c;
111 
112  return res;
113 }
114 
116 {
117  double res = this->coeff_c;
118 
119  return res;
120 }
121 
122 double vpAdaptiveGain::value(double x) const
123 {
124  this->lambda = this->value_const(x);
125 
126  return lambda;
127 }
128 
129 double vpAdaptiveGain::limitValue(void) const
130 {
131  this->lambda = this->limitValue_const();
132 
133  return lambda;
134 }
135 
136 double vpAdaptiveGain::operator()(double x) const { return this->value(x); }
137 
138 double vpAdaptiveGain::operator()(void) const { return this->limitValue(); }
139 
140 double vpAdaptiveGain::operator()(const vpColVector &x) const { return this->value(x.infinityNorm()); }
141 
142 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda)
143 {
144  os << "Zero= " << lambda.coeff_a + lambda.coeff_c << "\tInf= " << lambda.coeff_c
145  << "\tSlope= " << lambda.coeff_a * lambda.coeff_b;
146 
147  return os;
148 }
149 
150 END_VISP_NAMESPACE
Adaptive gain computation.
double value_const(double x) const
double operator()(void) const
double setConstant(void)
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
static const double DEFAULT_LAMBDA_ZERO
double value(double x) const
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:611
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double infinityNorm() const