num_diff.hpp
Go to the documentation of this file.
1 
4 #ifndef SOPHUS_NUM_DIFF_HPP
5 #define SOPHUS_NUM_DIFF_HPP
6 
7 #include <functional>
8 #include <type_traits>
9 #include <utility>
10 
11 #include "types.hpp"
12 
13 namespace Sophus {
14 
15 namespace details {
16 template <class Scalar>
17 class Curve {
18  public:
19  template <class Fn>
20  static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) {
21  using ReturnType = decltype(curve(t));
22  static_assert(std::is_floating_point<Scalar>::value,
23  "Scalar must be a floating point type.");
25  "ReturnType must be either a floating point scalar, "
26  "vector or matrix.");
27 
28  return (curve(t + h) - curve(t - h)) / (Scalar(2) * h);
29  }
30 };
31 
32 template <class Scalar, int N, int M>
33 class VectorField {
34  public:
35  static Eigen::Matrix<Scalar, N, M> num_diff(
37  vector_field,
38  Sophus::Vector<Scalar, M> const& a, Scalar eps) {
39  static_assert(std::is_floating_point<Scalar>::value,
40  "Scalar must be a floating point type.");
41  Eigen::Matrix<Scalar, N, M> J;
43  h.setZero();
44  for (int i = 0; i < M; ++i) {
45  h[i] = eps;
46  J.col(i) =
47  (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps);
48  h[i] = Scalar(0);
49  }
50 
51  return J;
52  }
53 };
54 
55 template <class Scalar, int N>
56 class VectorField<Scalar, N, 1> {
57  public:
58  static Eigen::Matrix<Scalar, N, 1> num_diff(
59  std::function<Sophus::Vector<Scalar, N>(Scalar)> vector_field,
60  Scalar const& a, Scalar eps) {
61  return details::Curve<Scalar>::num_diff(std::move(vector_field), a, eps);
62  }
63 };
64 } // namespace details
65 
71 template <class Scalar, class Fn>
72 auto curveNumDiff(Fn curve, Scalar t,
73  Scalar h = Constants<Scalar>::epsilonSqrt())
74  -> decltype(details::Curve<Scalar>::num_diff(std::move(curve), t, h)) {
75  return details::Curve<Scalar>::num_diff(std::move(curve), t, h);
76 }
77 
83 template <class Scalar, int N, int M, class ScalarOrVector, class Fn>
84 Eigen::Matrix<Scalar, N, M> vectorFieldNumDiff(
85  Fn vector_field, ScalarOrVector const& a,
86  Scalar eps = Constants<Scalar>::epsilonSqrt()) {
87  return details::VectorField<Scalar, N, M>::num_diff(std::move(vector_field),
88  a, eps);
89 }
90 
91 } // namespace Sophus
92 
93 #endif // SOPHUS_NUM_DIFF_HPP
Sophus::vectorFieldNumDiff
Eigen::Matrix< Scalar, N, M > vectorFieldNumDiff(Fn vector_field, ScalarOrVector const &a, Scalar eps=Constants< Scalar >::epsilonSqrt())
Definition: num_diff.hpp:84
Sophus::details::VectorField
Definition: num_diff.hpp:33
Sophus::details::VectorField::num_diff
static Eigen::Matrix< Scalar, N, M > num_diff(std::function< Sophus::Vector< Scalar, N >(Sophus::Vector< Scalar, M >)> vector_field, Sophus::Vector< Scalar, M > const &a, Scalar eps)
Definition: num_diff.hpp:35
types.hpp
Sophus::details::Curve::num_diff
static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t))
Definition: num_diff.hpp:20
Sophus::IsFloatingPoint
Definition: types.hpp:200
Sophus::curveNumDiff
auto curveNumDiff(Fn curve, Scalar t, Scalar h=Constants< Scalar >::epsilonSqrt()) -> decltype(details::Curve< Scalar >::num_diff(std::move(curve), t, h))
Definition: num_diff.hpp:72
Sophus::details::VectorField< Scalar, N, 1 >::num_diff
static Eigen::Matrix< Scalar, N, 1 > num_diff(std::function< Sophus::Vector< Scalar, N >(Scalar)> vector_field, Scalar const &a, Scalar eps)
Definition: num_diff.hpp:58
Sophus
Definition: average.hpp:17
Sophus::details::Curve
Definition: num_diff.hpp:17
Sophus::Constants
Definition: common.hpp:146
Sophus::Vector
Eigen::Matrix< Scalar, M, 1, Options > Vector
Definition: types.hpp:13


sophus
Author(s): Hauke Strasdat
autogenerated on Wed Mar 2 2022 01:01:47