55 template<
class Y,
class X=
double>
69 template <class X, int N = traits<X>::dimension>
71 std::function<
double(
const X&)>
h,
const X&
x,
double delta = 1
e-5) {
72 double factor = 1.0 / (2.0 *
delta);
76 "Template argument X must be a manifold type.");
77 static_assert(
N>0,
"Template argument X must be fixed-size type or N must be specified.");
85 for (
int j = 0;
j <
N;
j++) {
91 g(
j) = (hxplus - hxmin) * factor;
108 template <class Y, class X, int N = traits<X>::dimension>
111 std::function<
Y(
const X&)>
h,
const X&
x,
double delta = 1
e-5) {
115 "Template argument Y must be a manifold type.");
119 "Template argument X must be a manifold type.");
120 static_assert(
N>0,
"Template argument X must be fixed-size type or N must be specified.");
127 const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx);
128 const size_t m = zeroY.size();
136 const double factor = 1.0 / (2.0 *
delta);
137 for (
int j = 0;
j <
N;
j++) {
139 const auto dy1 = TraitsY::Local(hx,
h(TraitsX::Retract(
x, dx)));
141 const auto dy2 = TraitsY::Local(hx,
h(TraitsX::Retract(
x, dx)));
143 H.col(
j) << (dy1 - dy2) * factor;
149 template<
class Y,
class X>
152 return numericalDerivative11<Y, X>(std::bind(
h, std::placeholders::_1),
x,
165 template<class Y, class X1, class X2, int N = traits<X1>::dimension>
169 "Template argument Y must be a manifold type.");
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);
177 template<
class Y,
class X1,
class X2>
180 return numericalDerivative21<Y, X1, X2>(
181 std::bind(
h, std::placeholders::_1, std::placeholders::_2),
x1,
x2,
194 template<class Y, class X1, class X2, int N = traits<X2>::dimension>
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);
206 template<
class Y,
class X1,
class X2>
209 return numericalDerivative22<Y, X1, X2>(
210 std::bind(
h, std::placeholders::_1, std::placeholders::_2),
x1,
x2,
225 template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
227 std::function<
Y(
const X1&,
const X2&,
const X3&)>
h,
const X1&
x1,
230 "Template argument Y must be a manifold type.");
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)),
238 template<
class Y,
class X1,
class X2,
class X3>
241 return numericalDerivative31<Y, X1, X2, X3>(
242 std::bind(
h, std::placeholders::_1, std::placeholders::_2,
243 std::placeholders::_3),
258 template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
260 std::function<
Y(
const X1&,
const X2&,
const X3&)>
h,
const X1&
x1,
263 "Template argument Y must be a manifold type.");
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)),
271 template<
class Y,
class X1,
class X2,
class X3>
274 return numericalDerivative32<Y, X1, X2, X3>(
275 std::bind(
h, std::placeholders::_1, std::placeholders::_2,
276 std::placeholders::_3),
291 template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
293 std::function<
Y(
const X1&,
const X2&,
const X3&)>
h,
const X1&
x1,
296 "Template argument Y must be a manifold type.");
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),
304 template<
class Y,
class X1,
class X2,
class X3>
307 return numericalDerivative33<Y, X1, X2, X3>(
308 std::bind(
h, std::placeholders::_1, std::placeholders::_2,
309 std::placeholders::_3),
324 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
326 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&)>
h,
const X1&
x1,
329 "Template argument Y must be a manifold type.");
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),
338 template<
class Y,
class X1,
class X2,
class X3,
class X4>
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),
358 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
360 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&)>
h,
const X1&
x1,
363 "Template argument Y must be a manifold type.");
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),
372 template<
class Y,
class X1,
class X2,
class X3,
class X4>
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),
392 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
394 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&)>
h,
const X1&
x1,
397 "Template argument Y must be a manifold type.");
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,
406 template<
class Y,
class X1,
class X2,
class X3,
class X4>
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),
426 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
428 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&)>
h,
const X1&
x1,
431 "Template argument Y must be a manifold type.");
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),
440 template<
class Y,
class X1,
class X2,
class X3,
class X4>
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),
461 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
463 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&)>
h,
const X1&
x1,
466 "Template argument Y must be a manifold type.");
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)),
475 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5>
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),
497 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
499 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&)>
h,
const X1&
x1,
502 "Template argument Y must be a manifold type.");
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)),
511 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5>
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),
533 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
535 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&)>
h,
const X1&
x1,
538 "Template argument Y must be a manifold type.");
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)),
547 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5>
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),
569 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
571 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&)>
h,
const X1&
x1,
574 "Template argument Y must be a manifold type.");
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)),
583 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5>
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),
605 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
607 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&)>
h,
const X1&
x1,
610 "Template argument Y must be a manifold type.");
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),
619 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5>
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),
642 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
644 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&,
const X6&)>
h,
const X1&
x1,
647 "Template argument Y must be a manifold type.");
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)),
656 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5,
class X6>
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),
679 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
681 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&,
const X6&)>
h,
const X1&
x1,
684 "Template argument Y must be a manifold type.");
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)),
693 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5,
class X6>
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),
716 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
718 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&,
const X6&)>
h,
const X1&
x1,
721 "Template argument Y must be a manifold type.");
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)),
730 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5,
class X6>
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),
753 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
755 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&,
const X6&)>
h,
const X1&
x1,
758 "Template argument Y must be a manifold type.");
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)),
767 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5,
class X6>
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),
790 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
792 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&,
const X6&)>
h,
const X1&
x1,
795 "Template argument Y must be a manifold type.");
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)),
804 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5,
class X6>
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),
827 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
829 std::function<
Y(
const X1&,
const X2&,
const X3&,
const X4&,
const X5&,
const X6&)>
h,
833 "Template argument Y must be a manifold type.");
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),
842 template<
class Y,
class X1,
class X2,
class X3,
class X4,
class X5,
class X6>
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),
864 "Template argument X must be a manifold type.");
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);
882 template<
class X1,
class X2>
884 const std::function<double(
const X1&,
const X2&)>&
f_;
895 return numericalGradient<X1>(
896 std::bind(
f_, std::placeholders::_1, std::cref(
x2)),
x1_,
delta_);
900 template<
class X1,
class X2>
902 std::function<
double(
const X1&,
const X2&)>
f,
const X1&
x1,
const X2&
x2,
906 return numericalDerivative11<Vector, X2>(
908 std::bind<Vector>(
std::ref(g_x1), std::placeholders::_1)),
912 template<
class X1,
class X2>
919 template<
class X1,
class X2>
921 std::function<
double(
const X1&,
const X2&)>
f,
const X1&
x1,
const X2&
x2,
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)));
931 return numericalDerivative11<Vector, X1>(
933 std::bind(numGrad,
f2, std::placeholders::_1,
delta)),
937 template<
class X1,
class X2>
944 template<
class X1,
class X2>
946 std::function<
double(
const X1&,
const X2&)>
f,
const X1&
x1,
const X2&
x2,
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));
954 return numericalDerivative11<Vector, X2>(
956 std::bind(numGrad,
f2, std::placeholders::_1,
delta)),
960 template<
class X1,
class X2>
971 template<
class X1,
class X2,
class X3>
973 std::function<
double(
const X1&,
const X2&,
const X3&)>
f,
const X1&
x1,
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)));
981 return numericalDerivative11<Vector, X1>(
983 std::bind(numGrad,
f2, std::placeholders::_1,
delta)),
987 template<
class X1,
class X2,
class X3>
991 std::function<
double(
const X1&,
const X2&,
const X3&)>(
f),
x1,
x2,
x3,
996 template<
class X1,
class X2,
class X3>
998 std::function<
double(
const X1&,
const X2&,
const X3&)>
f,
const X1&
x1,
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)));
1006 return numericalDerivative11<Vector, X2>(
1008 std::bind(numGrad,
f2, std::placeholders::_1,
delta)),
1012 template<
class X1,
class X2,
class X3>
1016 std::function<
double(
const X1&,
const X2&,
const X3&)>(
f),
x1,
x2,
x3,
1021 template<
class X1,
class X2,
class X3>
1023 std::function<
double(
const X1&,
const X2&,
const X3&)>
f,
const X1&
x1,
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));
1031 return numericalDerivative11<Vector, X3>(
1033 std::bind(numGrad,
f2, std::placeholders::_1,
delta)),
1037 template<
class X1,
class X2,
class X3>
1041 std::function<
double(
const X1&,
const X2&,
const X3&)>(
f),
x1,
x2,
x3,
1046 template<
class X1,
class X2,
class X3>
1048 std::function<
double(
const X1&,
const X2&,
const X3&)>
f,
const X1&
x1,
1050 return numericalHessian212<X1, X2>(
1051 std::function<
double(
const X1&,
const X2&)>(
1052 std::bind(
f, std::placeholders::_1, std::placeholders::_2,
1057 template<
class X1,
class X2,
class X3>
1059 std::function<
double(
const X1&,
const X2&,
const X3&)>
f,
const X1&
x1,
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)),
1068 template<
class X1,
class X2,
class X3>
1070 std::function<
double(
const X1&,
const X2&,
const X3&)>
f,
const X1&
x1,
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)),
1080 template<
class X1,
class X2,
class X3>
1084 std::function<
double(
const X1&,
const X2&,
const X3&)>(
f),
x1,
x2,
x3,
1088 template<
class X1,
class X2,
class X3>
1092 std::function<
double(
const X1&,
const X2&,
const X3&)>(
f),
x1,
x2,
x3,
1096 template<
class X1,
class X2,
class X3>
1100 std::function<
double(
const X1&,
const X2&,
const X3&)>(
f),
x1,
x2,
x3,