Main MRPT website > C++ reference for MRPT 1.5.7
robust_kernels.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #ifndef mrpt_robust_kernels_H
11 #define mrpt_robust_kernels_H
12 
13 #include <mrpt/utils/types.h>
14 
15 namespace mrpt
16 {
17 namespace math
18 {
19  /** \addtogroup mrpt_base_grp
20  * @{ */
21 
22  /** The different types of kernels for usage within a robustified least-squares estimator.
23  * \sa Use these types as arguments of the template RobustKernel<>
24  */
26  {
27  rkLeastSquares = 0, //!< No robust kernel, use standard least squares: rho(r)= 1/2 * r^2
28  rkPseudoHuber //!< Pseudo-huber robust kernel
29  };
30 
31  // Generic declaration.
32  template <TRobustKernelType KERNEL_TYPE, typename T=double> struct RobustKernel;
33 
34  /** No robust kernel, use standard least squares: rho(r) = r^2 */
35  template <typename T>
37  {
38  T param_sq; //!< The kernel parameter (the "threshold") squared [Not used in this class, provided for consistency with the other classes]
39 
40  /** Evaluates the kernel function for the squared error r2 and returns robustified squared error and derivatives of sqrt(2*rho(r)) at this point. */
41  inline T eval(const T r2, T & out_1st_deriv, T & out_2nd_deriv)
42  {
43  out_1st_deriv = 1; out_2nd_deriv = 0;
44  return r2; // return: 2*cost; cost: 0.5* |r|^2
45  }
46  };
47 
48  /** Pseudo-huber robust kernel: rho(r) = 2 * delta^2 * ( -1+sqrt( 1+ r^2/delta^2 ) ) */
49  template <typename T>
51  {
52  T param_sq; //!< The kernel parameter (the "threshold") squared.
53 
54  /** Evaluates the kernel function for the squared error r2 and returns robustified squared error and derivatives of sqrt(2*rho(r)) at this point. */
55  inline T eval(const T r2, T & out_1st_deriv, T & out_2nd_deriv)
56  {
57  const T param_sq_inv = 1.0/param_sq;
58  const T a = 1+r2*param_sq_inv;
59  const T b = std::sqrt(a);
60  out_1st_deriv = 1./b;
61  out_2nd_deriv = -0.5*param_sq_inv*out_1st_deriv/a;
62  return 2*param_sq*(b-1);; // return: 2*cost
63  }
64  };
65 
66 
67  /** @} */ // end of grouping
68 }
69 }
70 
71 #endif
T eval(const T r2, T &out_1st_deriv, T &out_2nd_deriv)
Evaluates the kernel function for the squared error r2 and returns robustified squared error and deri...
T param_sq
The kernel parameter (the "threshold") squared [Not used in this class, provided for consistency with...
Pseudo-huber robust kernel.
T eval(const T r2, T &out_1st_deriv, T &out_2nd_deriv)
Evaluates the kernel function for the squared error r2 and returns robustified squared error and deri...
No robust kernel, use standard least squares: rho(r)= 1/2 * r^2.
TRobustKernelType
The different types of kernels for usage within a robustified least-squares estimator.
GLubyte GLubyte b
Definition: glext.h:5575
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
T param_sq
The kernel parameter (the "threshold") squared.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019