ViSP  2.9.0
vpRobust.h
1 /****************************************************************************
2  *
3  * $Id: vpRobust.h 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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  * M-Estimator and various influence function.
36  *
37  * Authors:
38  * Andrew Comport
39  * Jean Laneurit
40  *
41  *****************************************************************************/
42 
49 #ifndef CROBUST_HH
50 #define CROBUST_HH
51 
52 #include <visp/vpColVector.h>
53 #include <visp/vpMath.h>
54 
55 
63 class VISP_EXPORT vpRobust
64 {
65 
66 public:
68  typedef enum
69  {
72  // MCLURE,
73  HUBER
74  } vpRobustEstimatorType;
75 
76 private:
77 
79  vpColVector normres;
81  vpColVector sorted_normres;
83  vpColVector sorted_residues;
84 
86  double NoiseThreshold;
88  double sig_prev;
90  unsigned int it;
92  double swap;
94  unsigned int size;
95 
96 public:
97 
99  vpRobust(unsigned int n_data);
100 
102  virtual ~vpRobust(){};
103 
105  void resize(unsigned int n_data);
106 
108  void MEstimator(const vpRobustEstimatorType method,
109  const vpColVector &residues,
110  vpColVector &weights);
111 
113  void MEstimator(const vpRobustEstimatorType method,
114  const vpColVector &residues,
115  const vpColVector& all_residues,
116  vpColVector &weights);
117 
119  vpColVector simultMEstimator(vpColVector &residues);
120 
122  void setIteration(const unsigned int iter){it=iter;}
123 
128  inline void setThreshold(const double noise_threshold) {
129  NoiseThreshold=noise_threshold;
130  }
131 
132 //public :
133 //double residualMedian ;
134 //double normalizedResidualMedian ;
135 // private:
136 // double median(const vpColVector &x);
137 // double median(const vpColVector &x, vpColVector &weights);
138 
139  private:
141  double computeNormalizedMedian(vpColVector &all_normres,
142  const vpColVector &residues,
143  const vpColVector &all_residues,
144  const vpColVector &weights
145  );
146 
147 
149  double scale(vpRobustEstimatorType method, vpColVector &x);
150  double simultscale(vpColVector &x);
151 
152 
153  //---------------------------------
154  // Partial derivative of loss function with respect to the residue
155  //---------------------------------
158  void psiTukey(double sigma, vpColVector &x, vpColVector &w);
161  void psiCauchy(double sigma, vpColVector &x, vpColVector &w);
163  void psiMcLure(double sigma, vpColVector &x, vpColVector &w);
165  void psiHuber(double sigma, vpColVector &x, vpColVector &w);
167 
170  double simult_chi_huber(double x);
171 
172  //---------------------------------
173  // Constrained Partial derivative of loss function with respect to the scale
174  //---------------------------------
177  double constrainedChi(vpRobustEstimatorType method, double x);
180  double constrainedChiTukey(double x);
182  double constrainedChiCauchy(double x);
184  double constrainedChiHuber(double x);
186 
187  //---------------------------------
188  // Mathematic functions used to calculate the Expectation
189  //---------------------------------
192  double erf(double x);
193  double gammp(double a, double x);
194  void gser(double *gamser, double a, double x, double *gln);
195  void gcf(double *gammcf, double a, double x, double *gln);
196  double gammln(double xx);
198 
201  void exch(double &A, double &B){swap = A; A = B; B = swap;}
204  unsigned int partition(vpColVector &a, unsigned int l, unsigned int r);
206  double select(vpColVector &a, unsigned int l, unsigned int r, unsigned int k);
208 };
209 
210 #endif
virtual ~vpRobust()
Destructor.
Definition: vpRobust.h:102
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:63
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:128
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:122