MRPT  1.9.9
num_jacobian.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 |
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, CArrayNumeric<>, double [N],
24  * ...
25  * MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
26  */
27 template <class VECTORLIKE, class VECTORLIKE2, class VECTORLIKE3,
28  class MATRIXLIKE, class USERPARAM>
30  const VECTORLIKE& x,
31  const std::function<void(
32  const VECTORLIKE& x, const USERPARAM& y, VECTORLIKE3& out)>& functor,
33  const VECTORLIKE2& increments, const USERPARAM& userParam,
34  MATRIXLIKE& out_Jacobian)
35 {
37  ASSERT_(x.size() > 0 && increments.size() == x.size());
38
39  size_t m = 0; // will determine automatically on the first call to "f":
40  const size_t n = x.size();
41
42  for (size_t j = 0; j < n; j++)
43  {
44  ASSERT_(increments[j] > 0);
45  } // Who knows...
46
47  VECTORLIKE3 f_minus, f_plus;
48  VECTORLIKE x_mod(x);
49
50  // Evaluate the function "i" with increments in the "j" input x variable:
51  for (size_t j = 0; j < n; j++)
52  {
53  // Create the modified "x" vector:
54  x_mod[j] = x[j] + increments[j];
55  functor(x_mod, userParam, f_plus);
56
57  x_mod[j] = x[j] - increments[j];
58  functor(x_mod, userParam, f_minus);
59
60  x_mod[j] = x[j]; // Leave as original
61  const double Ax_2_inv = 0.5 / increments[j];
62
63  // The first time?
64  if (j == 0)
65  {
66  m = f_plus.size();
67  out_Jacobian.setSize(m, n);
68  }
69
70  for (size_t i = 0; i < m; i++)
71  out_Jacobian.get_unsafe(i, j) = Ax_2_inv * (f_plus[i] - f_minus[i]);
72
73  } // end for j
74
75  MRPT_END
76 }
77 } // End of namespace
78
79
#define MRPT_START
Definition: exceptions.h:262
GLenum GLsizei n
Definition: glext.h:5074
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:29
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
#define MRPT_END
Definition: exceptions.h:266
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLint x
Definition: glext.h:3538

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020