ViSP  2.6.2
vpAdaptiveGain.cpp
1 /****************************************************************************
2  *
3  * $Id: vpAdaptiveGain.cpp 3530 2012-01-03 10:52:12Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Adaptive gain.
36  *
37  * Authors:
38  * Nicolas Mansard
39  *
40  *****************************************************************************/
45 /* --- VISP --- */
46 #include <visp/vpColVector.h>
47 #include <visp/vpDebug.h>
48 #include <visp/vpAdaptiveGain.h>
49 
50 #include <iostream>
51 #include <cmath> // std::fabs
52 #include <limits> // numeric_limits
53 
54 const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
55 const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINI = 0.1666;
56 const double vpAdaptiveGain::DEFAULT_LAMBDA_PENTE = 1.666;
57 
58 /* -------------------------------------------------------------------------- */
59 /* --- CONSTRUCTION --------------------------------------------------------- */
60 /* -------------------------------------------------------------------------- */
61 
67  :
68  coeff_a (),
69  coeff_b (),
70  coeff_c ()
71 {
72  vpDEBUG_TRACE (10, "# Entree constructeur par default.");
73  this ->initFromVoid ();
74 
75  vpDEBUG_TRACE (10, "# Sortie constructeur par default.");
76  return;
77 }
78 
79 /* -------------------------------------------------------------------------- */
80 /* --- INIT ----------------------------------------------------------------- */
81 /* -------------------------------------------------------------------------- */
82 
90 initFromConstant (const double lambda)
91 {
92  vpDEBUG_TRACE (10, "# Entree.");
93 
94  this ->coeff_a = 0;
95  this ->coeff_b = 1;
96  this ->coeff_c = lambda;
97 
98  vpDEBUG_TRACE (10, "# Sortie.");
99  return;
100 }
101 
102 
109 void vpAdaptiveGain::
111 {
112  vpDEBUG_TRACE (10, "# Entree.");
113 
114  this ->initStandard (vpAdaptiveGain::DEFAULT_LAMBDA_ZERO,
117 
118  vpDEBUG_TRACE (10, "# Sortie.");
119  return;
120 }
121 
122 
132 void vpAdaptiveGain::
133 initStandard (const double en_zero,
134  const double en_infini,
135  const double pente_en_zero)
136 {
137  vpDEBUG_TRACE (10, "# Entree.");
138 
139  this ->coeff_a = en_zero - en_infini;
140  //if (0 == this ->coeff_a)
141  if (std::fabs(this ->coeff_a) <= std::numeric_limits<double>::epsilon())
142  {
143  this ->coeff_b = 0;
144  }
145  else
146  {
147  this ->coeff_b = pente_en_zero / ( this ->coeff_a);
148  }
149  this ->coeff_c = en_infini;
150 
151  vpDEBUG_TRACE (10, "# Sortie :a,b,c= %.3f,%.3f,%.3f.",
152  this ->coeff_a, this ->coeff_b, this ->coeff_c);
153  return;
154 }
155 
156 
157 
158 /* -------------------------------------------------------------------------- */
159 /* --- MODIFICATOR ---------------------------------------------------------- */
160 /* -------------------------------------------------------------------------- */
161 
167 double vpAdaptiveGain::
169 {
170  vpDEBUG_TRACE (10, "# Entree.");
171 
172  double res = this ->coeff_a + this ->coeff_c;
173 
174  this ->coeff_a = 0;
175  this ->coeff_b = 1;
176  this ->coeff_c = res;
177 
178  vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
179  return res;
180 }
181 
182 /* -------------------------------------------------------------------------- */
183 /* --- VALEUR --------------------------------------------------------------- */
184 /* -------------------------------------------------------------------------- */
185 
196 double vpAdaptiveGain::
197 value_const (const double val_e) const
198 {
199  vpDEBUG_TRACE (10, "# Entree.");
200 
201  double res = this ->coeff_a * exp (- this ->coeff_b * val_e)
202  + this ->coeff_c;
203 
204  vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
205  return res;
206 }
207 
213 double vpAdaptiveGain::
214 limitValue_const (void) const
215 {
216  vpDEBUG_TRACE (10, "# Entree.");
217 
218  double res = this ->coeff_c;
219 
220  vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
221  return res;
222 }
223 
224 
225 
237 double vpAdaptiveGain::
238 value (const double val_e) const
239 {
240  vpDEBUG_TRACE (10, "# Entree.");
241 
242  this ->lambda = this ->value_const (val_e);
243 
244  vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
245  return lambda;
246 }
247 
248 
255 double vpAdaptiveGain::
256 limitValue (void) const
257 {
258  vpDEBUG_TRACE (10, "# Entree.");
259 
260  this ->lambda = this ->limitValue_const ();
261 
262  vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
263  return lambda;
264 }
265 
266 /* -------------------------------------------------------------------------- */
267 /* --- ACCESSORS ------------------------------------------------------------ */
268 /* -------------------------------------------------------------------------- */
269 
270 
271 
272 // double vpAdaptiveGain::
273 // getLastValue (void) const
274 // {
275 // return this ->lambda;
276 // }
277 
286 double vpAdaptiveGain::
287 operator() (const double val_e) const
288 {
289  return this ->value (val_e);
290 }
291 
297 double vpAdaptiveGain::
298 operator() (void) const
299 {
300  return this ->limitValue ();
301 }
302 
311 double vpAdaptiveGain::
312 operator() (const vpColVector & e) const
313 {
314  return this ->value (e .infinityNorm());
315 }
316 
317 
318 // double operator() (double val_e) const;
319 // double operator() (const CColVector & e) const;
320 
321 /* -------------------------------------------------------------------------- */
322 /* --- OUTPUT --------------------------------------------------------------- */
323 /* -------------------------------------------------------------------------- */
324 
325 
326 
334 std::ostream&
335 operator<< (std::ostream &os, const vpAdaptiveGain& lambda)
336 {
337  os << "Zero= " << lambda .coeff_a + lambda .coeff_c
338  << "\tInf= " << lambda .coeff_c
339  << "\tDeriv= " << lambda .coeff_a * lambda .coeff_b;
340 
341  return os;
342 }
343 
344 
345 
346 /*
347  * Local variables:
348  * c-basic-offset: 4
349  * End:
350  */
#define vpDEBUG_TRACE
Definition: vpDebug.h:454
Adaptive gain computation.
static const double DEFAULT_LAMBDA_ZERO
void initFromConstant(double lambda)
void initFromVoid(void)
static const double DEFAULT_LAMBDA_PENTE
double limitValue_const(void) const
double operator()(void) const
double setConstant(void)
VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpImagePoint &ip)
Definition: vpImagePoint.h:529
static const double DEFAULT_LAMBDA_INFINI
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double limitValue(void) const
double value_const(double val_e) const
void initStandard(double en_zero, double en_infini, double pente_en_zero)
double value(double val_e) const