Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #pragma once
11 
12 #include <cmath> // std::sqrt()
13 
14 namespace mrpt
15 {
16 namespace math
17 {
18 /** \addtogroup mrpt_math_grp
19  * @{ */
20 
21 /** The different types of kernels for usage within a robustified least-squares
22  * estimator.
23  * \sa Use these types as arguments of the template RobustKernel<>
24  */
26 {
27  /** No robust kernel, use standard least squares: rho(r)= 1/2 * r^2 */
29  /** Pseudo-huber robust kernel */
31 };
32 
33 // Generic declaration.
34 template <TRobustKernelType KERNEL_TYPE, typename T = double>
35 struct RobustKernel;
36 
37 /** No robust kernel, use standard least squares: rho(r) = r^2 */
38 template <typename T>
40 {
41  /** The kernel parameter (the "threshold") squared [Not used in this class,
42  * provided for consistency with the other classes] */
44 
45  /** Evaluates the kernel function for the squared error r2 and returns
46  * robustified squared error and derivatives of sqrt(2*rho(r)) at this
47  * point. */
48  inline T eval(const T r2, T& out_1st_deriv, T& out_2nd_deriv)
49  {
50  out_1st_deriv = 1;
51  out_2nd_deriv = 0;
52  return r2; // return: 2*cost; cost: 0.5* |r|^2
53  }
54 };
55 
56 /** Pseudo-huber robust kernel: rho(r) = 2 * delta^2 * ( -1+sqrt( 1+
57  * r^2/delta^2 ) ) */
58 template <typename T>
60 {
61  /** The kernel parameter (the "threshold") squared. */
63 
64  /** Evaluates the kernel function for the squared error r2 and returns
65  * robustified squared error and derivatives of sqrt(2*rho(r)) at this
66  * point. */
67  inline T eval(const T r2, T& out_1st_deriv, T& out_2nd_deriv)
68  {
69  const T param_sq_inv = 1.0 / param_sq;
70  const T a = 1 + r2 * param_sq_inv;
71  const T b = std::sqrt(a);
72  out_1st_deriv = 1. / b;
73  out_2nd_deriv = -0.5 * param_sq_inv * out_1st_deriv / a;
74  return 2 * param_sq * (b - 1);
75  ; // return: 2*cost
76  }
77 };
78 
79 /** @} */ // end of grouping
80 } // namespace math
81 } // namespace mrpt
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.
GLubyte GLubyte b
Definition: glext.h:6279
TRobustKernelType
The different types of kernels for usage within a robustified least-squares estimator.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
T param_sq
The kernel parameter (the "threshold") squared.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019