Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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#pragma once
10
12
13namespace mrpt
14{
15 namespace math
16 {
17 /** Estimate the Jacobian of a multi-dimensional function around a point "x", 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 functor.
19 * If it is not required, set to "int" or any other basic type.
20 *
21 * This is a generic template which works with:
22 * VECTORLIKE: vector_float, CVectorDouble, CArrayNumeric<>, double [N], ...
23 * MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
24 */
25 template <class VECTORLIKE,class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM >
27 const VECTORLIKE &x,
28 void (*functor) (const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3 &out),
29 const VECTORLIKE2 &increments,
30 const USERPARAM &userParam,
31 MATRIXLIKE &out_Jacobian )
32 {
34 ASSERT_(x.size()>0 && increments.size() == x.size());
35
36 size_t m = 0; // will determine automatically on the first call to "f":
37 const size_t n = x.size();
38
39 for (size_t j=0;j<n;j++) { ASSERT_( increments[j]>0 ) } // Who knows...
40
41 VECTORLIKE3 f_minus, f_plus;
42 VECTORLIKE x_mod(x);
43
44 // Evaluate the function "i" with increments in the "j" input x variable:
45 for (size_t j=0;j<n;j++)
46 {
47 // Create the modified "x" vector:
48 x_mod[j]=x[j]+increments[j];
49 functor(x_mod,userParam, f_plus);
50
51 x_mod[j]=x[j]-increments[j];
52 functor(x_mod,userParam, f_minus);
53
54 x_mod[j]=x[j]; // Leave as original
55 const double Ax_2_inv = 0.5/increments[j];
56
57 // The first time?
58 if (j==0)
59 {
60 m = f_plus.size();
61 out_Jacobian.setSize(m,n);
62 }
63
64 for (size_t i=0;i<m;i++)
65 out_Jacobian.get_unsafe(i,j) = Ax_2_inv* (f_plus[i]-f_minus[i]);
66
67 } // end for j
68
70 }
71
72 } // End of MATH namespace
73} // End of namespace
74
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define MRPT_END
Definition: mrpt_macros.h:353
void estimateJacobian(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), 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:26
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Sat Jan 21 06:46:15 UTC 2023