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 
77 template <class X, int N = traits<X>::dimension>
78 typename Eigen::Matrix<double, N, 1> numericalGradient(
79  boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
80  double factor = 1.0 / (2.0 * delta);
81 
82  BOOST_STATIC_ASSERT_MSG(
83  (boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
84  "Template argument X must be a manifold type.");
85  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
86 
87  // Prepare a tangent vector to perturb x with, only works for fixed size
88  typename traits<X>::TangentVector d;
89  d.setZero();
90 
91  Eigen::Matrix<double,N,1> g;
92  g.setZero();
93  for (int j = 0; j < N; j++) {
94  d(j) = delta;
95  double hxplus = h(traits<X>::Retract(x, d));
96  d(j) = -delta;
97  double hxmin = h(traits<X>::Retract(x, d));
98  d(j) = 0;
99  g(j) = (hxplus - hxmin) * factor;
100  }
101  return g;
102 }
103 
116 template <class Y, class X, int N = traits<X>::dimension>
117 // TODO Should compute fixed-size matrix
118 typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
119  boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
120  typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
121 
122  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
123  "Template argument Y must be a manifold type.");
124  typedef traits<Y> TraitsY;
125 
126  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
127  "Template argument X must be a manifold type.");
128  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
129  typedef traits<X> TraitsX;
130 
131  // get value at x, and corresponding chart
132  const Y hx = h(x);
133 
134  // Bit of a hack for now to find number of rows
135  const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx);
136  const size_t m = zeroY.size();
137 
138  // Prepare a tangent vector to perturb x with, only works for fixed size
139  Eigen::Matrix<double, N, 1> dx;
140  dx.setZero();
141 
142  // Fill in Jacobian H
143  Matrix H = Matrix::Zero(m, N);
144  const double factor = 1.0 / (2.0 * delta);
145  for (int j = 0; j < N; j++) {
146  dx(j) = delta;
147  const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
148  dx(j) = -delta;
149  const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
150  dx(j) = 0;
151  H.col(j) << (dy1 - dy2) * factor;
152  }
153  return H;
154 }
155 
157 template<class Y, class X>
158 typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
159  double delta = 1e-5) {
160  return numericalDerivative11<Y, X>(boost::bind(h, _1), x, delta);
161 }
162 
171 template<class Y, class X1, class X2>
172 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
173  const X1& x1, const X2& x2, double delta = 1e-5) {
174  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
175  "Template argument Y must be a manifold type.");
176  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
177  "Template argument X1 must be a manifold type.");
178  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
179 }
180 
182 template<class Y, class X1, class X2>
183 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
184  const X2& x2, double delta = 1e-5) {
185  return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
186 }
187 
196 template<class Y, class X1, class X2>
197 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
198  const X1& x1, const X2& x2, double delta = 1e-5) {
199 // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
200 // "Template argument X1 must be a manifold type.");
201  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
202  "Template argument X2 must be a manifold type.");
203  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1), x2, delta);
204 }
205 
207 template<class Y, class X1, class X2>
208 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
209  const X2& x2, double delta = 1e-5) {
210  return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
211 }
212 
223 template<class Y, class X1, class X2, class X3>
224 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
225  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
226  const X2& x2, const X3& x3, double delta = 1e-5) {
227  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
228  "Template argument Y must be a manifold type.");
229  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
230  "Template argument X1 must be a manifold type.");
231  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
232 }
233 
234 template<class Y, class X1, class X2, class X3>
235 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
236  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
237  return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
238  x2, x3, delta);
239 }
240 
251 template<class Y, class X1, class X2, class X3>
252 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
253  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
254  const X2& x2, const X3& x3, double delta = 1e-5) {
255  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
256  "Template argument Y must be a manifold type.");
257  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
258  "Template argument X2 must be a manifold type.");
259  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
260 }
261 
262 template<class Y, class X1, class X2, class X3>
263 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
264  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
265  return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
266  x2, x3, delta);
267 }
268 
279 template<class Y, class X1, class X2, class X3>
280 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
281  boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
282  const X2& x2, const X3& x3, double delta = 1e-5) {
283  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
284  "Template argument Y must be a manifold type.");
285  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
286  "Template argument X3 must be a manifold type.");
287  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
288 }
289 
290 template<class Y, class X1, class X2, class X3>
291 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
292  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
293  return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
294  x2, x3, delta);
295 }
296 
307 template<class Y, class X1, class X2, class X3, class X4>
308 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
309  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
310  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
311  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
312  "Template argument Y must be a manifold type.");
313  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
314  "Template argument X1 must be a manifold type.");
315  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
316 }
317 
318 template<class Y, class X1, class X2, class X3, class X4>
319 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
320  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
321  return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
322 }
323 
334 template<class Y, class X1, class X2, class X3, class X4>
335 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
336  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
337  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
338  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
339  "Template argument Y must be a manifold type.");
340  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
341  "Template argument X2 must be a manifold type.");
342  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
343 }
344 
345 template<class Y, class X1, class X2, class X3, class X4>
346 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
347  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
348  return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
349 }
350 
361 template<class Y, class X1, class X2, class X3, class X4>
362 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
363  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
364  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
365  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
366  "Template argument Y must be a manifold type.");
367  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
368  "Template argument X3 must be a manifold type.");
369  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
370 }
371 
372 template<class Y, class X1, class X2, class X3, class X4>
373 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(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 numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
376 }
377 
388 template<class Y, class X1, class X2, class X3, class X4>
389 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
390  boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
391  const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
392  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
393  "Template argument Y must be a manifold type.");
394  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
395  "Template argument X4 must be a manifold type.");
396  return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
397 }
398 
399 template<class Y, class X1, class X2, class X3, class X4>
400 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
401  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
402  return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
403 }
404 
416 template<class Y, class X1, class X2, class X3, class X4, class X5>
417 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
418  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
419  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
420  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
421  "Template argument Y must be a manifold type.");
422  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
423  "Template argument X1 must be a manifold type.");
424  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
425 }
426 
427 template<class Y, class X1, class X2, class X3, class X4, class X5>
428 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
429  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
430  return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
431 }
432 
444 template<class Y, class X1, class X2, class X3, class X4, class X5>
445 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
446  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
447  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
448  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
449  "Template argument Y must be a manifold type.");
450  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
451  "Template argument X1 must be a manifold type.");
452  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
453 }
454 
455 template<class Y, class X1, class X2, class X3, class X4, class X5>
456 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
457  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
458  return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
459 }
460 
472 template<class Y, class X1, class X2, class X3, class X4, class X5>
473 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
474  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
475  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
476  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
477  "Template argument Y must be a manifold type.");
478  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
479  "Template argument X1 must be a manifold type.");
480  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
481 }
482 
483 template<class Y, class X1, class X2, class X3, class X4, class X5>
484 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
485  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
486  return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
487 }
488 
500 template<class Y, class X1, class X2, class X3, class X4, class X5>
501 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
502  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
503  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
504  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
505  "Template argument Y must be a manifold type.");
506  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
507  "Template argument X1 must be a manifold type.");
508  return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
509 }
510 
511 template<class Y, class X1, class X2, class X3, class X4, class X5>
512 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative53(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 numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
515 }
516 
528 template<class Y, class X1, class X2, class X3, class X4, class X5>
529 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
530  boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
531  const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
532  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
533  "Template argument Y must be a manifold type.");
534  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
535  "Template argument X1 must be a manifold type.");
536  return numericalDerivative11<Y, X5>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
537 }
538 
539 template<class Y, class X1, class X2, class X3, class X4, class X5>
540 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
541  const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
542  return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
543 }
544 
553 template<class X>
554 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
555  double delta = 1e-5) {
556  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
557  "Template argument X must be a manifold type.");
558  typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
559  typedef boost::function<double(const X&)> F;
560  typedef boost::function<VectorD(F, const X&, double)> G;
561  G ng = static_cast<G>(numericalGradient<X> );
562  return numericalDerivative11<VectorD, X>(boost::bind(ng, f, _1, delta), x,
563  delta);
564 }
565 
566 template<class X>
567 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
568  1e-5) {
569  return numericalHessian(boost::function<double(const X&)>(f), x, delta);
570 }
571 
575 template<class X1, class X2>
576 class G_x1 {
577  const boost::function<double(const X1&, const X2&)>& f_;
578  X1 x1_;
579  double delta_;
580 public:
581  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
582 
583  G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
584  double delta) :
585  f_(f), x1_(x1), delta_(delta) {
586  }
587  Vector operator()(const X2& x2) {
588  return numericalGradient<X1>(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_);
589  }
590 };
591 
592 template<class X1, class X2>
593 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
594  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
595  double delta = 1e-5) {
596  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
597  G_x1<X1, X2> g_x1(f, x1, delta);
598  return numericalDerivative11<Vector, X2>(
599  boost::function<Vector(const X2&)>(
600  boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta);
601 }
602 
603 template<class X1, class X2>
604 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
605  const X1& x1, const X2& x2, double delta = 1e-5) {
606  return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
607  x1, x2, delta);
608 }
609 
610 template<class X1, class X2>
611 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
612  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
613  double delta = 1e-5) {
614 
615  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
616 
617  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
618  double) = &numericalGradient<X1>;
619  boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2)));
620 
621  return numericalDerivative11<Vector, X1>(
622  boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
623  x1, delta);
624 }
625 
626 template<class X1, class X2>
627 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
628  const X1& x1, const X2& x2, double delta = 1e-5) {
629  return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
630  x1, x2, delta);
631 }
632 
633 template<class X1, class X2>
634 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
635  boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
636  double delta = 1e-5) {
637  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
638  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
639  double) = &numericalGradient<X2>;
640  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1));
641 
642  return numericalDerivative11<Vector, X2>(
643  boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
644  x2, delta);
645 }
646 
647 template<class X1, class X2>
648 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
649  const X1& x1, const X2& x2, double delta = 1e-5) {
650  return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
651  x1, x2, delta);
652 }
653 
657 /* **************************************************************** */
658 template<class X1, class X2, class X3>
659 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
660  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
661  const X2& x2, const X3& x3, double delta = 1e-5) {
662  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
663  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
664  double) = &numericalGradient<X1>;
665  boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3)));
666 
667  return numericalDerivative11<Vector, X1>(
668  boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
669  x1, delta);
670 }
671 
672 template<class X1, class X2, class X3>
673 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
674  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
675  return numericalHessian311(
676  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
677  delta);
678 }
679 
680 /* **************************************************************** */
681 template<class X1, class X2, class X3>
682 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
683  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
684  const X2& x2, const X3& x3, double delta = 1e-5) {
685  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
686  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
687  double) = &numericalGradient<X2>;
688  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3)));
689 
690  return numericalDerivative11<Vector, X2>(
691  boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
692  x2, delta);
693 }
694 
695 template<class X1, class X2, class X3>
696 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
697  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
698  return numericalHessian322(
699  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
700  delta);
701 }
702 
703 /* **************************************************************** */
704 template<class X1, class X2, class X3>
705 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
706  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
707  const X2& x2, const X3& x3, double delta = 1e-5) {
708  typedef typename internal::FixedSizeMatrix<X3>::type Vector;
709  Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
710  double) = &numericalGradient<X3>;
711  boost::function<double(const X3&)> f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1));
712 
713  return numericalDerivative11<Vector, X3>(
714  boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
715  x3, delta);
716 }
717 
718 template<class X1, class X2, class X3>
719 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
720  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
721  return numericalHessian333(
722  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
723  delta);
724 }
725 
726 /* **************************************************************** */
727 template<class X1, class X2, class X3>
728 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
729  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
730  const X2& x2, const X3& x3, double delta = 1e-5) {
731  return numericalHessian212<X1, X2>(
732  boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, boost::cref(x3))),
733  x1, x2, delta);
734 }
735 
736 template<class X1, class X2, class X3>
737 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
738  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
739  const X2& x2, const X3& x3, double delta = 1e-5) {
740  return numericalHessian212<X1, X3>(
741  boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, boost::cref(x2), _2)),
742  x1, x3, delta);
743 }
744 
745 template<class X1, class X2, class X3>
746 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
747  boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
748  const X2& x2, const X3& x3, double delta = 1e-5) {
749  return numericalHessian212<X2, X3>(
750  boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), _1, _2)),
751  x2, x3, delta);
752 }
753 
754 /* **************************************************************** */
755 template<class X1, class X2, class X3>
756 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
757  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
758  return numericalHessian312(
759  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
760  delta);
761 }
762 
763 template<class X1, class X2, class X3>
764 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
765  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
766  return numericalHessian313(
767  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
768  delta);
769 }
770 
771 template<class X1, class X2, class X3>
772 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
773  const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
774  return numericalHessian323(
775  boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
776  delta);
777 }
778 
779 } // namespace gtsam
780 
gtsam::numericalDerivative51
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative51(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Compute numerical derivative in argument 1 of 5-argument function.
Definition: numericalDerivative.h:417
gtsam::internal::FixedSizeMatrix
Definition: numericalDerivative.h:64
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:335
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:224
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:308
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:172
gtsam::numericalGradient
Eigen::Matrix< double, N, 1 > numericalGradient(boost::function< double(const X &)> h, const X &x, double delta=1e-5)
Numerically compute gradient of scalar function.
Definition: numericalDerivative.h:78
gtsam::numericalDerivative54
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative54(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Compute numerical derivative in argument 4 of 5-argument function.
Definition: numericalDerivative.h:501
gtsam::numericalDerivative55
internal::FixedSizeMatrix< Y, X5 >::type numericalDerivative55(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Compute numerical derivative in argument 5 of 5-argument function.
Definition: numericalDerivative.h:529
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:659
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:554
gtsam::G_x1
Helper class that computes the derivative of f w.r.t.
Definition: numericalDerivative.h:576
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:197
gtsam::manifold_tag
tag to assert a type is a manifold
Definition: Manifold.h:33
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
Values.h
A non-templated config holding any types of Manifold-group elements.
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:362
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:252
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:118
gtsam::numericalDerivative52
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative52(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Compute numerical derivative in argument 2 of 5-argument function.
Definition: numericalDerivative.h:445
gtsam::numericalDerivative53
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative53(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Compute numerical derivative in argument 3 of 5-argument function.
Definition: numericalDerivative.h:473
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:389
Lie.h
Base class and basic functions for Lie types.
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:280
VectorValues.h
Factor Graph Values.