MRPT  1.9.9
num_jacobian.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 #pragma once
10 
11 #include <mrpt/core/exceptions.h>
12 #include <functional>
13 
14 namespace mrpt::math
15 {
16 /** Estimate the Jacobian of a multi-dimensional function around a point "x",
17  * using finite differences of a given size in each input dimension.
18  * The template argument USERPARAM is for the data can be passed to the
19  * functor.
20  * If it is not required, set to "int" or any other basic type.
21  *
22  * This is a generic template which works with:
23  * VECTORLIKE: vector_float, CVectorDouble, CVectorFixed<>, double [N],
24  * ...
25  * MATRIXLIKE: CMatrixDynamic, CMatrixFixed
26  * \ingroup mrpt_math_grp
27  */
28 template <
29  class VECTORLIKE, class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE,
30  class USERPARAM>
32  const VECTORLIKE& x,
33  const std::function<void(
34  const VECTORLIKE& x, const USERPARAM& y, VECTORLIKE3& out)>& functor,
35  const VECTORLIKE2& increments, const USERPARAM& userParam,
36  MATRIXLIKE& out_Jacobian)
37 {
39  ASSERT_(x.size() > 0 && increments.size() == x.size());
40 
41  size_t m = 0; // will determine automatically on the first call to "f":
42  const size_t n = x.size();
43 
44  for (size_t j = 0; j < n; j++)
45  {
46  ASSERT_(increments[j] > 0);
47  } // Who knows...
48 
49  VECTORLIKE3 f_minus, f_plus;
50  VECTORLIKE x_mod(x);
51 
52  // Evaluate the function "i" with increments in the "j" input x variable:
53  for (size_t j = 0; j < n; j++)
54  {
55  // Create the modified "x" vector:
56  x_mod[j] = x[j] + increments[j];
57  functor(x_mod, userParam, f_plus);
58 
59  x_mod[j] = x[j] - increments[j];
60  functor(x_mod, userParam, f_minus);
61 
62  x_mod[j] = x[j]; // Leave as original
63  const double Ax_2_inv = 0.5 / increments[j];
64 
65  // The first time?
66  if (j == 0)
67  {
68  m = f_plus.size();
69  out_Jacobian.resize(m, n);
70  }
71 
72  for (size_t i = 0; i < m; i++)
73  out_Jacobian(i, j) = Ax_2_inv * (f_plus[i] - f_minus[i]);
74 
75  } // end for j
76 
77  MRPT_END
78 }
79 } // namespace mrpt::math
#define MRPT_START
Definition: exceptions.h:241
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020