gtsam  4.0.0
gtsam
numericalDerivative.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 // \callgraph
19 #pragma once
20 
21 #include <boost/function.hpp>
22 #ifdef __GNUC__
23 #pragma GCC diagnostic push
24 #pragma GCC diagnostic ignored "-Wunused-variable"
25 #endif
26 #include <boost/bind.hpp>
27 #ifdef __GNUC__
28 #pragma GCC diagnostic pop
29 #endif
30 
31 #include <gtsam/linear/VectorValues.h>
32 #include <gtsam/linear/JacobianFactor.h>
33 #include <gtsam/nonlinear/Values.h>
34 #include <gtsam/base/Lie.h>
35 
36 namespace gtsam {
37 
38 /*
39  * Note that all of these functions have two versions, a boost.function version and a
40  * standard C++ function pointer version. This allows reformulating the arguments of
41  * a function to fit the correct structure, which is useful for situations like
42  * member functions and functions with arguments not involved in the derivative:
43  *
44  * Usage of the boost bind version to rearrange arguments:
45  * for a function with one relevant param and an optional derivative:
46  * Foo bar(const Obj& a, boost::optional<Matrix&> H1)
47  * Use boost.bind to restructure:
48  * boost::bind(bar, _1, boost::none)
49  * This syntax will fix the optional argument to boost::none, while using the first argument provided
50  *
51  * For member functions, such as below, with an instantiated copy instanceOfSomeClass
52  * Foo SomeClass::bar(const Obj& a)
53  * Use boost bind as follows to create a function pointer that uses the member function:
54  * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1)
55  *
56  * For additional details, see the documentation:
57  * http://www.boost.org/doc/libs/release/libs/bind/bind.html
58  */
59 
60 
61 // a quick helper struct to get the appropriate fixed sized matrix from two value types
62 namespace internal {
63 template<class Y, class X=double>
64 struct FixedSizeMatrix {
65  typedef Eigen::Matrix<double,traits<Y>::dimension, traits<X>::dimension> type;
66 };
67 }
68 
74 template<class X>
75 typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<double(const X&)> h, const X& x,
76  double delta = 1e-5) {
77  double factor = 1.0 / (2.0 * delta);
78 
79  BOOST_STATIC_ASSERT_MSG(
80  (boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
81  "Template argument X must be a manifold type.");
82  static const int N = traits<X>::dimension;
83  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
84 
85  typedef typename traits<X>::TangentVector TangentX;
86 
87  // Prepare a tangent vector to perturb x with, only works for fixed size
88  TangentX d;
89  d.setZero();
90 
91  Eigen::Matrix<double,N,1> g; g.setZero(); // Can be fixed size
92  for (int j = 0; j < N; j++) {
93  d(j) = delta;
94  double hxplus = h(traits<X>::Retract(x, d));
95  d(j) = -delta;
96  double hxmin = h(traits<X>::Retract(x, d));
97  d(j) = 0;
98  g(j) = (hxplus - hxmin) * factor;
99  }
100  return g;
101 }
102 
114 template<class Y, class X>
115 // TODO Should compute fixed-size matrix
116 typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
117  double delta = 1e-5) {
118 
119  typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
120 
121  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
122  "Template argument Y must be a manifold type.");
123  typedef traits<Y> TraitsY;
124  typedef typename TraitsY::TangentVector TangentY;
125 
126  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
127  "Template argument X must be a manifold type.");
128  static const int N = traits<X>::dimension;
129  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
130  typedef traits<X> TraitsX;
131  typedef typename TraitsX::TangentVector TangentX;
132 
133  // get value at x, and corresponding chart
134  const Y hx = h(x);
135 
136  // Bit of a hack for now to find number of rows
137  const TangentY zeroY = TraitsY::Local(hx, hx);
138  const size_t m = zeroY.size();
139 
140  // Prepare a tangent vector to perturb x with, only works for fixed size
141  TangentX dx;
142  dx.setZero();
143 
144  // Fill in Jacobian H
145  Matrix H = Matrix::Zero(m, N);
146  const double factor = 1.0 / (2.0 * delta);
147  for (int j = 0; j < N; j++) {
148  dx(j) = delta;
149  const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
150  dx(j) = -delta;
151  const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
152  dx(j) = 0;
153  H.col(j) << (dy1 - dy2) * factor;
154  }
155  return H;
156 }
157 
159 template<class Y, class X>
160 typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
161  double delta = 1e-5) {
162  return numericalDerivative11<Y, X>(boost::bind(h, _1), x, delta);
163 }
164 
173 template<class Y, class X1, class X2>
174 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
175  const X1& x1, const X2& x2, double delta = 1e-5) {
176  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
177  "Template argument Y must be a manifold type.");
178  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
179  "Template argument X1 must be a manifold type.");
180  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2), x1, delta);
181 }
182 
184 template<class Y, class X1, class X2>
185 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
186  const X2& x2, double delta = 1e-5) {
187  return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
188 }
189 
198 template<class Y, class X1, class X2>
199 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
200  const X1& x1, const X2& x2, double delta = 1e-5) {
201 // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
202 // "Template argument X1 must be a manifold type.");
203  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
204  "Template argument X2 must be a manifold type.");
205  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1), x2, delta);
206 }
207 
209 template<class Y, class X1, class X2>
210 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
211  const X2& x2, double delta = 1e-5) {
212  return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
213 }
214 
225 template<class Y, class X1, class X2, class X3>
226 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
227  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
228  const X2& x2, const X3& x3, double delta = 1e-5) {
229  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
230  "Template argument Y must be a manifold type.");
231  BOOST_STATIC_ASSERT_MSG( (boost::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>(boost::bind(h, _1, x2, x3), x1, delta);
234 }
235 
236 template<class Y, class X1, class X2, class X3>
237 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
238  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
239  return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
240  x2, x3, delta);
241 }
242 
253 template<class Y, class X1, class X2, class X3>
254 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
255  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
256  const X2& x2, const X3& x3, double delta = 1e-5) {
257  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
258  "Template argument Y must be a manifold type.");
259  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
260  "Template argument X2 must be a manifold type.");
261  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3), x2, delta);
262 }
263 
264 template<class Y, class X1, class X2, class X3>
265 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
266  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
267  return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
268  x2, x3, delta);
269 }
270 
281 template<class Y, class X1, class X2, class X3>
282 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
283  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
284  const X2& x2, const X3& x3, double delta = 1e-5) {
285  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
286  "Template argument Y must be a manifold type.");
287  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
288  "Template argument X3 must be a manifold type.");
289  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1), x3, delta);
290 }
291 
292 template<class Y, class X1, class X2, class X3>
293 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
294  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
295  return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
296  x2, x3, delta);
297 }
298 
309 template<class Y, class X1, class X2, class X3, class X4>
310 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
311  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
312  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
313  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
314  "Template argument Y must be a manifold type.");
315  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
316  "Template argument X1 must be a manifold type.");
317  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4), x1, delta);
318 }
319 
330 template<class Y, class X1, class X2, class X3, class X4>
331 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
332  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
333  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
334  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
335  "Template argument Y must be a manifold type.");
336  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
337  "Template argument X2 must be a manifold type.");
338  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta);
339 }
340 
351 template<class Y, class X1, class X2, class X3, class X4>
352 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
353  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
354  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
355  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
356  "Template argument Y must be a manifold type.");
357  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
358  "Template argument X3 must be a manifold type.");
359  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4), x3, delta);
360 }
361 
372 template<class Y, class X1, class X2, class X3, class X4>
373 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
374  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
375  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
376  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
377  "Template argument Y must be a manifold type.");
378  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
379  "Template argument X4 must be a manifold type.");
380  return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1), x4, delta);
381 }
382 
391 template<class X>
392 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
393  double delta = 1e-5) {
394  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
395  "Template argument X must be a manifold type.");
396  typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
397  typedef boost::function<double(const X&)> F;
398  typedef boost::function<VectorD(F, const X&, double)> G;
399  G ng = static_cast<G>(numericalGradient<X> );
400  return numericalDerivative11<VectorD, X>(boost::bind(ng, f, _1, delta), x,
401  delta);
402 }
403 
404 template<class X>
405 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
406  1e-5) {
407  return numericalHessian(boost::function<double(const X&)>(f), x, delta);
408 }
409 
413 template<class X1, class X2>
414 class G_x1 {
415  const boost::function<double(const X1&, const X2&)>& f_;
416  X1 x1_;
417  double delta_;
418 public:
419  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
420 
421  G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
422  double delta) :
423  f_(f), x1_(x1), delta_(delta) {
424  }
425  Vector operator()(const X2& x2) {
426  return numericalGradient<X1>(boost::bind(f_, _1, x2), x1_, delta_);
427  }
428 };
429 
430 template<class X1, class X2>
431 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
432  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
433  double delta = 1e-5) {
434  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
435  G_x1<X1, X2> g_x1(f, x1, delta);
436  return numericalDerivative11<Vector, X2>(
437  boost::function<Vector(const X2&)>(
438  boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta);
439 }
440 
441 template<class X1, class X2>
442 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
443  const X1& x1, const X2& x2, double delta = 1e-5) {
444  return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
445  x1, x2, delta);
446 }
447 
448 template<class X1, class X2>
449 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
450  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
451  double delta = 1e-5) {
452 
453  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
454 
455  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
456  double) = &numericalGradient<X1>;
457  boost::function<double(const X1&)> f2(boost::bind(f, _1, x2));
458 
459  return numericalDerivative11<Vector, X1>(
460  boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
461  x1, delta);
462 }
463 
464 template<class X1, class X2>
465 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
466  const X1& x1, const X2& x2, double delta = 1e-5) {
467  return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
468  x1, x2, delta);
469 }
470 
471 template<class X1, class X2>
472 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
473  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
474  double delta = 1e-5) {
475  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
476  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
477  double) = &numericalGradient<X2>;
478  boost::function<double(const X2&)> f2(boost::bind(f, x1, _1));
479 
480  return numericalDerivative11<Vector, X2>(
481  boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
482  x2, delta);
483 }
484 
485 template<class X1, class X2>
486 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
487  const X1& x1, const X2& x2, double delta = 1e-5) {
488  return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
489  x1, x2, delta);
490 }
491 
495 /* **************************************************************** */
496 template<class X1, class X2, class X3>
497 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
498  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
499  const X2& x2, const X3& x3, double delta = 1e-5) {
500  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
501  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
502  double) = &numericalGradient<X1>;
503  boost::function<double(const X1&)> f2(boost::bind(f, _1, x2, x3));
504 
505  return numericalDerivative11<Vector, X1>(
506  boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
507  x1, delta);
508 }
509 
510 template<class X1, class X2, class X3>
511 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
512  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
513  return numericalHessian311(
514  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
515  delta);
516 }
517 
518 /* **************************************************************** */
519 template<class X1, class X2, class X3>
520 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
521  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
522  const X2& x2, const X3& x3, double delta = 1e-5) {
523  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
524  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
525  double) = &numericalGradient<X2>;
526  boost::function<double(const X2&)> f2(boost::bind(f, x1, _1, x3));
527 
528  return numericalDerivative11<Vector, X2>(
529  boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
530  x2, delta);
531 }
532 
533 template<class X1, class X2, class X3>
534 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
535  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
536  return numericalHessian322(
537  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
538  delta);
539 }
540 
541 /* **************************************************************** */
542 template<class X1, class X2, class X3>
543 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
544  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
545  const X2& x2, const X3& x3, double delta = 1e-5) {
546  typedef typename internal::FixedSizeMatrix<X3>::type Vector;
547  Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
548  double) = &numericalGradient<X3>;
549  boost::function<double(const X3&)> f2(boost::bind(f, x1, x2, _1));
550 
551  return numericalDerivative11<Vector, X3>(
552  boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
553  x3, delta);
554 }
555 
556 template<class X1, class X2, class X3>
557 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
558  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
559  return numericalHessian333(
560  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
561  delta);
562 }
563 
564 /* **************************************************************** */
565 template<class X1, class X2, class X3>
566 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
567  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
568  const X2& x2, const X3& x3, double delta = 1e-5) {
569  return numericalHessian212<X1, X2>(
570  boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, x3)),
571  x1, x2, delta);
572 }
573 
574 template<class X1, class X2, class X3>
575 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
576  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
577  const X2& x2, const X3& x3, double delta = 1e-5) {
578  return numericalHessian212<X1, X3>(
579  boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, x2, _2)),
580  x1, x3, delta);
581 }
582 
583 template<class X1, class X2, class X3>
584 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
585  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
586  const X2& x2, const X3& x3, double delta = 1e-5) {
587  return numericalHessian212<X2, X3>(
588  boost::function<double(const X2&, const X3&)>(boost::bind(f, x1, _1, _2)),
589  x2, x3, delta);
590 }
591 
592 /* **************************************************************** */
593 template<class X1, class X2, class X3>
594 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
595  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
596  return numericalHessian312(
597  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
598  delta);
599 }
600 
601 template<class X1, class X2, class X3>
602 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
603  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
604  return numericalHessian313(
605  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
606  delta);
607 }
608 
609 template<class X1, class X2, class X3>
610 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
611  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
612  return numericalHessian323(
613  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
614  delta);
615 }
616 
617 } // namespace gtsam
618 
gtsam::numericalGradient
internal::FixedSizeMatrix< X >::type numericalGradient(boost::function< double(const X &)> h, const X &x, double delta=1e-5)
Numerically compute gradient of scalar function Class X is the input argument The class X needs to ha...
Definition: numericalDerivative.h:75
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Compute numerical derivative in argument 1 of binary function.
Definition: numericalDerivative.h:174
Lie.h
Base class and basic functions for Lie types.
gtsam::G_x1
Helper class that computes the derivative of f w.r.t.
Definition: numericalDerivative.h:414
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::numericalHessian
internal::FixedSizeMatrix< X, X >::type numericalHessian(boost::function< double(const X &)> f, const X &x, double delta=1e-5)
Compute numerical Hessian matrix.
Definition: numericalDerivative.h:392
gtsam::numericalHessian311
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian311(boost::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Numerical Hessian for tenary functions.
Definition: numericalDerivative.h:497
gtsam::manifold_tag
tag to assert a type is a manifold
Definition: Manifold.h:33
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Compute numerical derivative in argument 2 of binary function.
Definition: numericalDerivative.h:199
gtsam::numericalDerivative33
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Compute numerical derivative in argument 3 of ternary function.
Definition: numericalDerivative.h:282
gtsam::numericalDerivative43
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative43(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Compute numerical derivative in argument 3 of 4-argument function.
Definition: numericalDerivative.h:352
gtsam::internal::FixedSizeMatrix
Definition: numericalDerivative.h:64
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:116
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::numericalDerivative42
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative42(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Compute numerical derivative in argument 2 of 4-argument function.
Definition: numericalDerivative.h:331
VectorValues.h
Factor Graph Values.
gtsam::numericalDerivative44
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative44(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Compute numerical derivative in argument 4 of 4-argument function.
Definition: numericalDerivative.h:373
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Compute numerical derivative in argument 1 of ternary function.
Definition: numericalDerivative.h:226
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Compute numerical derivative in argument 2 of ternary function.
Definition: numericalDerivative.h:254
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::numericalDerivative41
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative41(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Compute numerical derivative in argument 1 of 4-argument function.
Definition: numericalDerivative.h:310