gtsam
Loading...
Searching...
No Matches
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
26
27// \callgraph
28#pragma once
29
30#include <gtsam/base/Lie.h>
31#include <gtsam/nonlinear/Values.h>
32
33#include <functional>
34#include <type_traits>
35
41namespace gtsam {
42
45
53
56namespace internal {
58template <class Y, class X = double>
59struct FixedSizeMatrix {
60 typedef Eigen::Matrix<double, traits<Y>::dimension, traits<X>::dimension>
61 type;
62};
63
65template <int M, int N>
66struct MatrixMN {
67 typedef Eigen::Matrix<double, M, N> type;
68};
69
71struct DeducedOutput {};
72
73template <class RequestedY, class F, class... Args>
74using OutputType =
75 std::conditional_t<std::is_same_v<RequestedY, DeducedOutput>,
76 std::decay_t<std::invoke_result_t<F&, const Args&...>>,
77 RequestedY>;
78
79template <class F, class... Args>
80using EnableIfInvocable =
81 std::enable_if_t<std::is_invocable_v<F&, const Args&...>, int>;
82
83template <class F, class... Args>
84using EnableIfScalarInvocable =
85 std::enable_if_t<std::is_invocable_r_v<double, F&, const Args&...>, int>;
86
87template <class X, int N>
88constexpr void ValidateGradientInput() {
89 static_assert(std::is_base_of_v<gtsam::manifold_tag,
90 typename traits<X>::structure_category>,
91 "Template argument X must be a manifold type.");
92 static_assert(
93 N > 0,
94 "Template argument X must be fixed-size type or N must be specified.");
95}
96
97template <class Y, class X, int N>
98constexpr void ValidateUnaryDerivativeTypes() {
99 static_assert(std::is_base_of_v<gtsam::manifold_tag,
100 typename traits<Y>::structure_category>,
101 "Template argument Y must be a manifold type.");
102 ValidateGradientInput<X, N>();
103}
104} // namespace internal
106
109
131template <class X, int N = traits<X>::dimension, class F,
132 internal::EnableIfScalarInvocable<F, X> = 0>
133typename Eigen::Matrix<double, N, 1> numericalGradient(F&& h, const X& x,
134 double delta = 1e-5) {
135 internal::ValidateGradientInput<X, N>();
136 double factor = 1.0 / (2.0 * delta);
137
138 // Prepare a tangent vector to perturb x with.
139 Eigen::Matrix<double, N, 1> d;
140 d.setZero();
141
142 Eigen::Matrix<double, N, 1> g;
143 g.setZero();
144 for (int j = 0; j < N; j++) {
145 d(j) = delta;
146 double hx_plus = std::invoke(h, traits<X>::Retract(x, d));
147 d(j) = -delta;
148 double hx_min = std::invoke(h, traits<X>::Retract(x, d));
149 d(j) = 0;
150 g(j) = (hx_plus - hx_min) * factor;
151 }
152 return g;
153}
154
156template <class X, int N = traits<X>::dimension>
157typename Eigen::Matrix<double, N, 1> numericalGradient(double (&h)(const X&),
158 const X& x,
159 double delta = 1e-5) {
160 return numericalGradient<X, N>([&](const X& x_) { return h(x_); }, x, delta);
161}
162
175template <class Y = internal::DeducedOutput, class X,
176 int N = traits<X>::dimension, class F,
177 internal::EnableIfInvocable<F, X> = 0>
178typename internal::MatrixMN<traits<internal::OutputType<Y, F, X>>::dimension,
179 N>::type
180numericalDerivative11(F&& h, const X& x, double delta = 1e-5) {
181 using ActualY = internal::OutputType<Y, F, X>;
182 internal::ValidateUnaryDerivativeTypes<ActualY, X, N>();
183 typedef
184 typename internal::MatrixMN<traits<ActualY>::dimension, N>::type MatrixYN;
185 typedef traits<ActualY> TraitsY;
186 typedef traits<X> TraitsX;
187
188 // get value at x, and corresponding chart
189 const ActualY hx = std::invoke(h, x);
190
191 // Find number of rows m
192 const size_t m = TraitsY::GetDimension(hx);
193
194 // Prepare a tangent vector to perturb x with
195 Eigen::Matrix<double, N, 1> dx;
196 dx.setZero();
197
198 // Fill in Jacobian H
199 MatrixYN H = MatrixYN::Zero(m, N);
200 const double factor = 1.0 / (2.0 * delta);
201 for (int j = 0; j < N; j++) {
202 dx(j) = delta;
203 const auto dy1 =
204 TraitsY::Local(hx, std::invoke(h, TraitsX::Retract(x, dx)));
205 dx(j) = -delta;
206 const auto dy2 =
207 TraitsY::Local(hx, std::invoke(h, TraitsX::Retract(x, dx)));
208 dx(j) = 0;
209 H.col(j) = (dy1 - dy2) * factor;
210 }
211 return H;
212}
213
215template <class Y, class X, int N = traits<X>::dimension>
216typename internal::MatrixMN<traits<Y>::dimension, N>::type
217numericalDerivative11(Y (&h)(const X&), const X& x, double delta = 1e-5) {
218 return numericalDerivative11<Y, X, N>([&](const X& x_) { return h(x_); }, x,
219 delta);
220}
221
231template <class Y = internal::DeducedOutput, class X1, class X2,
232 int N = traits<X1>::dimension, class F,
233 internal::EnableIfInvocable<F, X1, X2> = 0>
234typename internal::MatrixMN<
235 traits<internal::OutputType<Y, F, X1, X2>>::dimension, N>::type
236numericalDerivative21(F&& h, const X1& x1, const X2& x2, double delta = 1e-5) {
237 using ActualY = internal::OutputType<Y, F, X1, X2>;
238 return numericalDerivative11<ActualY, X1, N>(
239 [&](const X1& x1_) { return std::invoke(h, x1_, x2); }, x1, delta);
240}
241
243template <class Y, class X1, class X2, int N = traits<X1>::dimension>
244typename internal::MatrixMN<traits<Y>::dimension, N>::type
245numericalDerivative21(Y (&h)(const X1&, const X2&), const X1& x1, const X2& x2,
246 double delta = 1e-5) {
247 return numericalDerivative11<Y, X1, N>(
248 [&](const X1& x1_) { return h(x1_, x2); }, x1, delta);
249}
250
260template <class Y = internal::DeducedOutput, class X1, class X2,
261 int N = traits<X2>::dimension, class F,
262 internal::EnableIfInvocable<F, X1, X2> = 0>
263typename internal::MatrixMN<
264 traits<internal::OutputType<Y, F, X1, X2>>::dimension, N>::type
265numericalDerivative22(F&& h, const X1& x1, const X2& x2, double delta = 1e-5) {
266 using ActualY = internal::OutputType<Y, F, X1, X2>;
267 return numericalDerivative11<ActualY, X2, N>(
268 [&](const X2& x2_) { return std::invoke(h, x1, x2_); }, x2, delta);
269}
270
272template <class Y, class X1, class X2, int N = traits<X2>::dimension>
273typename internal::MatrixMN<traits<Y>::dimension, N>::type
274numericalDerivative22(Y (&h)(const X1&, const X2&), const X1& x1, const X2& x2,
275 double delta = 1e-5) {
276 return numericalDerivative11<Y, X2, N>(
277 [&](const X2& x2_) { return h(x1, x2_); }, x2, delta);
278}
279
289template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
290 int N = traits<X1>::dimension, class F,
291 internal::EnableIfInvocable<F, X1, X2, X3> = 0>
292typename internal::MatrixMN<
293 traits<internal::OutputType<Y, F, X1, X2, X3>>::dimension, N>::type
294numericalDerivative31(F&& h, const X1& x1, const X2& x2, const X3& x3,
295 double delta = 1e-5) {
296 using ActualY = internal::OutputType<Y, F, X1, X2, X3>;
297 return numericalDerivative11<ActualY, X1, N>(
298 [&](const X1& x1_) { return std::invoke(h, x1_, x2, x3); }, x1, delta);
299}
300
302template <class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
303typename internal::MatrixMN<traits<Y>::dimension, N>::type
304numericalDerivative31(Y (&h)(const X1&, const X2&, const X3&), const X1& x1,
305 const X2& x2, const X3& x3, double delta = 1e-5) {
306 return numericalDerivative11<Y, X1, N>(
307 [&](const X1& x1_) { return h(x1_, x2, x3); }, x1, delta);
308}
309
319template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
320 int N = traits<X2>::dimension, class F,
321 internal::EnableIfInvocable<F, X1, X2, X3> = 0>
322typename internal::MatrixMN<
323 traits<internal::OutputType<Y, F, X1, X2, X3>>::dimension, N>::type
324numericalDerivative32(F&& h, const X1& x1, const X2& x2, const X3& x3,
325 double delta = 1e-5) {
326 using ActualY = internal::OutputType<Y, F, X1, X2, X3>;
327 return numericalDerivative11<ActualY, X2, N>(
328 [&](const X2& x2_) { return std::invoke(h, x1, x2_, x3); }, x2, delta);
329}
330
332template <class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
333typename internal::MatrixMN<traits<Y>::dimension, N>::type
334numericalDerivative32(Y (&h)(const X1&, const X2&, const X3&), const X1& x1,
335 const X2& x2, const X3& x3, double delta = 1e-5) {
336 return numericalDerivative11<Y, X2, N>(
337 [&](const X2& x2_) { return h(x1, x2_, x3); }, x2, delta);
338}
339
349template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
350 int N = traits<X3>::dimension, class F,
351 internal::EnableIfInvocable<F, X1, X2, X3> = 0>
352typename internal::MatrixMN<
353 traits<internal::OutputType<Y, F, X1, X2, X3>>::dimension, N>::type
354numericalDerivative33(F&& h, const X1& x1, const X2& x2, const X3& x3,
355 double delta = 1e-5) {
356 using ActualY = internal::OutputType<Y, F, X1, X2, X3>;
357 return numericalDerivative11<ActualY, X3, N>(
358 [&](const X3& x3_) { return std::invoke(h, x1, x2, x3_); }, x3, delta);
359}
360
362template <class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
363typename internal::MatrixMN<traits<Y>::dimension, N>::type
364numericalDerivative33(Y (&h)(const X1&, const X2&, const X3&), const X1& x1,
365 const X2& x2, const X3& x3, double delta = 1e-5) {
366 return numericalDerivative11<Y, X3, N>(
367 [&](const X3& x3_) { return h(x1, x2, x3_); }, x3, delta);
368}
369
379template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
380 class X4, int N = traits<X1>::dimension, class F,
381 internal::EnableIfInvocable<F, X1, X2, X3, X4> = 0>
382typename internal::MatrixMN<
383 traits<internal::OutputType<Y, F, X1, X2, X3, X4>>::dimension, N>::type
384numericalDerivative41(F&& h, const X1& x1, const X2& x2, const X3& x3,
385 const X4& x4, double delta = 1e-5) {
386 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4>;
387 return numericalDerivative11<ActualY, X1, N>(
388 [&](const X1& x1_) { return std::invoke(h, x1_, x2, x3, x4); }, x1,
389 delta);
390}
391
393template <class Y, class X1, class X2, class X3, class X4,
394 int N = traits<X1>::dimension>
395typename internal::MatrixMN<traits<Y>::dimension, N>::type
396numericalDerivative41(Y (&h)(const X1&, const X2&, const X3&, const X4&),
397 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
398 double delta = 1e-5) {
399 return numericalDerivative11<Y, X1, N>(
400 [&](const X1& x1_) { return h(x1_, x2, x3, x4); }, x1, delta);
401}
402
412template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
413 class X4, int N = traits<X2>::dimension, class F,
414 internal::EnableIfInvocable<F, X1, X2, X3, X4> = 0>
415typename internal::MatrixMN<
416 traits<internal::OutputType<Y, F, X1, X2, X3, X4>>::dimension, N>::type
417numericalDerivative42(F&& h, const X1& x1, const X2& x2, const X3& x3,
418 const X4& x4, double delta = 1e-5) {
419 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4>;
420 return numericalDerivative11<ActualY, X2, N>(
421 [&](const X2& x2_) { return std::invoke(h, x1, x2_, x3, x4); }, x2,
422 delta);
423}
424
426template <class Y, class X1, class X2, class X3, class X4,
427 int N = traits<X2>::dimension>
428typename internal::MatrixMN<traits<Y>::dimension, N>::type
429numericalDerivative42(Y (&h)(const X1&, const X2&, const X3&, const X4&),
430 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
431 double delta = 1e-5) {
432 return numericalDerivative11<Y, X2, N>(
433 [&](const X2& x2_) { return h(x1, x2_, x3, x4); }, x2, delta);
434}
435
445template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
446 class X4, int N = traits<X3>::dimension, class F,
447 internal::EnableIfInvocable<F, X1, X2, X3, X4> = 0>
448typename internal::MatrixMN<
449 traits<internal::OutputType<Y, F, X1, X2, X3, X4>>::dimension, N>::type
450numericalDerivative43(F&& h, const X1& x1, const X2& x2, const X3& x3,
451 const X4& x4, double delta = 1e-5) {
452 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4>;
453 return numericalDerivative11<ActualY, X3, N>(
454 [&](const X3& x3_) { return std::invoke(h, x1, x2, x3_, x4); }, x3,
455 delta);
456}
457
459template <class Y, class X1, class X2, class X3, class X4,
460 int N = traits<X3>::dimension>
461typename internal::MatrixMN<traits<Y>::dimension, N>::type
462numericalDerivative43(Y (&h)(const X1&, const X2&, const X3&, const X4&),
463 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
464 double delta = 1e-5) {
465 return numericalDerivative11<Y, X3, N>(
466 [&](const X3& x3_) { return h(x1, x2, x3_, x4); }, x3, delta);
467}
468
478template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
479 class X4, int N = traits<X4>::dimension, class F,
480 internal::EnableIfInvocable<F, X1, X2, X3, X4> = 0>
481typename internal::MatrixMN<
482 traits<internal::OutputType<Y, F, X1, X2, X3, X4>>::dimension, N>::type
483numericalDerivative44(F&& h, const X1& x1, const X2& x2, const X3& x3,
484 const X4& x4, double delta = 1e-5) {
485 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4>;
486 return numericalDerivative11<ActualY, X4, N>(
487 [&](const X4& x4_) { return std::invoke(h, x1, x2, x3, x4_); }, x4,
488 delta);
489}
490
492template <class Y, class X1, class X2, class X3, class X4,
493 int N = traits<X4>::dimension>
494typename internal::MatrixMN<traits<Y>::dimension, N>::type
495numericalDerivative44(Y (&h)(const X1&, const X2&, const X3&, const X4&),
496 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
497 double delta = 1e-5) {
498 return numericalDerivative11<Y, X4, N>(
499 [&](const X4& x4_) { return h(x1, x2, x3, x4_); }, x4, delta);
500}
501
511template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
512 class X4, class X5, int N = traits<X1>::dimension, class F,
513 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5> = 0>
514typename internal::MatrixMN<
515 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5>>::dimension, N>::type
516numericalDerivative51(F&& h, const X1& x1, const X2& x2, const X3& x3,
517 const X4& x4, const X5& x5, double delta = 1e-5) {
518 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5>;
519 return numericalDerivative11<ActualY, X1, N>(
520 [&](const X1& x1_) { return std::invoke(h, x1_, x2, x3, x4, x5); }, x1,
521 delta);
522}
523
525template <class Y, class X1, class X2, class X3, class X4, class X5,
526 int N = traits<X1>::dimension>
527typename internal::MatrixMN<traits<Y>::dimension, N>::type
528numericalDerivative51(Y (&h)(const X1&, const X2&, const X3&, const X4&,
529 const X5&),
530 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
531 const X5& x5, double delta = 1e-5) {
532 return numericalDerivative11<Y, X1, N>(
533 [&](const X1& x1_) { return h(x1_, x2, x3, x4, x5); }, x1, delta);
534}
535
545template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
546 class X4, class X5, int N = traits<X2>::dimension, class F,
547 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5> = 0>
548typename internal::MatrixMN<
549 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5>>::dimension, N>::type
550numericalDerivative52(F&& h, const X1& x1, const X2& x2, const X3& x3,
551 const X4& x4, const X5& x5, double delta = 1e-5) {
552 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5>;
553 return numericalDerivative11<ActualY, X2, N>(
554 [&](const X2& x2_) { return std::invoke(h, x1, x2_, x3, x4, x5); }, x2,
555 delta);
556}
557
559template <class Y, class X1, class X2, class X3, class X4, class X5,
560 int N = traits<X2>::dimension>
561typename internal::MatrixMN<traits<Y>::dimension, N>::type
562numericalDerivative52(Y (&h)(const X1&, const X2&, const X3&, const X4&,
563 const X5&),
564 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
565 const X5& x5, double delta = 1e-5) {
566 return numericalDerivative11<Y, X2, N>(
567 [&](const X2& x2_) { return h(x1, x2_, x3, x4, x5); }, x2, delta);
568}
569
579template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
580 class X4, class X5, int N = traits<X3>::dimension, class F,
581 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5> = 0>
582typename internal::MatrixMN<
583 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5>>::dimension, N>::type
584numericalDerivative53(F&& h, const X1& x1, const X2& x2, const X3& x3,
585 const X4& x4, const X5& x5, double delta = 1e-5) {
586 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5>;
587 return numericalDerivative11<ActualY, X3, N>(
588 [&](const X3& x3_) { return std::invoke(h, x1, x2, x3_, x4, x5); }, x3,
589 delta);
590}
591
593template <class Y, class X1, class X2, class X3, class X4, class X5,
594 int N = traits<X3>::dimension>
595typename internal::MatrixMN<traits<Y>::dimension, N>::type
596numericalDerivative53(Y (&h)(const X1&, const X2&, const X3&, const X4&,
597 const X5&),
598 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
599 const X5& x5, double delta = 1e-5) {
600 return numericalDerivative11<Y, X3, N>(
601 [&](const X3& x3_) { return h(x1, x2, x3_, x4, x5); }, x3, delta);
602}
603
613template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
614 class X4, class X5, int N = traits<X4>::dimension, class F,
615 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5> = 0>
616typename internal::MatrixMN<
617 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5>>::dimension, N>::type
618numericalDerivative54(F&& h, const X1& x1, const X2& x2, const X3& x3,
619 const X4& x4, const X5& x5, double delta = 1e-5) {
620 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5>;
621 return numericalDerivative11<ActualY, X4, N>(
622 [&](const X4& x4_) { return std::invoke(h, x1, x2, x3, x4_, x5); }, x4,
623 delta);
624}
625
627template <class Y, class X1, class X2, class X3, class X4, class X5,
628 int N = traits<X4>::dimension>
629typename internal::MatrixMN<traits<Y>::dimension, N>::type
630numericalDerivative54(Y (&h)(const X1&, const X2&, const X3&, const X4&,
631 const X5&),
632 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
633 const X5& x5, double delta = 1e-5) {
634 return numericalDerivative11<Y, X4, N>(
635 [&](const X4& x4_) { return h(x1, x2, x3, x4_, x5); }, x4, delta);
636}
637
647template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
648 class X4, class X5, int N = traits<X5>::dimension, class F,
649 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5> = 0>
650typename internal::MatrixMN<
651 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5>>::dimension, N>::type
652numericalDerivative55(F&& h, const X1& x1, const X2& x2, const X3& x3,
653 const X4& x4, const X5& x5, double delta = 1e-5) {
654 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5>;
655 return numericalDerivative11<ActualY, X5, N>(
656 [&](const X5& x5_) { return std::invoke(h, x1, x2, x3, x4, x5_); }, x5,
657 delta);
658}
659
661template <class Y, class X1, class X2, class X3, class X4, class X5,
662 int N = traits<X5>::dimension>
663typename internal::MatrixMN<traits<Y>::dimension, N>::type
664numericalDerivative55(Y (&h)(const X1&, const X2&, const X3&, const X4&,
665 const X5&),
666 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
667 const X5& x5, double delta = 1e-5) {
668 return numericalDerivative11<Y, X5, N>(
669 [&](const X5& x5_) { return h(x1, x2, x3, x4, x5_); }, x5, delta);
670}
671
681template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
682 class X4, class X5, class X6, int N = traits<X1>::dimension, class F,
683 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5, X6> = 0>
684typename internal::MatrixMN<
685 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>>::dimension,
686 N>::type
687numericalDerivative61(F&& h, const X1& x1, const X2& x2, const X3& x3,
688 const X4& x4, const X5& x5, const X6& x6,
689 double delta = 1e-5) {
690 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>;
691 return numericalDerivative11<ActualY, X1, N>(
692 [&](const X1& x1_) { return std::invoke(h, x1_, x2, x3, x4, x5, x6); },
693 x1, delta);
694}
695
697template <class Y, class X1, class X2, class X3, class X4, class X5, class X6,
698 int N = traits<X1>::dimension>
699typename internal::MatrixMN<traits<Y>::dimension, N>::type
700numericalDerivative61(Y (&h)(const X1&, const X2&, const X3&, const X4&,
701 const X5&, const X6&),
702 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
703 const X5& x5, const X6& x6, double delta = 1e-5) {
704 return numericalDerivative11<Y, X1, N>(
705 [&](const X1& x1_) { return h(x1_, x2, x3, x4, x5, x6); }, x1, delta);
706}
707
717template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
718 class X4, class X5, class X6, int N = traits<X2>::dimension, class F,
719 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5, X6> = 0>
720typename internal::MatrixMN<
721 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>>::dimension,
722 N>::type
723numericalDerivative62(F&& h, const X1& x1, const X2& x2, const X3& x3,
724 const X4& x4, const X5& x5, const X6& x6,
725 double delta = 1e-5) {
726 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>;
727 return numericalDerivative11<ActualY, X2, N>(
728 [&](const X2& x2_) { return std::invoke(h, x1, x2_, x3, x4, x5, x6); },
729 x2, delta);
730}
731
733template <class Y, class X1, class X2, class X3, class X4, class X5, class X6,
734 int N = traits<X2>::dimension>
735typename internal::MatrixMN<traits<Y>::dimension, N>::type
736numericalDerivative62(Y (&h)(const X1&, const X2&, const X3&, const X4&,
737 const X5&, const X6&),
738 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
739 const X5& x5, const X6& x6, double delta = 1e-5) {
740 return numericalDerivative11<Y, X2, N>(
741 [&](const X2& x2_) { return h(x1, x2_, x3, x4, x5, x6); }, x2, delta);
742}
743
753template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
754 class X4, class X5, class X6, int N = traits<X3>::dimension, class F,
755 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5, X6> = 0>
756typename internal::MatrixMN<
757 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>>::dimension,
758 N>::type
759numericalDerivative63(F&& h, const X1& x1, const X2& x2, const X3& x3,
760 const X4& x4, const X5& x5, const X6& x6,
761 double delta = 1e-5) {
762 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>;
763 return numericalDerivative11<ActualY, X3, N>(
764 [&](const X3& x3_) { return std::invoke(h, x1, x2, x3_, x4, x5, x6); },
765 x3, delta);
766}
767
769template <class Y, class X1, class X2, class X3, class X4, class X5, class X6,
770 int N = traits<X3>::dimension>
771typename internal::MatrixMN<traits<Y>::dimension, N>::type
772numericalDerivative63(Y (&h)(const X1&, const X2&, const X3&, const X4&,
773 const X5&, const X6&),
774 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
775 const X5& x5, const X6& x6, double delta = 1e-5) {
776 return numericalDerivative11<Y, X3, N>(
777 [&](const X3& x3_) { return h(x1, x2, x3_, x4, x5, x6); }, x3, delta);
778}
779
789template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
790 class X4, class X5, class X6, int N = traits<X4>::dimension, class F,
791 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5, X6> = 0>
792typename internal::MatrixMN<
793 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>>::dimension,
794 N>::type
795numericalDerivative64(F&& h, const X1& x1, const X2& x2, const X3& x3,
796 const X4& x4, const X5& x5, const X6& x6,
797 double delta = 1e-5) {
798 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>;
799 return numericalDerivative11<ActualY, X4, N>(
800 [&](const X4& x4_) { return std::invoke(h, x1, x2, x3, x4_, x5, x6); },
801 x4, delta);
802}
803
805template <class Y, class X1, class X2, class X3, class X4, class X5, class X6,
806 int N = traits<X4>::dimension>
807typename internal::MatrixMN<traits<Y>::dimension, N>::type
808numericalDerivative64(Y (&h)(const X1&, const X2&, const X3&, const X4&,
809 const X5&, const X6&),
810 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
811 const X5& x5, const X6& x6, double delta = 1e-5) {
812 return numericalDerivative11<Y, X4, N>(
813 [&](const X4& x4_) { return h(x1, x2, x3, x4_, x5, x6); }, x4, delta);
814}
815
825template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
826 class X4, class X5, class X6, int N = traits<X5>::dimension, class F,
827 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5, X6> = 0>
828typename internal::MatrixMN<
829 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>>::dimension,
830 N>::type
831numericalDerivative65(F&& h, const X1& x1, const X2& x2, const X3& x3,
832 const X4& x4, const X5& x5, const X6& x6,
833 double delta = 1e-5) {
834 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>;
835 return numericalDerivative11<ActualY, X5, N>(
836 [&](const X5& x5_) { return std::invoke(h, x1, x2, x3, x4, x5_, x6); },
837 x5, delta);
838}
839
841template <class Y, class X1, class X2, class X3, class X4, class X5, class X6,
842 int N = traits<X5>::dimension>
843typename internal::MatrixMN<traits<Y>::dimension, N>::type
844numericalDerivative65(Y (&h)(const X1&, const X2&, const X3&, const X4&,
845 const X5&, const X6&),
846 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
847 const X5& x5, const X6& x6, double delta = 1e-5) {
848 return numericalDerivative11<Y, X5, N>(
849 [&](const X5& x5_) { return h(x1, x2, x3, x4, x5_, x6); }, x5, delta);
850}
851
861template <class Y = internal::DeducedOutput, class X1, class X2, class X3,
862 class X4, class X5, class X6, int N = traits<X6>::dimension, class F,
863 internal::EnableIfInvocable<F, X1, X2, X3, X4, X5, X6> = 0>
864typename internal::MatrixMN<
865 traits<internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>>::dimension,
866 N>::type
867numericalDerivative66(F&& h, const X1& x1, const X2& x2, const X3& x3,
868 const X4& x4, const X5& x5, const X6& x6,
869 double delta = 1e-5) {
870 using ActualY = internal::OutputType<Y, F, X1, X2, X3, X4, X5, X6>;
871 return numericalDerivative11<ActualY, X6, N>(
872 [&](const X6& x6_) { return std::invoke(h, x1, x2, x3, x4, x5, x6_); },
873 x6, delta);
874}
875
877template <class Y, class X1, class X2, class X3, class X4, class X5, class X6,
878 int N = traits<X6>::dimension>
879typename internal::MatrixMN<traits<Y>::dimension, N>::type
880numericalDerivative66(Y (&h)(const X1&, const X2&, const X3&, const X4&,
881 const X5&, const X6&),
882 const X1& x1, const X2& x2, const X3& x3, const X4& x4,
883 const X5& x5, const X6& x6, double delta = 1e-5) {
884 return numericalDerivative11<Y, X6, N>(
885 [&](const X6& x6_) { return h(x1, x2, x3, x4, x5, x6_); }, x6, delta);
886}
887
888
891
899template <class X, int N = traits<X>::dimension, class F,
900 internal::EnableIfScalarInvocable<F, X> = 0>
901inline typename internal::MatrixMN<N, N>::type numericalHessian(
902 F&& f, const X& x, double delta = 1e-5) {
903 typedef Eigen::Matrix<double, N, 1> VectorD;
904 return numericalDerivative11<VectorD, X, N>(
905 [&](const X& x_) { return numericalGradient<X, N>(f, x_, delta); }, x,
906 delta);
907}
908
910template <class X, int N = traits<X>::dimension>
911inline typename internal::MatrixMN<N, N>::type numericalHessian(
912 double (&f)(const X&), const X& x, double delta = 1e-5) {
913 return numericalHessian<X, N>([&](const X& x_) { return f(x_); }, x, delta);
914}
915
917template <class X1, class X2, int N1 = traits<X1>::dimension,
918 int N2 = traits<X2>::dimension, class F,
919 internal::EnableIfScalarInvocable<F, X1, X2> = 0>
920inline typename internal::MatrixMN<N1, N2>::type numericalHessian212(
921 F&& f, const X1& x1, const X2& x2, double delta = 1e-5) {
922 typedef Eigen::Matrix<double, N1, 1> Vector;
923 return numericalDerivative11<Vector, X2, N2>(
924 [&](const X2& x2_) {
925 return numericalGradient<X1, N1>(
926 [&](const X1& x1_) { return std::invoke(f, x1_, x2_); }, x1, delta);
927 },
928 x2, delta);
929}
930
932template <class X1, class X2, int N1 = traits<X1>::dimension,
933 int N2 = traits<X2>::dimension>
934inline typename internal::MatrixMN<N1, N2>::type numericalHessian212(
935 double (&f)(const X1&, const X2&), const X1& x1, const X2& x2,
936 double delta = 1e-5) {
937 return numericalHessian212<X1, X2, N1, N2>(
938 [&](const X1& x1_, const X2& x2_) { return f(x1_, x2_); }, x1, x2, delta);
939}
940
942template <class X1, class X2, int N1 = traits<X1>::dimension, class F,
943 internal::EnableIfScalarInvocable<F, X1, X2> = 0>
944inline typename internal::MatrixMN<N1, N1>::type numericalHessian211(
945 F&& f, const X1& x1, const X2& x2, double delta = 1e-5) {
946 typedef Eigen::Matrix<double, N1, 1> Vector;
947 return numericalDerivative11<Vector, X1, N1>(
948 [&](const X1& x1_) {
949 return numericalGradient<X1, N1>(
950 [&](const X1& x1__) { return std::invoke(f, x1__, x2); }, x1_,
951 delta);
952 },
953 x1, delta);
954}
955
957template <class X1, class X2, int N1 = traits<X1>::dimension>
958inline typename internal::MatrixMN<N1, N1>::type numericalHessian211(
959 double (&f)(const X1&, const X2&), const X1& x1, const X2& x2,
960 double delta = 1e-5) {
961 return numericalHessian211<X1, X2, N1>(
962 [&](const X1& x1_, const X2& x2_) { return f(x1_, x2_); }, x1, x2, delta);
963}
964
966template <class X1, class X2, int N2 = traits<X2>::dimension, class F,
967 internal::EnableIfScalarInvocable<F, X1, X2> = 0>
968inline typename internal::MatrixMN<N2, N2>::type numericalHessian222(
969 F&& f, const X1& x1, const X2& x2, double delta = 1e-5) {
970 typedef Eigen::Matrix<double, N2, 1> Vector;
971 return numericalDerivative11<Vector, X2, N2>(
972 [&](const X2& x2_) {
973 return numericalGradient<X2, N2>(
974 [&](const X2& x2__) { return std::invoke(f, x1, x2__); }, x2_,
975 delta);
976 },
977 x2, delta);
978}
979
981template <class X1, class X2, int N2 = traits<X2>::dimension>
982inline typename internal::MatrixMN<N2, N2>::type numericalHessian222(
983 double (&f)(const X1&, const X2&), const X1& x1, const X2& x2,
984 double delta = 1e-5) {
985 return numericalHessian222<X1, X2, N2>(
986 [&](const X1& x1_, const X2& x2_) { return f(x1_, x2_); }, x1, x2, delta);
987}
988
992/* **************************************************************** */
994template <class X1, class X2, class X3, int N1 = traits<X1>::dimension, class F,
995 internal::EnableIfScalarInvocable<F, X1, X2, X3> = 0>
996inline typename internal::MatrixMN<N1, N1>::type numericalHessian311(
997 F&& f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
998 typedef Eigen::Matrix<double, N1, 1> Vector;
999 return numericalDerivative11<Vector, X1, N1>(
1000 [&](const X1& x1_) {
1001 return numericalGradient<X1, N1>(
1002 [&](const X1& x1__) { return std::invoke(f, x1__, x2, x3); }, x1_,
1003 delta);
1004 },
1005 x1, delta);
1006}
1007
1009template <class X1, class X2, class X3, int N1 = traits<X1>::dimension>
1010inline typename internal::MatrixMN<N1, N1>::type numericalHessian311(
1011 double (&f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2,
1012 const X3& x3, double delta = 1e-5) {
1013 return numericalHessian311<X1, X2, X3, N1>(
1014 [&](const X1& x1_, const X2& x2_, const X3& x3_) {
1015 return f(x1_, x2_, x3_);
1016 },
1017 x1, x2, x3, delta);
1018}
1019
1020/* **************************************************************** */
1022template <class X1, class X2, class X3, int N2 = traits<X2>::dimension, class F,
1023 internal::EnableIfScalarInvocable<F, X1, X2, X3> = 0>
1024inline typename internal::MatrixMN<N2, N2>::type numericalHessian322(
1025 F&& f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1026 typedef Eigen::Matrix<double, N2, 1> Vector;
1027 return numericalDerivative11<Vector, X2, N2>(
1028 [&](const X2& x2_) {
1029 return numericalGradient<X2, N2>(
1030 [&](const X2& x2__) { return std::invoke(f, x1, x2__, x3); }, x2_,
1031 delta);
1032 },
1033 x2, delta);
1034}
1035
1037template <class X1, class X2, class X3, int N2 = traits<X2>::dimension>
1038inline typename internal::MatrixMN<N2, N2>::type numericalHessian322(
1039 double (&f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2,
1040 const X3& x3, double delta = 1e-5) {
1041 return numericalHessian322<X1, X2, X3, N2>(
1042 [&](const X1& x1_, const X2& x2_, const X3& x3_) {
1043 return f(x1_, x2_, x3_);
1044 },
1045 x1, x2, x3, delta);
1046}
1047
1048/* **************************************************************** */
1050template <class X1, class X2, class X3, int N3 = traits<X3>::dimension, class F,
1051 internal::EnableIfScalarInvocable<F, X1, X2, X3> = 0>
1052inline typename internal::MatrixMN<N3, N3>::type numericalHessian333(
1053 F&& f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1054 typedef Eigen::Matrix<double, N3, 1> Vector;
1055 return numericalDerivative11<Vector, X3, N3>(
1056 [&](const X3& x3_) {
1057 return numericalGradient<X3, N3>(
1058 [&](const X3& x3__) { return std::invoke(f, x1, x2, x3__); }, x3_,
1059 delta);
1060 },
1061 x3, delta);
1062}
1063
1065template <class X1, class X2, class X3, int N3 = traits<X3>::dimension>
1066inline typename internal::MatrixMN<N3, N3>::type numericalHessian333(
1067 double (&f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2,
1068 const X3& x3, double delta = 1e-5) {
1069 return numericalHessian333<X1, X2, X3, N3>(
1070 [&](const X1& x1_, const X2& x2_, const X3& x3_) {
1071 return f(x1_, x2_, x3_);
1072 },
1073 x1, x2, x3, delta);
1074}
1075
1076/* **************************************************************** */
1079template <class X1, class X2, class X3, int N1 = traits<X1>::dimension,
1080 int N2 = traits<X2>::dimension, class F,
1081 internal::EnableIfScalarInvocable<F, X1, X2, X3> = 0>
1082inline typename internal::MatrixMN<N1, N2>::type numericalHessian312(
1083 F&& f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1084 return numericalHessian212<X1, X2, N1, N2>(
1085 [&](const X1& x1_, const X2& x2_) {
1086 return std::invoke(f, x1_, x2_, x3);
1087 },
1088 x1, x2, delta);
1089}
1090
1093template <class X1, class X2, class X3, int N1 = traits<X1>::dimension,
1094 int N3 = traits<X3>::dimension, class F,
1095 internal::EnableIfScalarInvocable<F, X1, X2, X3> = 0>
1096inline typename internal::MatrixMN<N1, N3>::type numericalHessian313(
1097 F&& f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1098 return numericalHessian212<X1, X3, N1, N3>(
1099 [&](const X1& x1_, const X3& x3_) {
1100 return std::invoke(f, x1_, x2, x3_);
1101 },
1102 x1, x3, delta);
1103}
1104
1107template <class X1, class X2, class X3, int N2 = traits<X2>::dimension,
1108 int N3 = traits<X3>::dimension, class F,
1109 internal::EnableIfScalarInvocable<F, X1, X2, X3> = 0>
1110inline typename internal::MatrixMN<N2, N3>::type numericalHessian323(
1111 F&& f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
1112 return numericalHessian212<X2, X3, N2, N3>(
1113 [&](const X2& x2_, const X3& x3_) {
1114 return std::invoke(f, x1, x2_, x3_);
1115 },
1116 x2, x3, delta);
1117}
1118
1119/* **************************************************************** */
1121template <class X1, class X2, class X3, int N1 = traits<X1>::dimension,
1122 int N2 = traits<X2>::dimension>
1123inline typename internal::MatrixMN<N1, N2>::type numericalHessian312(
1124 double (&f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2,
1125 const X3& x3, double delta = 1e-5) {
1126 return numericalHessian312<X1, X2, X3, N1, N2>(
1127 [&](const X1& x1_, const X2& x2_, const X3& x3_) {
1128 return f(x1_, x2_, x3_);
1129 },
1130 x1, x2, x3, delta);
1131}
1132
1134template <class X1, class X2, class X3, int N1 = traits<X1>::dimension,
1135 int N3 = traits<X3>::dimension>
1136inline typename internal::MatrixMN<N1, N3>::type numericalHessian313(
1137 double (&f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2,
1138 const X3& x3, double delta = 1e-5) {
1139 return numericalHessian313<X1, X2, X3, N1, N3>(
1140 [&](const X1& x1_, const X2& x2_, const X3& x3_) {
1141 return f(x1_, x2_, x3_);
1142 },
1143 x1, x2, x3, delta);
1144}
1145
1147template <class X1, class X2, class X3, int N2 = traits<X2>::dimension,
1148 int N3 = traits<X3>::dimension>
1149inline typename internal::MatrixMN<N2, N3>::type numericalHessian323(
1150 double (&f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2,
1151 const X3& x3, double delta = 1e-5) {
1152 return numericalHessian323<X1, X2, X3, N2, N3>(
1153 [&](const X1& x1_, const X2& x2_, const X3& x3_) {
1154 return f(x1_, x2_, x3_);
1155 },
1156 x1, x2, x3, delta);
1157}
1158
1161} // namespace gtsam
Lie.h
Base class and basic functions for Lie types.
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::numericalDerivative31
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3 > >::dimension, N >::type numericalDerivative31(F &&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:294
gtsam::numericalHessian312
internal::MatrixMN< N1, N2 >::type numericalHessian312(F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Mixed Hessian with respect to arguments 1 and 2 of a ternary scalar function.
Definition numericalDerivative.h:1082
gtsam::numericalDerivative64
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type numericalDerivative64(F &&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:795
gtsam::numericalDerivative51
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type numericalDerivative51(F &&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:516
gtsam::numericalDerivative52
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type numericalDerivative52(F &&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:550
gtsam::numericalDerivative53
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type numericalDerivative53(F &&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:584
gtsam::numericalHessian323
internal::MatrixMN< N2, N3 >::type numericalHessian323(F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Mixed Hessian with respect to arguments 2 and 3 of a ternary scalar function.
Definition numericalDerivative.h:1110
gtsam::numericalDerivative65
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type numericalDerivative65(F &&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:831
gtsam::numericalDerivative21
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2 > >::dimension, N >::type numericalDerivative21(F &&h, const X1 &x1, const X2 &x2, double delta=1e-5)
Compute numerical derivative in argument 1 of binary function.
Definition numericalDerivative.h:236
gtsam::numericalGradient
Eigen::Matrix< double, N, 1 > numericalGradient(F &&h, const X &x, double delta=1e-5)
Numerically compute gradient of scalar function.
Definition numericalDerivative.h:133
gtsam::numericalDerivative61
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type numericalDerivative61(F &&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:687
gtsam::numericalHessian333
internal::MatrixMN< N3, N3 >::type numericalHessian333(F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Hessian with respect to argument 3 of a ternary scalar function.
Definition numericalDerivative.h:1052
gtsam::numericalHessian222
internal::MatrixMN< N2, N2 >::type numericalHessian222(F &&f, const X1 &x1, const X2 &x2, double delta=1e-5)
Hessian with respect to argument 2 of a binary scalar function.
Definition numericalDerivative.h:968
gtsam::numericalHessian311
internal::MatrixMN< N1, N1 >::type numericalHessian311(F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Numerical Hessian for ternary functions.
Definition numericalDerivative.h:996
gtsam::numericalHessian
internal::MatrixMN< N, N >::type numericalHessian(F &&f, const X &x, double delta=1e-5)
Compute numerical Hessian matrix.
Definition numericalDerivative.h:901
gtsam::numericalDerivative62
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type numericalDerivative62(F &&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:723
gtsam::numericalDerivative42
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4 > >::dimension, N >::type numericalDerivative42(F &&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:417
gtsam::numericalDerivative22
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2 > >::dimension, N >::type numericalDerivative22(F &&h, const X1 &x1, const X2 &x2, double delta=1e-5)
Compute numerical derivative in argument 2 of binary function.
Definition numericalDerivative.h:265
gtsam::numericalDerivative66
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type numericalDerivative66(F &&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:867
gtsam::numericalDerivative55
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type numericalDerivative55(F &&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:652
gtsam::numericalDerivative32
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3 > >::dimension, N >::type numericalDerivative32(F &&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:324
gtsam::numericalDerivative63
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type numericalDerivative63(F &&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:759
gtsam::numericalDerivative54
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type numericalDerivative54(F &&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:618
gtsam::numericalHessian313
internal::MatrixMN< N1, N3 >::type numericalHessian313(F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Mixed Hessian with respect to arguments 1 and 3 of a ternary scalar function.
Definition numericalDerivative.h:1096
gtsam::numericalDerivative43
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4 > >::dimension, N >::type numericalDerivative43(F &&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:450
gtsam::numericalDerivative44
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4 > >::dimension, N >::type numericalDerivative44(F &&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:483
gtsam::numericalDerivative11
internal::MatrixMN< traits< internal::OutputType< Y, F, X > >::dimension, N >::type numericalDerivative11(F &&h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition numericalDerivative.h:180
gtsam::numericalHessian211
internal::MatrixMN< N1, N1 >::type numericalHessian211(F &&f, const X1 &x1, const X2 &x2, double delta=1e-5)
Hessian with respect to argument 1 of a binary scalar function.
Definition numericalDerivative.h:944
gtsam::numericalHessian212
internal::MatrixMN< N1, N2 >::type numericalHessian212(F &&f, const X1 &x1, const X2 &x2, double delta=1e-5)
Mixed Hessian with respect to argument 1 then argument 2.
Definition numericalDerivative.h:920
gtsam::numericalDerivative33
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3 > >::dimension, N >::type numericalDerivative33(F &&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:354
gtsam::numericalHessian322
internal::MatrixMN< N2, N2 >::type numericalHessian322(F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Hessian with respect to argument 2 of a ternary scalar function.
Definition numericalDerivative.h:1024
gtsam::numericalDerivative41
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4 > >::dimension, N >::type numericalDerivative41(F &&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:384
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition Group.h:37
gtsam::manifold_tag
tag to assert a type is a manifold
Definition Manifold.h:33
gtsam::internal::FixedSizeMatrix
Select the fixed-size Jacobian type associated with two manifold types.
Definition numericalDerivative.h:59
gtsam::internal::MatrixMN
Select a fixed-size matrix from explicit row and column counts.
Definition numericalDerivative.h:66
gtsam::internal::DeducedOutput
Marker type used when the callable return type should be deduced.
Definition numericalDerivative.h:71