Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpRobust.h
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  * M-Estimator and various influence function.
32  */
33 
38 #ifndef vpRobust_h
39 #define vpRobust_h
40 
41 #include <visp3/core/vpColVector.h>
42 #include <visp3/core/vpConfig.h>
43 #include <visp3/core/vpMath.h>
44 
82 class VISP_EXPORT vpRobust
83 {
84 public:
86  typedef enum
87  {
90  HUBER
91  } vpRobustEstimatorType;
92 
93 private:
95  vpColVector m_normres;
97  vpColVector m_sorted_normres;
99  vpColVector m_sorted_residues;
100 
102  double m_mad_min;
104  double m_mad_prev;
105 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
107  unsigned int m_iter;
108 #endif
110  unsigned int m_size;
112  double m_mad;
113 
114 public:
115  vpRobust();
116  vpRobust(const vpRobust &other);
117 
119  virtual ~vpRobust() { };
120 
130  double getMedianAbsoluteDeviation() { return m_mad; };
131 
138  double getMinMedianAbsoluteDeviation() { return m_mad_min; };
139 
140  void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights);
141 
142  vpRobust &operator=(const vpRobust &other);
143 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
144  vpRobust &operator=(const vpRobust &&other);
145 #endif
146 
156  inline void setMinMedianAbsoluteDeviation(double mad_min) { m_mad_min = mad_min; }
157 
158 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
163  vp_deprecated explicit vpRobust(unsigned int n_data);
165  void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, const vpColVector &all_residues,
166  vpColVector &weights);
171  vp_deprecated void setIteration(unsigned int iter) { m_iter = iter; }
180  vp_deprecated inline void setThreshold(double mad_min) { m_mad_min = mad_min; }
181  vp_deprecated vpColVector simultMEstimator(vpColVector &residues);
183 #endif
184 private:
186  void resize(unsigned int n_data);
187 
188  //---------------------------------
189  // Partial derivative of loss function with respect to the residue
190  //---------------------------------
193  void psiTukey(double sigma, const vpColVector &x, vpColVector &w);
196  void psiCauchy(double sigma, const vpColVector &x, vpColVector &w);
198  void psiHuber(double sigma, const vpColVector &x, vpColVector &w);
200 
201 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
202  double computeNormalizedMedian(vpColVector &all_normres, const vpColVector &residues, const vpColVector &all_residues,
203  const vpColVector &weights);
205  double simultscale(const vpColVector &x);
207  double simult_chi_huber(double x);
208 
209  //---------------------------------
210  // Constrained Partial derivative of loss function with respect to the scale
211  //---------------------------------
214  double constrainedChi(vpRobustEstimatorType method, double x);
217  double constrainedChiTukey(double x);
219  double constrainedChiCauchy(double x);
221  double constrainedChiHuber(double x);
223 
224 #if !defined(VISP_HAVE_FUNC_ERFC) && !defined(VISP_HAVE_FUNC_STD_ERFC)
225  //---------------------------------
226  // Mathematic functions used to calculate the Expectation
227  //---------------------------------
230  double erf(double x);
231  double gammp(double a, double x);
232  void gser(double *gamser, double a, double x, double *gln);
233  void gcf(double *gammcf, double a, double x, double *gln);
234  double gammln(double xx);
236 #endif
237 #endif
238 
242  int partition(vpColVector &a, int l, int r);
244  double select(vpColVector &a, int l, int r, int k);
246 };
247 
248 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Contains an M-estimator and various influence function.
Definition: vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:88
@ CAUCHY
Cauchy influence function.
Definition: vpRobust.h:89
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:156
vp_deprecated void setThreshold(double mad_min)
Definition: vpRobust.h:180
double getMedianAbsoluteDeviation()
Definition: vpRobust.h:130
double getMinMedianAbsoluteDeviation()
Definition: vpRobust.h:138
virtual ~vpRobust()
Destructor.
Definition: vpRobust.h:119
vp_deprecated void setIteration(unsigned int iter)
Definition: vpRobust.h:171