starter/math/robust_estimators.h

   1 /*
   2 Copyright 2005, 2006 Computer Vision Lab,
   3 Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
   4 All rights reserved.
   5
   6 This file is part of BazAR.
   7
   8 BazAR is free software; you can redistribute it and/or modify it under the
   9 terms of the GNU General Public License as published by the Free Software
  10 Foundation; either version 2 of the License, or (at your option) any later
  11 version.
  12
  13 BazAR is distributed in the hope that it will be useful, but WITHOUT ANY
  14 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
  15 PARTICULAR PURPOSE. See the GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License along with
  18 BazAR; if not, write to the Free Software Foundation, Inc., 51 Franklin
  19 Street, Fifth Floor, Boston, MA 02110-1301, USA
  20 */
  21 #ifndef ROBUST_ESTIMATORS_H
  22 #define ROBUST_ESTIMATORS_H
  23
  24 #include <math.h>
  25
  26 //! \ingroup starter
  27 //@{
  28
  29 double rho_tukey_without_sqrt(const double x2, const double c);
  30 double rho_tukey_without_sqrt_derivative(const double x2, const double c);
  31
  32 double rho_huber_without_sqrt(const double x2, const double c);
  33 double rho_huber_without_sqrt_derivative(const double x2, const double c);
  34
  35
  36 inline double rho_tukey_without_sqrt(const double x2, const double c)
  37 {
  38   double c2 = c * c;
  39
  40   if (x2 > c2)
  41     return c2 / 6;
  42   else
  43   {
  44     double t = 1. - x2 / c2;
  45
  46     return c2 / 6 * (1. - t * t * t);
  47   }
  48 }
  49
  50 inline double rho_tukey_without_sqrt_derivative(const double x2, const double c)
  51 {
  52   double c2 = c * c;
  53
  54   if (x2 > c2)
  55     return 0;
  56   else
  57   {
  58     double t = 1. - x2 / c2;
  59
  60     return t * t / 2.;
  61   }
  62 }
  63
  64 inline double rho_huber_without_sqrt(const double x2, const double c)
  65 {
  66   double c2 = c * c;
  67
  68   if (x2 < c2)
  69     return x2;
  70   else
  71     return 2 * c * sqrt(x2) - c2;
  72 }
  73
  74 inline double rho_huber_without_sqrt_derivative(const double x2, const double c)
  75 {
  76   double c2 = c * c;
  77
  78   if (x2 < c2)
  79     return 1;
  80   else
  81     return c / sqrt(x2);
  82 }
  83
  84 //@}
  85 #endif // ROBUST_ESTIMATORS_H