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