MRPT  2.0.4
robust_kernels.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
12 #include <cmath> // std::sqrt()
13 
14 namespace mrpt::math
15 {
16 /** \addtogroup mrpt_math_grp
17  * @{ */
18 
19 /** The different types of kernels for usage within a robustified least-squares
20  * estimator.
21  * \sa Use these types as arguments of the template RobustKernel<>
22  */
24 {
25  /** No robust kernel, use standard least squares: rho(r)= 1/2 * r^2 */
27  /** Pseudo-huber robust kernel */
29 };
30 
31 // Generic declaration.
32 template <TRobustKernelType KERNEL_TYPE, typename T = double>
33 struct RobustKernel;
34 
35 /** No robust kernel, use standard least squares: rho(r) = r^2 */
36 template <typename T>
38 {
39  /** The kernel parameter (the "threshold") squared [Not used in this class,
40  * provided for consistency with the other classes] */
41  T param_sq = 1;
42 
43  /** Evaluates the kernel function for the squared error r2 and returns
44  * robustified squared error and derivatives of sqrt(2*rho(r)) at this
45  * point. */
46  inline T eval(const T r2, T& out_1st_deriv, T& out_2nd_deriv)
47  {
48  out_1st_deriv = 1;
49  out_2nd_deriv = 0;
50  return r2; // return: 2*cost; cost: 0.5* |r|^2
51  }
52 };
53 
54 /** Pseudo-huber robust kernel: rho(r) = 2 * delta^2 * ( -1+sqrt( 1+
55  * r^2/delta^2 ) ) */
56 template <typename T>
58 {
59  /** The kernel parameter (the "threshold") squared. */
60  T param_sq = 1;
61 
62  /** Evaluates the kernel function for the squared error r2 and returns
63  * robustified squared error and derivatives of sqrt(2*rho(r)) at this
64  * point. */
65  inline T eval(const T r2, T& out_1st_deriv, T& out_2nd_deriv)
66  {
67  const T param_sq_inv = 1.0 / param_sq;
68  const T a = 1 + r2 * param_sq_inv;
69  const T b = std::sqrt(a);
70  out_1st_deriv = 1. / b;
71  out_2nd_deriv = -0.5 * param_sq_inv * out_1st_deriv / a;
72  return 2 * param_sq * (b - 1);
73  ; // return: 2*cost
74  }
75 };
76 
77 /** @} */ // end of grouping
78 } // namespace mrpt::math
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...
Pseudo-huber robust kernel.
This base provides a set of functions for maths stuff.
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.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020