numericalDerivative.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 // \callgraph
19 #pragma once
20 
21 #include <boost/function.hpp>
22 #ifdef __GNUC__
23 #pragma GCC diagnostic push
24 #pragma GCC diagnostic ignored "-Wunused-variable"
25 #endif
26 #include <boost/bind.hpp>
27 #ifdef __GNUC__
28 #pragma GCC diagnostic pop
29 #endif
30 
31 #include <gtsam/linear/VectorValues.h>
32 #include <gtsam/linear/JacobianFactor.h>
33 #include <gtsam/nonlinear/Values.h>
34 #include <gtsam/base/Lie.h>
35 
36 namespace gtsam {
37 
38 /*
39  * Note that all of these functions have two versions, a boost.function version and a
40  * standard C++ function pointer version. This allows reformulating the arguments of
41  * a function to fit the correct structure, which is useful for situations like
42  * member functions and functions with arguments not involved in the derivative:
43  *
44  * Usage of the boost bind version to rearrange arguments:
45  * for a function with one relevant param and an optional derivative:
46  * Foo bar(const Obj& a, boost::optional<Matrix&> H1)
47  * Use boost.bind to restructure:
48  * boost::bind(bar, _1, boost::none)
49  * This syntax will fix the optional argument to boost::none, while using the first argument provided
50  *
51  * For member functions, such as below, with an instantiated copy instanceOfSomeClass
52  * Foo SomeClass::bar(const Obj& a)
53  * Use boost bind as follows to create a function pointer that uses the member function:
54  * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1)
55  *
56  * For additional details, see the documentation:
57  * http://www.boost.org/doc/libs/release/libs/bind/bind.html
58  */
59 
60 
61 // a quick helper struct to get the appropriate fixed sized matrix from two value types
62 namespace internal {
63 template<class Y, class X=double>
64 struct FixedSizeMatrix {
65  typedef Eigen::Matrix<double,traits<Y>::dimension, traits<X>::dimension> type;
66 };
67 }
68 
77 template <class X, int N = traits<X>::dimension>
78 typename Eigen::Matrix<double, N, 1> numericalGradient(
79  boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
80  double factor = 1.0 / (2.0 * delta);
81 
82  BOOST_STATIC_ASSERT_MSG(
83  (boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
84  "Template argument X must be a manifold type.");
85  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
86 
87  // Prepare a tangent vector to perturb x with, only works for fixed size
88  typename traits<X>::TangentVector d;
89  d.setZero();
90 
91  Eigen::Matrix<double,N,1> g;
92  g.setZero();
93  for (int j = 0; j < N; j++) {
94  d(j) = delta;
95  double hxplus = h(traits<X>::Retract(x, d));
96  d(j) = -delta;
97  double hxmin = h(traits<X>::Retract(x, d));
98  d(j) = 0;
99  g(j) = (hxplus - hxmin) * factor;
100  }
101  return g;
102 }
103 
116 template <class Y, class X, int N = traits<X>::dimension>
117 // TODO Should compute fixed-size matrix
118 typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
119  boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
120  typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
121 
122  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
123  "Template argument Y must be a manifold type.");
124  typedef traits<Y> TraitsY;
125 
126  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
127  "Template argument X must be a manifold type.");
128  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
129  typedef traits<X> TraitsX;
130 
131  // get value at x, and corresponding chart
132  const Y hx = h(x);
133 
134  // Bit of a hack for now to find number of rows
135  const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx);
136  const size_t m = zeroY.size();
137 
138  // Prepare a tangent vector to perturb x with, only works for fixed size
139  Eigen::Matrix<double, N, 1> dx;
140  dx.setZero();
141 
142  // Fill in Jacobian H
143  Matrix H = Matrix::Zero(m, N);
144  const double factor = 1.0 / (2.0 * delta);
145  for (int j = 0; j < N; j++) {
146  dx(j) = delta;
147  const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
148  dx(j) = -delta;
149  const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
150  dx(j) = 0;
151  H.col(j) << (dy1 - dy2) * factor;
152  }
153  return H;
154 }
155 
157 template<class Y, class X>
158 typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
159  double delta = 1e-5) {
160  return numericalDerivative11<Y, X>(boost::bind(h, _1), x, delta);
161 }
162 
172 template<class Y, class X1, class X2, int N = traits<X1>::dimension>
173 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
174  const X1& x1, const X2& x2, double delta = 1e-5) {
175  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
176  "Template argument Y must be a manifold type.");
177  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
178  "Template argument X1 must be a manifold type.");
179  return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
180 }
181 
183 template<class Y, class X1, class X2>
184 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
185  const X2& x2, double delta = 1e-5) {
186  return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
187 }
188 
198 template<class Y, class X1, class X2, int N = traits<X2>::dimension>
199 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
200  const X1& x1, const X2& x2, double delta = 1e-5) {
201 // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
202 // "Template argument X1 must be a manifold type.");
203  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
204  "Template argument X2 must be a manifold type.");
205  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1), x2, delta);
206 }
207 
209 template<class Y, class X1, class X2>
210 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
211  const X2& x2, double delta = 1e-5) {
212  return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
213 }
214 
226 template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
227 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
228  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
229  const X2& x2, const X3& x3, double delta = 1e-5) {
230  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
231  "Template argument Y must be a manifold type.");
232  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
233  "Template argument X1 must be a manifold type.");
234  return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
235 }
236 
237 template<class Y, class X1, class X2, class X3>
238 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
239  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
240  return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
241  x2, x3, delta);
242 }
243 
255 template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
256 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
257  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
258  const X2& x2, const X3& x3, double delta = 1e-5) {
259  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
260  "Template argument Y must be a manifold type.");
261  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
262  "Template argument X2 must be a manifold type.");
263  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
264 }
265 
266 template<class Y, class X1, class X2, class X3>
267 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
268  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
269  return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
270  x2, x3, delta);
271 }
272 
284 template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
285 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
286  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
287  const X2& x2, const X3& x3, double delta = 1e-5) {
288  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
289  "Template argument Y must be a manifold type.");
290  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
291  "Template argument X3 must be a manifold type.");
292  return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
293 }
294 
295 template<class Y, class X1, class X2, class X3>
296 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
297  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
298  return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
299  x2, x3, delta);
300 }
301 
313 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
314 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
315  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
316  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
317  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
318  "Template argument Y must be a manifold type.");
319  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
320  "Template argument X1 must be a manifold type.");
321  return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
322 }
323 
324 template<class Y, class X1, class X2, class X3, class X4>
325 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
326  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
327  return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
328 }
329 
341 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
342 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
343  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
344  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
345  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
346  "Template argument Y must be a manifold type.");
347  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
348  "Template argument X2 must be a manifold type.");
349  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
350 }
351 
352 template<class Y, class X1, class X2, class X3, class X4>
353 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
354  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
355  return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
356 }
357 
369 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
370 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
371  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
372  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
373  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
374  "Template argument Y must be a manifold type.");
375  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
376  "Template argument X3 must be a manifold type.");
377  return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
378 }
379 
380 template<class Y, class X1, class X2, class X3, class X4>
381 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
382  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
383  return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
384 }
385 
397 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
398 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
399  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
400  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
401  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
402  "Template argument Y must be a manifold type.");
403  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
404  "Template argument X4 must be a manifold type.");
405  return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
406 }
407 
408 template<class Y, class X1, class X2, class X3, class X4>
409 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
410  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
411  return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
412 }
413 
426 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
427 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
428  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
429  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
430  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
431  "Template argument Y must be a manifold type.");
432  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
433  "Template argument X1 must be a manifold type.");
434  return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
435 }
436 
437 template<class Y, class X1, class X2, class X3, class X4, class X5>
438 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
439  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
440  return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
441 }
442 
455 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
456 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
457  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
458  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
459  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
460  "Template argument Y must be a manifold type.");
461  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
462  "Template argument X1 must be a manifold type.");
463  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
464 }
465 
466 template<class Y, class X1, class X2, class X3, class X4, class X5>
467 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
468  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
469  return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
470 }
471 
484 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
485 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
486  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
487  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
488  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
489  "Template argument Y must be a manifold type.");
490  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
491  "Template argument X1 must be a manifold type.");
492  return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
493 }
494 
495 template<class Y, class X1, class X2, class X3, class X4, class X5>
496 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
497  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
498  return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
499 }
500 
513 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
514 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
515  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
516  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
517  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
518  "Template argument Y must be a manifold type.");
519  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
520  "Template argument X1 must be a manifold type.");
521  return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
522 }
523 
524 template<class Y, class X1, class X2, class X3, class X4, class X5>
525 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
526  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
527  return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
528 }
529 
542 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
543 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
544  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
545  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
546  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
547  "Template argument Y must be a manifold type.");
548  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
549  "Template argument X1 must be a manifold type.");
550  return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
551 }
552 
553 template<class Y, class X1, class X2, class X3, class X4, class X5>
554 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
555  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
556  return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
557 }
558 
572 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
573 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
574  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
575  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
576  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
577  "Template argument Y must be a manifold type.");
578  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
579  "Template argument X1 must be a manifold type.");
580  return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
581 }
582 
583 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
584 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
585  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
586  return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
587 }
588 
602 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
603 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
604  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
605  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
606  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
607  "Template argument Y must be a manifold type.");
608  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
609  "Template argument X1 must be a manifold type.");
610  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
611 }
612 
613 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
614 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
615  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
616  return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
617 }
618 
632 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
633 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
634  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
635  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
636  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
637  "Template argument Y must be a manifold type.");
638  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
639  "Template argument X1 must be a manifold type.");
640  return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
641 }
642 
643 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
644 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
645  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
646  return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
647 }
648 
662 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
663 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
664  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
665  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
666  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
667  "Template argument Y must be a manifold type.");
668  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
669  "Template argument X1 must be a manifold type.");
670  return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
671 }
672 
673 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
674 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
675  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
676  return numericalDerivative64<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
677 }
678 
692 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
693 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
694  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
695  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
696  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
697  "Template argument Y must be a manifold type.");
698  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
699  "Template argument X1 must be a manifold type.");
700  return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta);
701 }
702 
703 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
704 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
705  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
706  return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
707 }
708 
722 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
723 typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
724  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
725  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
726  double delta = 1e-5) {
727  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
728  "Template argument Y must be a manifold type.");
729  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
730  "Template argument X1 must be a manifold type.");
731  return numericalDerivative11<Y, X6, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
732 }
733 
734 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
735 inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
736  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
737  return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
738 }
739 
748 template<class X>
749 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
750  double delta = 1e-5) {
751  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
752  "Template argument X must be a manifold type.");
753  typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
754  typedef boost::function<double(const X&)> F;
755  typedef boost::function<VectorD(F, const X&, double)> G;
756  G ng = static_cast<G>(numericalGradient<X> );
757  return numericalDerivative11<VectorD, X>(boost::bind(ng, f, _1, delta), x,
758  delta);
759 }
760 
761 template<class X>
762 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
763  1e-5) {
764  return numericalHessian(boost::function<double(const X&)>(f), x, delta);
765 }
766 
770 template<class X1, class X2>
771 class G_x1 {
772  const boost::function<double(const X1&, const X2&)>& f_;
773  X1 x1_;
774  double delta_;
775 public:
776  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
777 
778  G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
779  double delta) :
780  f_(f), x1_(x1), delta_(delta) {
781  }
782  Vector operator()(const X2& x2) {
783  return numericalGradient<X1>(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_);
784  }
785 };
786 
787 template<class X1, class X2>
788 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
789  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
790  double delta = 1e-5) {
791  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
792  G_x1<X1, X2> g_x1(f, x1, delta);
793  return numericalDerivative11<Vector, X2>(
794  boost::function<Vector(const X2&)>(
795  boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta);
796 }
797 
798 template<class X1, class X2>
799 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
800  const X1& x1, const X2& x2, double delta = 1e-5) {
801  return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
802  x1, x2, delta);
803 }
804 
805 template<class X1, class X2>
806 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
807  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
808  double delta = 1e-5) {
809 
810  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
811 
812  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
813  double) = &numericalGradient<X1>;
814  boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2)));
815 
816  return numericalDerivative11<Vector, X1>(
817  boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
818  x1, delta);
819 }
820 
821 template<class X1, class X2>
822 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
823  const X1& x1, const X2& x2, double delta = 1e-5) {
824  return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
825  x1, x2, delta);
826 }
827 
828 template<class X1, class X2>
829 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
830  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
831  double delta = 1e-5) {
832  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
833  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
834  double) = &numericalGradient<X2>;
835  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1));
836 
837  return numericalDerivative11<Vector, X2>(
838  boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
839  x2, delta);
840 }
841 
842 template<class X1, class X2>
843 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
844  const X1& x1, const X2& x2, double delta = 1e-5) {
845  return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
846  x1, x2, delta);
847 }
848 
852 /* **************************************************************** */
853 template<class X1, class X2, class X3>
854 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
855  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
856  const X2& x2, const X3& x3, double delta = 1e-5) {
857  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
858  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
859  double) = &numericalGradient<X1>;
860  boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3)));
861 
862  return numericalDerivative11<Vector, X1>(
863  boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
864  x1, delta);
865 }
866 
867 template<class X1, class X2, class X3>
868 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
869  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
870  return numericalHessian311(
871  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
872  delta);
873 }
874 
875 /* **************************************************************** */
876 template<class X1, class X2, class X3>
877 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
878  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
879  const X2& x2, const X3& x3, double delta = 1e-5) {
880  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
881  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
882  double) = &numericalGradient<X2>;
883  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3)));
884 
885  return numericalDerivative11<Vector, X2>(
886  boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
887  x2, delta);
888 }
889 
890 template<class X1, class X2, class X3>
891 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
892  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
893  return numericalHessian322(
894  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
895  delta);
896 }
897 
898 /* **************************************************************** */
899 template<class X1, class X2, class X3>
900 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
901  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
902  const X2& x2, const X3& x3, double delta = 1e-5) {
903  typedef typename internal::FixedSizeMatrix<X3>::type Vector;
904  Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
905  double) = &numericalGradient<X3>;
906  boost::function<double(const X3&)> f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1));
907 
908  return numericalDerivative11<Vector, X3>(
909  boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
910  x3, delta);
911 }
912 
913 template<class X1, class X2, class X3>
914 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
915  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
916  return numericalHessian333(
917  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
918  delta);
919 }
920 
921 /* **************************************************************** */
922 template<class X1, class X2, class X3>
923 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
924  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
925  const X2& x2, const X3& x3, double delta = 1e-5) {
926  return numericalHessian212<X1, X2>(
927  boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, boost::cref(x3))),
928  x1, x2, delta);
929 }
930 
931 template<class X1, class X2, class X3>
932 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
933  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
934  const X2& x2, const X3& x3, double delta = 1e-5) {
935  return numericalHessian212<X1, X3>(
936  boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, boost::cref(x2), _2)),
937  x1, x3, delta);
938 }
939 
940 template<class X1, class X2, class X3>
941 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
942  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
943  const X2& x2, const X3& x3, double delta = 1e-5) {
944  return numericalHessian212<X2, X3>(
945  boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), _1, _2)),
946  x2, x3, delta);
947 }
948 
949 /* **************************************************************** */
950 template<class X1, class X2, class X3>
951 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
952  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
953  return numericalHessian312(
954  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
955  delta);
956 }
957 
958 template<class X1, class X2, class X3>
959 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
960  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
961  return numericalHessian313(
962  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
963  delta);
964 }
965 
966 template<class X1, class X2, class X3>
967 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
968  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
969  return numericalHessian323(
970  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
971  delta);
972 }
973 
974 } // namespace gtsam
975 
test_docs.d
d
Definition: test_docs.py:28
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::numericalHessian
internal::FixedSizeMatrix< X, X >::type numericalHessian(boost::function< double(const X &)> f, const X &x, double delta=1e-5)
Definition: numericalDerivative.h:749
gtsam::manifold_tag
tag to assert a type is a manifold
Definition: Manifold.h:33
Eigen::PlainObjectBase::setZero
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Definition: CwiseNullaryOp.h:515
gtsam::numericalHessian222
internal::FixedSizeMatrix< X2, X2 >::type numericalHessian222(boost::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:829
gtsam::numericalDerivative66
internal::FixedSizeMatrix< Y, X6 >::type numericalDerivative66(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Definition: numericalDerivative.h:723
gtsam::numericalDerivative53
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative53(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:485
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::numericalDerivative54
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative54(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:514
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
gtsam::numericalDerivative61
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative61(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Definition: numericalDerivative.h:573
gtsam::numericalHessian211
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian211(boost::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:806
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
gtsam::numericalDerivative33
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:285
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:227
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:173
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:12
N
#define N
Definition: gksort.c:12
x5
static string x5("x5")
gtsam::numericalDerivative42
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative42(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Definition: numericalDerivative.h:342
gtsam::numericalDerivative51
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative51(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:427
gtsam::numericalDerivative52
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative52(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:456
gtsam::internal::FixedSizeMatrix::type
Eigen::Matrix< double, traits< Y >::dimension, traits< X >::dimension > type
Definition: numericalDerivative.h:65
g
void g(const string &key, int i)
Definition: testBTree.cpp:43
gtsam::G_x1::delta_
double delta_
Definition: numericalDerivative.h:774
gtsam::numericalDerivative62
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative62(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Definition: numericalDerivative.h:603
gtsam::numericalHessian333
internal::FixedSizeMatrix< X3, X3 >::type numericalHessian333(boost::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:900
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:256
VectorValues.h
Factor Graph Values.
gtsam::parser::F
Signature::Row F
Definition: Signature.cpp:53
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::G_x1::x1_
X1 x1_
Definition: numericalDerivative.h:773
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:199
gtsam::G_x1
Definition: numericalDerivative.h:771
gtsam::numericalDerivative65
internal::FixedSizeMatrix< Y, X5 >::type numericalDerivative65(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Definition: numericalDerivative.h:693
test_callbacks.value
value
Definition: test_callbacks.py:116
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:211
gtsam::G_x1::Vector
internal::FixedSizeMatrix< X1 >::type Vector
Definition: numericalDerivative.h:776
gtsam::numericalHessian212
internal::FixedSizeMatrix< X1, X2 >::type numericalHessian212(boost::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:788
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::G_x1::f_
const boost::function< double(const X1 &, const X2 &)> & f_
Definition: numericalDerivative.h:772
gtsam::numericalGradient
Eigen::Matrix< double, N, 1 > numericalGradient(boost::function< double(const X &)> h, const X &x, double delta=1e-5)
Numerically compute gradient of scalar function.
Definition: numericalDerivative.h:78
gtsam::numericalDerivative63
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative63(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Definition: numericalDerivative.h:633
Lie.h
Base class and basic functions for Lie types.
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam::numericalHessian312
internal::FixedSizeMatrix< X1, X2 >::type numericalHessian312(boost::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:923
gtsam::numericalDerivative43
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative43(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Definition: numericalDerivative.h:370
gtsam
traits
Definition: chartTesting.h:28
gtsam::numericalDerivative41
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative41(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Definition: numericalDerivative.h:314
gtsam::numericalHessian322
internal::FixedSizeMatrix< X2, X2 >::type numericalHessian322(boost::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:877
h
const double h
Definition: testSimpleHelicopter.cpp:16
gtsam::numericalHessian323
internal::FixedSizeMatrix< X2, X3 >::type numericalHessian323(boost::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:941
gtsam::numericalHessian313
internal::FixedSizeMatrix< X1, X3 >::type numericalHessian313(boost::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:932
gtsam::numericalDerivative55
internal::FixedSizeMatrix< Y, X5 >::type numericalDerivative55(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:543
test_eigen.ref
ref
Definition: test_eigen.py:9
x1
Pose3 x1
Definition: testPose3.cpp:588
gtsam::G_x1::operator()
Vector operator()(const X2 &x2)
Definition: numericalDerivative.h:782
gtsam::G_x1::G_x1
G_x1(const boost::function< double(const X1 &, const X2 &)> &f, const X1 &x1, double delta)
Definition: numericalDerivative.h:778
gtsam::numericalHessian311
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian311(boost::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:854
gtsam::numericalDerivative64
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative64(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Definition: numericalDerivative.h:663
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:118
x4
static string x4("x4")
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
X
#define X
Definition: icosphere.cpp:20
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::numericalDerivative44
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative44(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Definition: numericalDerivative.h:398

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:05