GTSAM  4.0.2
C++ library for smoothing and mapping (SAM)
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 <functional>
22 
23 #include <gtsam/linear/VectorValues.h>
24 #include <gtsam/linear/JacobianFactor.h>
25 #include <gtsam/nonlinear/Values.h>
26 #include <gtsam/base/Lie.h>
27 
28 namespace gtsam {
29 
30 /*
31  * Note that all of these functions have two versions, a boost.function version and a
32  * standard C++ function pointer version. This allows reformulating the arguments of
33  * a function to fit the correct structure, which is useful for situations like
34  * member functions and functions with arguments not involved in the derivative:
35  *
36  * Usage of the boost bind version to rearrange arguments:
37  * for a function with one relevant param and an optional derivative:
38  * Foo bar(const Obj& a, OptionalMatrixType H1)
39  * Use boost.bind to restructure:
40  * std::bind(bar, std::placeholders::_1, {})
41  * This syntax will fix the optional argument to {}, while using the first argument provided
42  *
43  * For member functions, such as below, with an instantiated copy instanceOfSomeClass
44  * Foo SomeClass::bar(const Obj& a)
45  * Use boost bind as follows to create a function pointer that uses the member function:
46  * std::bind(&SomeClass::bar, ref(instanceOfSomeClass), std::placeholders::_1)
47  *
48  * For additional details, see the documentation:
49  * http://www.boost.org/doc/libs/release/libs/bind/bind.html
50  */
51 
52 
53 // a quick helper struct to get the appropriate fixed sized matrix from two value types
54 namespace internal {
55 template<class Y, class X=double>
56 struct FixedSizeMatrix {
57  typedef Eigen::Matrix<double,traits<Y>::dimension, traits<X>::dimension> type;
58 };
59 }
60 
69 template <class X, int N = traits<X>::dimension>
70 typename Eigen::Matrix<double, N, 1> numericalGradient(
71  std::function<double(const X&)> h, const X& x, double delta = 1e-5) {
72  double factor = 1.0 / (2.0 * delta);
73 
74  static_assert(
75  (std::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
76  "Template argument X must be a manifold type.");
77  static_assert(N>0, "Template argument X must be fixed-size type or N must be specified.");
78 
79  // Prepare a tangent vector to perturb x with, only works for fixed size
80  typename traits<X>::TangentVector d;
81  d.setZero();
82 
83  Eigen::Matrix<double,N,1> g;
84  g.setZero();
85  for (int j = 0; j < N; j++) {
86  d(j) = delta;
87  double hxplus = h(traits<X>::Retract(x, d));
88  d(j) = -delta;
89  double hxmin = h(traits<X>::Retract(x, d));
90  d(j) = 0;
91  g(j) = (hxplus - hxmin) * factor;
92  }
93  return g;
94 }
95 
108 template <class Y, class X, int N = traits<X>::dimension>
109 // TODO Should compute fixed-size matrix
110 typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
111  std::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
112  typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
113 
114  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
115  "Template argument Y must be a manifold type.");
116  typedef traits<Y> TraitsY;
117 
118  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
119  "Template argument X must be a manifold type.");
120  static_assert(N>0, "Template argument X must be fixed-size type or N must be specified.");
121  typedef traits<X> TraitsX;
122 
123  // get value at x, and corresponding chart
124  const Y hx = h(x);
125 
126  // Bit of a hack for now to find number of rows
127  const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx);
128  const size_t m = zeroY.size();
129 
130  // Prepare a tangent vector to perturb x with, only works for fixed size
131  Eigen::Matrix<double, N, 1> dx;
132  dx.setZero();
133 
134  // Fill in Jacobian H
135  Matrix H = Matrix::Zero(m, N);
136  const double factor = 1.0 / (2.0 * delta);
137  for (int j = 0; j < N; j++) {
138  dx(j) = delta;
139  const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
140  dx(j) = -delta;
141  const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
142  dx(j) = 0;
143  H.col(j) << (dy1 - dy2) * factor;
144  }
145  return H;
146 }
147 
149 template<class Y, class X>
150 typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
151  double delta = 1e-5) {
152  return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
153  delta);
154 }
155 
165 template<class Y, class X1, class X2, int N = traits<X1>::dimension>
166 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const std::function<Y(const X1&, const X2&)>& h,
167  const X1& x1, const X2& x2, double delta = 1e-5) {
168  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
169  "Template argument Y must be a manifold type.");
170  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
171  "Template argument X1 must be a manifold type.");
172  return numericalDerivative11<Y, X1, N>(
173  std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
174 }
175 
177 template<class Y, class X1, class X2>
178 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
179  const X2& x2, double delta = 1e-5) {
180  return numericalDerivative21<Y, X1, X2>(
181  std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
182  delta);
183 }
184 
194 template<class Y, class X1, class X2, int N = traits<X2>::dimension>
195 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(std::function<Y(const X1&, const X2&)> h,
196  const X1& x1, const X2& x2, double delta = 1e-5) {
197 // static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
198 // "Template argument X1 must be a manifold type.");
199  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
200  "Template argument X2 must be a manifold type.");
201  return numericalDerivative11<Y, X2, N>(
202  std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
203 }
204 
206 template<class Y, class X1, class X2>
207 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
208  const X2& x2, double delta = 1e-5) {
209  return numericalDerivative22<Y, X1, X2>(
210  std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
211  delta);
212 }
213 
225 template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
226 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
227  std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
228  const X2& x2, const X3& x3, double delta = 1e-5) {
229  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
230  "Template argument Y must be a manifold type.");
231  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
232  "Template argument X1 must be a manifold type.");
233  return numericalDerivative11<Y, X1, N>(
234  std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
235  x1, delta);
236 }
237 
238 template<class Y, class X1, class X2, class X3>
239 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
240  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
241  return numericalDerivative31<Y, X1, X2, X3>(
242  std::bind(h, std::placeholders::_1, std::placeholders::_2,
243  std::placeholders::_3),
244  x1, x2, x3, delta);
245 }
246 
258 template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
259 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
260  std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
261  const X2& x2, const X3& x3, double delta = 1e-5) {
262  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
263  "Template argument Y must be a manifold type.");
264  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
265  "Template argument X2 must be a manifold type.");
266  return numericalDerivative11<Y, X2, N>(
267  std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
268  x2, delta);
269 }
270 
271 template<class Y, class X1, class X2, class X3>
272 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
273  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
274  return numericalDerivative32<Y, X1, X2, X3>(
275  std::bind(h, std::placeholders::_1, std::placeholders::_2,
276  std::placeholders::_3),
277  x1, x2, x3, delta);
278 }
279 
291 template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
292 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
293  std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
294  const X2& x2, const X3& x3, double delta = 1e-5) {
295  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
296  "Template argument Y must be a manifold type.");
297  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
298  "Template argument X3 must be a manifold type.");
299  return numericalDerivative11<Y, X3, N>(
300  std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
301  x3, delta);
302 }
303 
304 template<class Y, class X1, class X2, class X3>
305 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
306  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
307  return numericalDerivative33<Y, X1, X2, X3>(
308  std::bind(h, std::placeholders::_1, std::placeholders::_2,
309  std::placeholders::_3),
310  x1, x2, x3, delta);
311 }
312 
324 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
325 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
326  std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
327  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
328  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
329  "Template argument Y must be a manifold type.");
330  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
331  "Template argument X1 must be a manifold type.");
332  return numericalDerivative11<Y, X1, N>(
333  std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
334  std::cref(x4)),
335  x1, delta);
336 }
337 
338 template<class Y, class X1, class X2, class X3, class X4>
339 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
340  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
341  return numericalDerivative41<Y, X1, X2, X3, X4>(
342  std::bind(h, std::placeholders::_1, std::placeholders::_2,
343  std::placeholders::_3, std::placeholders::_4),
344  x1, x2, x3, x4);
345 }
346 
358 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
359 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
360  std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
361  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
362  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
363  "Template argument Y must be a manifold type.");
364  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
365  "Template argument X2 must be a manifold type.");
366  return numericalDerivative11<Y, X2, N>(
367  std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
368  std::cref(x4)),
369  x2, delta);
370 }
371 
372 template<class Y, class X1, class X2, class X3, class X4>
373 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
374  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
375  return numericalDerivative42<Y, X1, X2, X3, X4>(
376  std::bind(h, std::placeholders::_1, std::placeholders::_2,
377  std::placeholders::_3, std::placeholders::_4),
378  x1, x2, x3, x4);
379 }
380 
392 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
393 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
394  std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
395  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
396  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
397  "Template argument Y must be a manifold type.");
398  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
399  "Template argument X3 must be a manifold type.");
400  return numericalDerivative11<Y, X3, N>(
401  std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
402  std::cref(x4)),
403  x3, delta);
404 }
405 
406 template<class Y, class X1, class X2, class X3, class X4>
407 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
408  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
409  return numericalDerivative43<Y, X1, X2, X3, X4>(
410  std::bind(h, std::placeholders::_1, std::placeholders::_2,
411  std::placeholders::_3, std::placeholders::_4),
412  x1, x2, x3, x4);
413 }
414 
426 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
427 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
428  std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
429  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
430  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
431  "Template argument Y must be a manifold type.");
432  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
433  "Template argument X4 must be a manifold type.");
434  return numericalDerivative11<Y, X4, N>(
435  std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
436  std::placeholders::_1),
437  x4, delta);
438 }
439 
440 template<class Y, class X1, class X2, class X3, class X4>
441 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
442  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
443  return numericalDerivative44<Y, X1, X2, X3, X4>(
444  std::bind(h, std::placeholders::_1, std::placeholders::_2,
445  std::placeholders::_3, std::placeholders::_4),
446  x1, x2, x3, x4);
447 }
448 
461 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
462 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
463  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
464  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
465  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
466  "Template argument Y must be a manifold type.");
467  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
468  "Template argument X1 must be a manifold type.");
469  return numericalDerivative11<Y, X1, N>(
470  std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
471  std::cref(x4), std::cref(x5)),
472  x1, delta);
473 }
474 
475 template<class Y, class X1, class X2, class X3, class X4, class X5>
476 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
477  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
478  return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
479  std::bind(h, std::placeholders::_1, std::placeholders::_2,
480  std::placeholders::_3, std::placeholders::_4,
481  std::placeholders::_5),
482  x1, x2, x3, x4, x5);
483 }
484 
497 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
498 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
499  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
500  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
501  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
502  "Template argument Y must be a manifold type.");
503  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
504  "Template argument X1 must be a manifold type.");
505  return numericalDerivative11<Y, X2, N>(
506  std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
507  std::cref(x4), std::cref(x5)),
508  x2, delta);
509 }
510 
511 template<class Y, class X1, class X2, class X3, class X4, class X5>
512 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
513  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
514  return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
515  std::bind(h, std::placeholders::_1, std::placeholders::_2,
516  std::placeholders::_3, std::placeholders::_4,
517  std::placeholders::_5),
518  x1, x2, x3, x4, x5);
519 }
520 
533 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
534 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
535  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
536  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
537  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
538  "Template argument Y must be a manifold type.");
539  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
540  "Template argument X1 must be a manifold type.");
541  return numericalDerivative11<Y, X3, N>(
542  std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
543  std::cref(x4), std::cref(x5)),
544  x3, delta);
545 }
546 
547 template<class Y, class X1, class X2, class X3, class X4, class X5>
548 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
549  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
550  return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
551  std::bind(h, std::placeholders::_1, std::placeholders::_2,
552  std::placeholders::_3, std::placeholders::_4,
553  std::placeholders::_5),
554  x1, x2, x3, x4, x5);
555 }
556 
569 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
570 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
571  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
572  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
573  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
574  "Template argument Y must be a manifold type.");
575  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
576  "Template argument X1 must be a manifold type.");
577  return numericalDerivative11<Y, X4, N>(
578  std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
579  std::placeholders::_1, std::cref(x5)),
580  x4, delta);
581 }
582 
583 template<class Y, class X1, class X2, class X3, class X4, class X5>
584 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
585  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
586  return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
587  std::bind(h, std::placeholders::_1, std::placeholders::_2,
588  std::placeholders::_3, std::placeholders::_4,
589  std::placeholders::_5),
590  x1, x2, x3, x4, x5);
591 }
592 
605 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
606 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
607  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
608  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
609  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
610  "Template argument Y must be a manifold type.");
611  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
612  "Template argument X1 must be a manifold type.");
613  return numericalDerivative11<Y, X5, N>(
614  std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
615  std::cref(x4), std::placeholders::_1),
616  x5, delta);
617 }
618 
619 template<class Y, class X1, class X2, class X3, class X4, class X5>
620 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
621  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
622  return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
623  std::bind(h, std::placeholders::_1, std::placeholders::_2,
624  std::placeholders::_3, std::placeholders::_4,
625  std::placeholders::_5),
626  x1, x2, x3, x4, x5);
627 }
628 
642 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
643 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
644  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
645  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
646  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
647  "Template argument Y must be a manifold type.");
648  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
649  "Template argument X1 must be a manifold type.");
650  return numericalDerivative11<Y, X1, N>(
651  std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
652  std::cref(x4), std::cref(x5), std::cref(x6)),
653  x1, delta);
654 }
655 
656 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
657 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
658  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
659  return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
660  std::bind(h, std::placeholders::_1, std::placeholders::_2,
661  std::placeholders::_3, std::placeholders::_4,
662  std::placeholders::_5, std::placeholders::_6),
663  x1, x2, x3, x4, x5, x6);
664 }
665 
679 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
680 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
681  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
682  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
683  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
684  "Template argument Y must be a manifold type.");
685  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
686  "Template argument X1 must be a manifold type.");
687  return numericalDerivative11<Y, X2, N>(
688  std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
689  std::cref(x4), std::cref(x5), std::cref(x6)),
690  x2, delta);
691 }
692 
693 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
694 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
695  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
696  return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
697  std::bind(h, std::placeholders::_1, std::placeholders::_2,
698  std::placeholders::_3, std::placeholders::_4,
699  std::placeholders::_5, std::placeholders::_6),
700  x1, x2, x3, x4, x5, x6);
701 }
702 
716 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
717 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
718  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
719  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
720  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
721  "Template argument Y must be a manifold type.");
722  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
723  "Template argument X1 must be a manifold type.");
724  return numericalDerivative11<Y, X3, N>(
725  std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
726  std::cref(x4), std::cref(x5), std::cref(x6)),
727  x3, delta);
728 }
729 
730 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
731 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
732  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
733  return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
734  std::bind(h, std::placeholders::_1, std::placeholders::_2,
735  std::placeholders::_3, std::placeholders::_4,
736  std::placeholders::_5, std::placeholders::_6),
737  x1, x2, x3, x4, x5, x6);
738 }
739 
753 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
754 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
755  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
756  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
757  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
758  "Template argument Y must be a manifold type.");
759  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
760  "Template argument X1 must be a manifold type.");
761  return numericalDerivative11<Y, X4, N>(
762  std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
763  std::placeholders::_1, std::cref(x5), std::cref(x6)),
764  x4, delta);
765 }
766 
767 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
768 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
769  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
770  return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
771  std::bind(h, std::placeholders::_1, std::placeholders::_2,
772  std::placeholders::_3, std::placeholders::_4,
773  std::placeholders::_5, std::placeholders::_6),
774  x1, x2, x3, x4, x5, x6);
775 }
776 
790 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
791 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
792  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
793  const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
794  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
795  "Template argument Y must be a manifold type.");
796  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
797  "Template argument X1 must be a manifold type.");
798  return numericalDerivative11<Y, X5, N>(
799  std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
800  std::cref(x4), std::placeholders::_1, std::cref(x6)),
801  x5, delta);
802 }
803 
804 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
805 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
806  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
807  return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
808  std::bind(h, std::placeholders::_1, std::placeholders::_2,
809  std::placeholders::_3, std::placeholders::_4,
810  std::placeholders::_5, std::placeholders::_6),
811  x1, x2, x3, x4, x5, x6);
812 }
813 
827 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
828 typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
829  std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
830  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
831  double delta = 1e-5) {
832  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
833  "Template argument Y must be a manifold type.");
834  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
835  "Template argument X1 must be a manifold type.");
836  return numericalDerivative11<Y, X6, N>(
837  std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
838  std::cref(x4), std::cref(x5), std::placeholders::_1),
839  x6, delta);
840 }
841 
842 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
843 inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
844  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
845  return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
846  std::bind(h, std::placeholders::_1, std::placeholders::_2,
847  std::placeholders::_3, std::placeholders::_4,
848  std::placeholders::_5, std::placeholders::_6),
849  x1, x2, x3, x4, x5, x6);
850 }
851 
860 template<class X>
861 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(std::function<double(const X&)> f, const X& x,
862  double delta = 1e-5) {
863  static_assert( (std::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
864  "Template argument X must be a manifold type.");
865  typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
866  typedef std::function<double(const X&)> F;
867  typedef std::function<VectorD(F, const X&, double)> G;
868  G ng = static_cast<G>(numericalGradient<X> );
869  return numericalDerivative11<VectorD, X>(
870  std::bind(ng, f, std::placeholders::_1, delta), x, delta);
871 }
872 
873 template<class X>
874 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
875  1e-5) {
876  return numericalHessian(std::function<double(const X&)>(f), x, delta);
877 }
878 
882 template<class X1, class X2>
883 class G_x1 {
884  const std::function<double(const X1&, const X2&)>& f_;
885  X1 x1_;
886  double delta_;
887 public:
888  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
889 
890  G_x1(const std::function<double(const X1&, const X2&)>& f, const X1& x1,
891  double delta) :
892  f_(f), x1_(x1), delta_(delta) {
893  }
894  Vector operator()(const X2& x2) {
895  return numericalGradient<X1>(
896  std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_);
897  }
898 };
899 
900 template<class X1, class X2>
901 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
902  std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
903  double delta = 1e-5) {
904  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
905  G_x1<X1, X2> g_x1(f, x1, delta);
906  return numericalDerivative11<Vector, X2>(
907  std::function<Vector(const X2&)>(
908  std::bind<Vector>(std::ref(g_x1), std::placeholders::_1)),
909  x2, delta);
910 }
911 
912 template<class X1, class X2>
913 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
914  const X1& x1, const X2& x2, double delta = 1e-5) {
915  return numericalHessian212(std::function<double(const X1&, const X2&)>(f),
916  x1, x2, delta);
917 }
918 
919 template<class X1, class X2>
920 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
921  std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
922  double delta = 1e-5) {
923 
924  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
925 
926  Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
927  double) = &numericalGradient<X1>;
928  std::function<double(const X1&)> f2(
929  std::bind(f, std::placeholders::_1, std::cref(x2)));
930 
931  return numericalDerivative11<Vector, X1>(
932  std::function<Vector(const X1&)>(
933  std::bind(numGrad, f2, std::placeholders::_1, delta)),
934  x1, delta);
935 }
936 
937 template<class X1, class X2>
938 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
939  const X1& x1, const X2& x2, double delta = 1e-5) {
940  return numericalHessian211(std::function<double(const X1&, const X2&)>(f),
941  x1, x2, delta);
942 }
943 
944 template<class X1, class X2>
945 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
946  std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
947  double delta = 1e-5) {
948  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
949  Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
950  double) = &numericalGradient<X2>;
951  std::function<double(const X2&)> f2(
952  std::bind(f, std::cref(x1), std::placeholders::_1));
953 
954  return numericalDerivative11<Vector, X2>(
955  std::function<Vector(const X2&)>(
956  std::bind(numGrad, f2, std::placeholders::_1, delta)),
957  x2, delta);
958 }
959 
960 template<class X1, class X2>
961 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
962  const X1& x1, const X2& x2, double delta = 1e-5) {
963  return numericalHessian222(std::function<double(const X1&, const X2&)>(f),
964  x1, x2, delta);
965 }
966 
970 /* **************************************************************** */
971 template<class X1, class X2, class X3>
972 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
973  std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
974  const X2& x2, const X3& x3, double delta = 1e-5) {
975  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
976  Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
977  double) = &numericalGradient<X1>;
978  std::function<double(const X1&)> f2(std::bind(
979  f, std::placeholders::_1, std::cref(x2), std::cref(x3)));
980 
981  return numericalDerivative11<Vector, X1>(
982  std::function<Vector(const X1&)>(
983  std::bind(numGrad, f2, std::placeholders::_1, delta)),
984  x1, delta);
985 }
986 
987 template<class X1, class X2, class X3>
988 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
989  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
990  return numericalHessian311(
991  std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
992  delta);
993 }
994 
995 /* **************************************************************** */
996 template<class X1, class X2, class X3>
997 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
998  std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
999  const X2& x2, const X3& x3, double delta = 1e-5) {
1000  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
1001  Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
1002  double) = &numericalGradient<X2>;
1003  std::function<double(const X2&)> f2(std::bind(
1004  f, std::cref(x1), std::placeholders::_1, std::cref(x3)));
1005 
1006  return numericalDerivative11<Vector, X2>(
1007  std::function<Vector(const X2&)>(
1008  std::bind(numGrad, f2, std::placeholders::_1, delta)),
1009  x2, delta);
1010 }
1011 
1012 template<class X1, class X2, class X3>
1013 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
1014  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1015  return numericalHessian322(
1016  std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
1017  delta);
1018 }
1019 
1020 /* **************************************************************** */
1021 template<class X1, class X2, class X3>
1022 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
1023  std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
1024  const X2& x2, const X3& x3, double delta = 1e-5) {
1025  typedef typename internal::FixedSizeMatrix<X3>::type Vector;
1026  Vector (*numGrad)(std::function<double(const X3&)>, const X3&,
1027  double) = &numericalGradient<X3>;
1028  std::function<double(const X3&)> f2(std::bind(
1029  f, std::cref(x1), std::cref(x2), std::placeholders::_1));
1030 
1031  return numericalDerivative11<Vector, X3>(
1032  std::function<Vector(const X3&)>(
1033  std::bind(numGrad, f2, std::placeholders::_1, delta)),
1034  x3, delta);
1035 }
1036 
1037 template<class X1, class X2, class X3>
1038 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
1039  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1040  return numericalHessian333(
1041  std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
1042  delta);
1043 }
1044 
1045 /* **************************************************************** */
1046 template<class X1, class X2, class X3>
1047 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
1048  std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
1049  const X2& x2, const X3& x3, double delta = 1e-5) {
1050  return numericalHessian212<X1, X2>(
1051  std::function<double(const X1&, const X2&)>(
1052  std::bind(f, std::placeholders::_1, std::placeholders::_2,
1053  std::cref(x3))),
1054  x1, x2, delta);
1055 }
1056 
1057 template<class X1, class X2, class X3>
1058 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
1059  std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
1060  const X2& x2, const X3& x3, double delta = 1e-5) {
1061  return numericalHessian212<X1, X3>(
1062  std::function<double(const X1&, const X3&)>(
1063  std::bind(f, std::placeholders::_1, std::cref(x2),
1064  std::placeholders::_2)),
1065  x1, x3, delta);
1066 }
1067 
1068 template<class X1, class X2, class X3>
1069 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
1070  std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
1071  const X2& x2, const X3& x3, double delta = 1e-5) {
1072  return numericalHessian212<X2, X3>(
1073  std::function<double(const X2&, const X3&)>(
1074  std::bind(f, std::cref(x1), std::placeholders::_1,
1075  std::placeholders::_2)),
1076  x2, x3, delta);
1077 }
1078 
1079 /* **************************************************************** */
1080 template<class X1, class X2, class X3>
1081 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
1082  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1083  return numericalHessian312(
1084  std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
1085  delta);
1086 }
1087 
1088 template<class X1, class X2, class X3>
1089 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
1090  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1091  return numericalHessian313(
1092  std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
1093  delta);
1094 }
1095 
1096 template<class X1, class X2, class X3>
1097 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
1098  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1099  return numericalHessian323(
1100  std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
1101  delta);
1102 }
1103 
1104 } // namespace gtsam
1105 
gtsam::manifold_tag
tag to assert a type is a manifold
Definition: Manifold.h:30
gtsam::numericalDerivative66
internal::FixedSizeMatrix< Y, X6 >::type numericalDerivative66(std::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:828
gtsam::numericalDerivative44
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative44(std::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:427
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::numericalDerivative62
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative62(std::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:680
gtsam::numericalDerivative64
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative64(std::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:754
gtsam::traits
Definition: Group.h:43
gtsam::internal::FixedSizeMatrix
Definition: numericalDerivative.h:56
gtsam::numericalDerivative63
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative63(std::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:717
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:110
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
gtsam::numericalHessian311
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian311(std::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:972
gtsam::numericalDerivative51
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative51(std::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:462
VectorValues.h
Factor Graph Values.
gtsam::numericalDerivative41
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative41(std::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:325
gtsam::numericalDerivative33
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(std::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:292
gtsam::numericalDerivative52
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative52(std::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:498
gtsam::numericalDerivative55
internal::FixedSizeMatrix< Y, X5 >::type numericalDerivative55(std::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:606
gtsam::G_x1
Definition: numericalDerivative.h:883
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(std::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:259
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(std::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:226
gtsam::numericalHessian
internal::FixedSizeMatrix< X, X >::type numericalHessian(std::function< double(const X &)> f, const X &x, double delta=1e-5)
Definition: numericalDerivative.h:861
gtsam::numericalDerivative53
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative53(std::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:534
Lie.h
Base class and basic functions for Lie types.
gtsam::numericalDerivative54
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative54(std::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:570
gtsam
Definition: chartTesting.h:28
gtsam::numericalDerivative42
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative42(std::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:359
gtsam::numericalDerivative65
internal::FixedSizeMatrix< Y, X5 >::type numericalDerivative65(std::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:791
gtsam::numericalGradient
Eigen::Matrix< double, N, 1 > numericalGradient(std::function< double(const X &)> h, const X &x, double delta=1e-5)
Numerically compute gradient of scalar function.
Definition: numericalDerivative.h:70
gtsam::numericalDerivative43
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative43(std::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:393
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
gtsam::numericalDerivative61
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative61(std::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:643
Generated by   doxygen 1.8.13