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 
test_docs.d
d
Definition: test_docs.py:28
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::manifold_tag
tag to assert a type is a manifold
Definition: Manifold.h:30
gtsam::numericalHessian333
internal::FixedSizeMatrix< X3, X3 >::type numericalHessian333(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:1022
Y
const char Y
Definition: test/EulerAngles.cpp:31
Eigen::PlainObjectBase::setZero
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Definition: CwiseNullaryOp.h:562
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
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
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
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
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::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::numericalHessian313
internal::FixedSizeMatrix< X1, X3 >::type numericalHessian313(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:1058
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::internal::FixedSizeMatrix::type
Eigen::Matrix< double, traits< Y >::dimension, traits< X >::dimension > type
Definition: numericalDerivative.h:57
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::G_x1::delta_
double delta_
Definition: numericalDerivative.h:886
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
gtsam::G_x1::G_x1
G_x1(const std::function< double(const X1 &, const X2 &)> &f, const X1 &x1, double delta)
Definition: numericalDerivative.h:890
gtsam::G_x1::f_
const std::function< double(const X1 &, const X2 &)> & f_
Definition: numericalDerivative.h:884
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::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::G_x1::x1_
X1 x1_
Definition: numericalDerivative.h:885
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
test_callbacks.value
value
Definition: test_callbacks.py:148
gtsam::numericalHessian211
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian211(std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:920
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::numericalHessian212
internal::FixedSizeMatrix< X1, X2 >::type numericalHessian212(std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:901
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::numericalHessian323
internal::FixedSizeMatrix< X2, X3 >::type numericalHessian323(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:1069
gtsam::numericalHessian312
internal::FixedSizeMatrix< X1, X2 >::type numericalHessian312(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:1047
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::G_x1::Vector
internal::FixedSizeMatrix< X1 >::type Vector
Definition: numericalDerivative.h:888
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
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::numericalHessian322
internal::FixedSizeMatrix< X2, X2 >::type numericalHessian322(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:997
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.
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
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
traits
Definition: chartTesting.h:28
h
const double h
Definition: testSimpleHelicopter.cpp:19
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
test_eigen.ref
ref
Definition: test_eigen.py:9
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
x1
Pose3 x1
Definition: testPose3.cpp:663
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::numericalHessian222
internal::FixedSizeMatrix< X2, X2 >::type numericalHessian222(std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:945
gtsam::G_x1::operator()
Vector operator()(const X2 &x2)
Definition: numericalDerivative.h:894
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
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::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

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:58