gtsam  3.2.1
gtsam
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 
20 #pragma once
21 
22 #include <boost/function.hpp>
23 #ifdef __GNUC__
24 #pragma GCC diagnostic push
25 #pragma GCC diagnostic ignored "-Wunused-variable"
26 #endif
27 #include <boost/bind.hpp>
28 #ifdef __GNUC__
29 #pragma GCC diagnostic pop
30 #endif
31 
32 #include <gtsam/base/LieVector.h>
33 #include <gtsam/base/Matrix.h>
34 
35 namespace gtsam {
36 
37  /*
38  * Note that all of these functions have two versions, a boost.function version and a
39  * standard C++ function pointer version. This allows reformulating the arguments of
40  * a function to fit the correct structure, which is useful for situations like
41  * member functions and functions with arguments not involved in the derivative:
42  *
43  * Usage of the boost bind version to rearrange arguments:
44  * for a function with one relevant param and an optional derivative:
45  * Foo bar(const Obj& a, boost::optional<Matrix&> H1)
46  * Use boost.bind to restructure:
47  * boost::bind(bar, _1, boost::none)
48  * This syntax will fix the optional argument to boost::none, while using the first argument provided
49  *
50  * For member functions, such as below, with an instantiated copy instanceOfSomeClass
51  * Foo SomeClass::bar(const Obj& a)
52  * Use boost bind as follows to create a function pointer that uses the member function:
53  * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1)
54  *
55  * For additional details, see the documentation:
56  * http://www.boost.org/doc/libs/release/libs/bind/bind.html
57  */
58 
59 
61  inline LieVector makeLieVector(const Vector& v) { return LieVector(v); }
62  inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); }
63 
69  template<class X>
70  Vector numericalGradient(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
71  double factor = 1.0/(2.0*delta);
72  const size_t n = x.dim();
73  Vector d = zero(n), g = zero(n);
74  for (size_t j=0;j<n;j++) {
75  d(j) += delta; double hxplus = h(x.retract(d));
76  d(j) -= 2*delta; double hxmin = h(x.retract(d));
77  d(j) += delta; g(j) = (hxplus-hxmin)*factor;
78  }
79  return g;
80  }
81 
82  template<class X>
83  Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) {
84  return numericalGradient<X>(boost::bind(h, _1), x, delta);
85  }
86 
97  template<class Y, class X>
98  Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x, double delta=1e-5) {
99  Y hx = h(x);
100  double factor = 1.0/(2.0*delta);
101  const size_t m = hx.dim(), n = x.dim();
102  Vector d = zero(n);
103  Matrix H = zeros(m,n);
104  for (size_t j=0;j<n;j++) {
105  d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
106  d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
107  d(j) += delta;
108  H.col(j) << (hxplus-hxmin)*factor;
109  }
110  return H;
111  }
112 
114  template<class Y, class X>
115  Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
116  return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
117  }
118 
119 // /** remapping for double valued functions */
120 // template<class X>
121 // Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
122 // return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
123 // }
124 
125  template<class X>
126  Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) {
127  return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
128  }
129 
131  template<class X>
132  Matrix numericalDerivative11(boost::function<Vector(const X&)> h, const X& x, double delta=1e-5) {
133  return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
134  }
135 
136  template<class X>
137  Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) {
138  return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
139  }
140 
150  template<class Y, class X1, class X2>
151  Matrix numericalDerivative21(boost::function<Y(const X1&, const X2&)> h,
152  const X1& x1, const X2& x2, double delta=1e-5) {
153  Y hx = h(x1,x2);
154  double factor = 1.0/(2.0*delta);
155  const size_t m = hx.dim(), n = x1.dim();
156  Vector d = zero(n);
157  Matrix H = zeros(m,n);
158  for (size_t j=0;j<n;j++) {
159  d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2));
160  d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2));
161  d(j) += delta;
162  H.col(j) << (hxplus-hxmin)*factor;
163  }
164  return H;
165  }
166 
168  template<class Y, class X1, class X2>
169  inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
170  const X1& x1, const X2& x2, double delta=1e-5) {
171  return numericalDerivative21<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
172  }
173 
175  template<class X1, class X2>
176  Matrix numericalDerivative21(boost::function<double(const X1&, const X2&)> h,
177  const X1& x1, const X2& x2, double delta=1e-5) {
178  return numericalDerivative21<LieVector,X1,X2>(
179  boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
180  }
181 
182  template<class X1, class X2>
183  Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
184  const X1& x1, const X2& x2, double delta=1e-5) {
185  return numericalDerivative21<LieVector,X1,X2>(
186  boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
187  }
188 
190  template<class X1, class X2>
191  Matrix numericalDerivative21(boost::function<Vector(const X1&, const X2&)> h,
192  const X1& x1, const X2& x2, double delta=1e-5) {
193  return numericalDerivative21<LieVector,X1,X2>(
194  boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
195  }
196 
197  template<class X1, class X2>
198  inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&),
199  const X1& x1, const X2& x2, double delta=1e-5) {
200  return numericalDerivative21<LieVector,X1,X2>(
201  boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
202  }
203 
213  template<class Y, class X1, class X2>
214  Matrix numericalDerivative22
215  (boost::function<Y(const X1&, const X2&)> h,
216  const X1& x1, const X2& x2, double delta=1e-5) {
217  Y hx = h(x1,x2);
218  double factor = 1.0/(2.0*delta);
219  const size_t m = hx.dim(), n = x2.dim();
220  Vector d = zero(n);
221  Matrix H = zeros(m,n);
222  for (size_t j=0;j<n;j++) {
223  d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1,x2.retract(d)));
224  d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1,x2.retract(d)));
225  d(j) += delta;
226  H.col(j) << (hxplus-hxmin)*factor;
227  }
228  return H;
229  }
230 
232  template<class Y, class X1, class X2>
233  inline Matrix numericalDerivative22
234  (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
235  return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
236  }
237 
239  template<class X1, class X2>
240  Matrix numericalDerivative22(boost::function<double(const X1&, const X2&)> h,
241  const X1& x1, const X2& x2, double delta=1e-5) {
242  return numericalDerivative22<LieVector,X1,X2>(
243  boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
244  }
245 
246  template<class X1, class X2>
247  inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&),
248  const X1& x1, const X2& x2, double delta=1e-5) {
249  return numericalDerivative22<LieVector,X1,X2>(
250  boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
251  }
252 
254  template<class X1, class X2>
255  Matrix numericalDerivative22(boost::function<Vector(const X1&, const X2&)> h,
256  const X1& x1, const X2& x2, double delta=1e-5) {
257  return numericalDerivative22<LieVector,X1,X2>(
258  boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
259  }
260 
261  template<class X1, class X2>
262  inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&),
263  const X1& x1, const X2& x2, double delta=1e-5) {
264  return numericalDerivative22<LieVector,X1,X2>(
265  boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
266  }
267 
278  template<class Y, class X1, class X2, class X3>
279  Matrix numericalDerivative31
280  (boost::function<Y(const X1&, const X2&, const X3&)> h,
281  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
282  {
283  Y hx = h(x1,x2,x3);
284  double factor = 1.0/(2.0*delta);
285  const size_t m = hx.dim(), n = x1.dim();
286  Vector d = zero(n);
287  Matrix H = zeros(m,n);
288  for (size_t j=0;j<n;j++) {
289  d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2,x3));
290  d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2,x3));
291  d(j) += delta;
292  H.col(j) << (hxplus-hxmin)*factor;
293  }
294  return H;
295  }
296  template<class Y, class X1, class X2, class X3>
297  inline Matrix numericalDerivative31
298  (Y (*h)(const X1&, const X2&, const X3&),
299  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
300  return numericalDerivative31<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
301  }
302 
304  template<class X1, class X2, class X3>
305  Matrix numericalDerivative31(boost::function<double(const X1&, const X2&, const X3&)> h,
306  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
307  return numericalDerivative31<LieVector,X1,X2,X3>(
308  boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
309  }
310 
311  template<class X1, class X2, class X3>
312  inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&),
313  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
314  return numericalDerivative31<LieVector,X1,X2,X3>(
315  boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
316  }
317 
319  template<class X1, class X2, class X3>
320  Matrix numericalDerivative31(boost::function<Vector(const X1&, const X2&, const X3&)> h,
321  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
322  return numericalDerivative31<LieVector,X1,X2,X3>(
323  boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
324  }
325 
326  template<class X1, class X2, class X3>
327  inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&),
328  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
329  return numericalDerivative31<LieVector,X1,X2,X3>(
330  boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
331  }
332 
343  template<class Y, class X1, class X2, class X3>
344  Matrix numericalDerivative32
345  (boost::function<Y(const X1&, const X2&, const X3&)> h,
346  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
347  {
348  Y hx = h(x1,x2,x3);
349  double factor = 1.0/(2.0*delta);
350  const size_t m = hx.dim(), n = x2.dim();
351  Vector d = zero(n);
352  Matrix H = zeros(m,n);
353  for (size_t j=0;j<n;j++) {
354  d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2.retract(d),x3));
355  d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2.retract(d),x3));
356  d(j) += delta;
357  H.col(j) << (hxplus-hxmin)*factor;
358  }
359  return H;
360  }
361  template<class Y, class X1, class X2, class X3>
362  inline Matrix numericalDerivative32
363  (Y (*h)(const X1&, const X2&, const X3&),
364  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
365  return numericalDerivative32<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
366  }
367 
369  template<class X1, class X2, class X3>
370  Matrix numericalDerivative32(boost::function<double(const X1&, const X2&, const X3&)> h,
371  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
372  return numericalDerivative32<LieVector,X1,X2,X3>(
373  boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
374  }
375 
376  template<class X1, class X2, class X3>
377  inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&),
378  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
379  return numericalDerivative32<LieVector,X1,X2,X3>(
380  boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
381  }
382 
384  template<class X1, class X2, class X3>
385  Matrix numericalDerivative32(boost::function<Vector(const X1&, const X2&, const X3&)> h,
386  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
387  return numericalDerivative32<LieVector,X1,X2,X3>(
388  boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
389  }
390 
391  template<class X1, class X2, class X3>
392  inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&),
393  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
394  return numericalDerivative32<LieVector,X1,X2,X3>(
395  boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
396  }
397 
408  template<class Y, class X1, class X2, class X3>
409  Matrix numericalDerivative33
410  (boost::function<Y(const X1&, const X2&, const X3&)> h,
411  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
412  {
413  Y hx = h(x1,x2,x3);
414  double factor = 1.0/(2.0*delta);
415  const size_t m = hx.dim(), n = x3.dim();
416  Vector d = zero(n);
417  Matrix H = zeros(m,n);
418  for (size_t j=0;j<n;j++) {
419  d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2, x3.retract(d)));
420  d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2, x3.retract(d)));
421  d(j) += delta;
422  H.col(j) << (hxplus-hxmin)*factor;
423  }
424  return H;
425  }
426  template<class Y, class X1, class X2, class X3>
427  inline Matrix numericalDerivative33
428  (Y (*h)(const X1&, const X2&, const X3&),
429  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
430  return numericalDerivative33<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
431  }
432 
434  template<class X1, class X2, class X3>
435  Matrix numericalDerivative33(boost::function<double(const X1&, const X2&, const X3&)> h,
436  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
437  return numericalDerivative33<LieVector,X1,X2,X3>(
438  boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
439  }
440 
441  template<class X1, class X2, class X3>
442  inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&),
443  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
444  return numericalDerivative33<LieVector,X1,X2,X3>(
445  boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
446  }
447 
449  template<class X1, class X2, class X3>
450  Matrix numericalDerivative33(boost::function<Vector(const X1&, const X2&, const X3&)> h,
451  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
452  return numericalDerivative33<LieVector,X1,X2,X3>(
453  boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
454  }
455 
456  template<class X1, class X2, class X3>
457  inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&),
458  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
459  return numericalDerivative33<LieVector,X1,X2,X3>(
460  boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
461  }
462 
471  template<class X>
472  inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x, double delta=1e-5) {
473  return numericalDerivative11<X>(boost::function<Vector(const X&)>(boost::bind(
474  static_cast<Vector (*)(boost::function<double(const X&)>,const X&, double)>(&numericalGradient<X>),
475  f, _1, delta)), x, delta);
476  }
477 
478  template<class X>
479  inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) {
480  return numericalHessian(boost::function<double(const X&)>(f), x, delta);
481  }
482 
483 
487  template<class X1, class X2>
488  class G_x1 {
489  const boost::function<double(const X1&, const X2&)>& f_;
490  const X1& x1_;
491  double delta_;
492  public:
493  G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) {}
494  Vector operator()(const X2& x2) {
495  return numericalGradient<X1>(boost::function<double (const X1&)>(boost::bind(f_, _1, x2)), x1_, delta_);
496  }
497  };
498 
499  template<class X1, class X2>
500  inline Matrix numericalHessian212(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
501  G_x1<X1,X2> g_x1(f, x1, delta);
502  return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta);
503  }
504 
505 
506  template<class X1, class X2>
507  inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
508  return numericalHessian212(boost::function<double (const X1&, const X2&)>(f), x1, x2, delta);
509  }
510 
511 
512  template<class X1, class X2>
513  inline Matrix numericalHessian211(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
514 
515  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&, double) = &numericalGradient<X1>;
516  boost::function<double (const X1&)> f2(boost::bind(f, _1, x2));
517 
518  return numericalDerivative11<X1>(boost::function<Vector (const X1&)>(boost::bind(numGrad, f2, _1, delta)), x1, delta);
519  }
520 
521 
522  template<class X1, class X2>
523  inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
524  return numericalHessian211(boost::function<double (const X1&, const X2&)>(f), x1, x2, delta);
525  }
526 
527 
528  template<class X1, class X2>
529  inline Matrix numericalHessian222(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
530 
531  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&, double) = &numericalGradient<X2>;
532  boost::function<double (const X2&)> f2(boost::bind(f, x1, _1));
533 
534  return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind(numGrad, f2, _1, delta)), x2, delta);
535  }
536 
537 
538  template<class X1, class X2>
539  inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
540  return numericalHessian222(boost::function<double (const X1&, const X2&)>(f), x1, x2, delta);
541  }
542 
546  /* **************************************************************** */
547  template<class X1, class X2, class X3>
548  inline Matrix numericalHessian311(boost::function<double(const X1&, const X2&, const X3&)> f,
549  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
550 
551  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&, double) = &numericalGradient<X1>;
552  boost::function<double (const X1&)> f2(boost::bind(f, _1, x2, x3));
553 
554  return numericalDerivative11<X1>(boost::function<Vector (const X1&)>(boost::bind(numGrad, f2, _1, delta)), x1, delta);
555  }
556 
557  template<class X1, class X2, class X3>
558  inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
559  return numericalHessian311(boost::function<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
560  }
561 
562  /* **************************************************************** */
563  template<class X1, class X2, class X3>
564  inline Matrix numericalHessian322(boost::function<double(const X1&, const X2&, const X3&)> f,
565  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
566 
567  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&, double) = &numericalGradient<X2>;
568  boost::function<double (const X2&)> f2(boost::bind(f, x1, _1, x3));
569 
570  return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind(numGrad, f2, _1, delta)), x2, delta);
571  }
572 
573  template<class X1, class X2, class X3>
574  inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
575  return numericalHessian322(boost::function<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
576  }
577 
578  /* **************************************************************** */
579  template<class X1, class X2, class X3>
580  inline Matrix numericalHessian333(boost::function<double(const X1&, const X2&, const X3&)> f,
581  const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
582 
583  Vector (*numGrad)(boost::function<double(const X3&)>, const X3&, double) = &numericalGradient<X3>;
584  boost::function<double (const X3&)> f2(boost::bind(f, x1, x2, _1));
585 
586  return numericalDerivative11<X3>(boost::function<Vector (const X3&)>(boost::bind(numGrad, f2, _1, delta)), x3, delta);
587  }
588 
589  template<class X1, class X2, class X3>
590  inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
591  return numericalHessian333(boost::function<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
592  }
593 
594  /* **************************************************************** */
595  template<class X1, class X2, class X3>
596  inline Matrix numericalHessian312(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
597  return numericalHessian212<X1,X2>(boost::function<double (const X1&, const X2&)>(boost::bind(f, _1, _2, x3)), x1, x2, delta );
598  }
599 
600  template<class X1, class X2, class X3>
601  inline Matrix numericalHessian313(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
602  return numericalHessian212<X1,X3>(boost::function<double (const X1&, const X3&)>(boost::bind(f, _1, x2, _2)), x1, x3, delta );
603  }
604 
605  template<class X1, class X2, class X3>
606  inline Matrix numericalHessian323(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
607  return numericalHessian212<X2,X3>(boost::function<double (const X2&, const X3&)>(boost::bind(f, x1, _1, _2)), x2, x3, delta );
608  }
609 
610  /* **************************************************************** */
611  template<class X1, class X2, class X3>
612  inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
613  return numericalHessian312(boost::function<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
614  }
615 
616  template<class X1, class X2, class X3>
617  inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
618  return numericalHessian313(boost::function<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
619  }
620 
621  template<class X1, class X2, class X3>
622  inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
623  return numericalHessian323(boost::function<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
624  }
625 }
gtsam::numericalDerivative22
Matrix 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:215
gtsam::numericalDerivative21
Matrix numericalDerivative21(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:151
gtsam::G_x1
Helper class that computes the derivative of f w.r.t.
Definition: numericalDerivative.h:488
gtsam::numericalHessian
Matrix numericalHessian(boost::function< double(const X &)> f, const X &x, double delta=1e-5)
Compute numerical Hessian matrix.
Definition: numericalDerivative.h:472
gtsam::numericalHessian311
Matrix 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:548
gtsam::numericalGradient
Vector 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:70
gtsam::zero
bool zero(const Vector &v)
check if all zero
Definition: Vector.cpp:39
LieVector.h
A wrapper around vector providing Lie compatibility.
gtsam::numericalDerivative33
Matrix 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:410
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::numericalDerivative32
Matrix 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:345
gtsam::makeLieVector
LieVector makeLieVector(const Vector &v)
global functions for converting to a LieVector for use with numericalDerivative
Definition: numericalDerivative.h:61
gtsam::LieVector
LieVector is a wrapper around vector to allow it to be a Lie type.
Definition: LieVector.h:29
gtsam::delta
Vector delta(size_t n, size_t i, double value)
Create basis vector of dimension n, with a constant in spot i.
Definition: Vector.cpp:53
gtsam::zeros
Matrix zeros(size_t m, size_t n)
Creates an zeros matrix, with matlab-like syntax.
Definition: Matrix.cpp:40
gtsam::numericalDerivative11
Matrix numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
Compute numerical derivative in argument 1 of unary function.
Definition: numericalDerivative.h:98
gtsam::numericalDerivative31
Matrix 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:280