Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpRobust.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * M-Estimator and various influence function.
33  *
34  * Authors:
35  * Andrew Comport
36  * Jean Laneurit
37  *
38  *****************************************************************************/
39 
44 #ifndef CROBUST_HH
45 #define CROBUST_HH
46 
47 #include <visp3/core/vpColVector.h>
48 #include <visp3/core/vpConfig.h>
49 #include <visp3/core/vpMath.h>
50 
58 class VISP_EXPORT vpRobust
59 {
60 
61 public:
63  typedef enum { TUKEY, CAUCHY, HUBER } vpRobustEstimatorType;
64 
65 private:
67  vpColVector normres;
69  vpColVector sorted_normres;
71  vpColVector sorted_residues;
72 
74  double NoiseThreshold;
76  double sig_prev;
78  unsigned int it;
80  double swap;
82  unsigned int size;
83 
84 public:
86  explicit vpRobust(unsigned int n_data);
87  vpRobust();
88  vpRobust(const vpRobust &other);
89 
91  virtual ~vpRobust(){};
92 
94  void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights);
95 
97  void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, const vpColVector &all_residues,
98  vpColVector &weights);
99 
100  vpRobust &operator=(const vpRobust &other);
101 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
102  vpRobust &operator=(const vpRobust &&other);
103 #endif
104 
106  void resize(unsigned int n_data);
107 
109  void setIteration(const unsigned int iter) { it = iter; }
110 
115  inline void setThreshold(const double noise_threshold) { NoiseThreshold = noise_threshold; }
116 
118  vpColVector simultMEstimator(vpColVector &residues);
119 
120  // public :
121  // double residualMedian ;
122  // double normalizedResidualMedian ;
123  // private:
124  // double median(const vpColVector &x);
125  // double median(const vpColVector &x, vpColVector &weights);
126 
127 private:
129  double computeNormalizedMedian(vpColVector &all_normres, const vpColVector &residues, const vpColVector &all_residues,
130  const vpColVector &weights);
131 
133  double simultscale(vpColVector &x);
134 
135  //---------------------------------
136  // Partial derivative of loss function with respect to the residue
137  //---------------------------------
140  void psiTukey(double sigma, vpColVector &x, vpColVector &w);
143  void psiCauchy(double sigma, vpColVector &x, vpColVector &w);
145  void psiHuber(double sigma, vpColVector &x, vpColVector &w);
147 
150  double simult_chi_huber(double x);
151 
152  //---------------------------------
153  // Constrained Partial derivative of loss function with respect to the scale
154  //---------------------------------
157  double constrainedChi(vpRobustEstimatorType method, double x);
160  double constrainedChiTukey(double x);
162  double constrainedChiCauchy(double x);
164  double constrainedChiHuber(double x);
166 
167 #if !defined(VISP_HAVE_FUNC_ERFC) && !defined(VISP_HAVE_FUNC_STD_ERFC)
168  //---------------------------------
169  // Mathematic functions used to calculate the Expectation
170  //---------------------------------
173  double erf(double x);
174  double gammp(double a, double x);
175  void gser(double *gamser, double a, double x, double *gln);
176  void gcf(double *gammcf, double a, double x, double *gln);
177  double gammln(double xx);
179 #endif
180 
183  void exch(double &A, double &B)
185  {
186  swap = A;
187  A = B;
188  B = swap;
189  }
191  int partition(vpColVector &a, int l, int r);
193  double select(vpColVector &a, int l, int r, int k);
195 };
196 
197 #endif
virtual ~vpRobust()
Destructor.
Definition: vpRobust.h:91
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:115
vpRobustEstimatorType
Enumeration of influence functions.
Definition: vpRobust.h:63
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:109