Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
11 #include <mrpt/utils/core_defs.h>
12 
13 namespace 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 
69  MRPT_END
70  }
71 
72  } // End of MATH namespace
73 } // End of namespace
74 
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
GLsizei n
Definition: glew.h:5051
#define MRPT_END
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
#define MRPT_START
#define ASSERT_(f)
const GLdouble * m
Definition: glew.h:5094
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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018