Exotica
finitediff_chain_jacobian.h
Go to the documentation of this file.
1 // Copyright (C) 2018
2 //
3 // This Source Code Form is subject to the terms of the Mozilla
4 // Public License v. 2.0. If a copy of the MPL was not distributed
5 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
6 //
7 // Modified from unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
8 // and unsupported/Eigen/src/NumericalDiff/NumericalDiff.h
9 
10 #ifndef EIGEN_FINITEDIFF_CHAIN_JACOBIAN_H_
11 #define EIGEN_FINITEDIFF_CHAIN_JACOBIAN_H_
12 
13 #include <functional>
14 
17 
18 namespace Eigen
19 {
20 template <typename Functor, NumericalDiffMode mode = Forward>
22 {
23 public:
24  typedef typename Functor::InputType InputType;
25  typedef typename Functor::ValueType ValueType;
26  typedef typename ValueType::Scalar Scalar;
27 
28  enum
29  {
30  InputsAtCompileTime = InputType::RowsAtCompileTime,
31  ValuesAtCompileTime = ValueType::RowsAtCompileTime,
32  JacobianInputsAtCompileTime = Functor::JacobianColsAtCompileTime // JacobianInputsAtCompileTime no longer have to match InputsAtCompileTime
33  };
34 
35  typedef Matrix<Scalar, ValuesAtCompileTime, JacobianInputsAtCompileTime> JacobianType;
36  typedef Matrix<Scalar, JacobianInputsAtCompileTime, 1> InputJacobianRowType;
37  typedef typename JacobianType::Index Index;
38 
39  typedef std::function<void(const InputJacobianRowType &, InputType &)> UpdateFunctionCallbackType;
40 
43 
44  FiniteDiffChainJacobian(Scalar epsfcn = 0.) : Functor(), epsfcn_(epsfcn) {}
45  FiniteDiffChainJacobian(const Functor &f, Scalar epsfcn = 0.) : Functor(f), epsfcn_(epsfcn) {}
46  FiniteDiffChainJacobian(const Functor &f, UpdateFunctionCallbackType update, Scalar epsfcn = 0.) : Functor(f), update_(update), epsfcn_(epsfcn) {}
47 // forward constructors
48 #if EIGEN_HAS_VARIADIC_TEMPLATES
49  template <typename... T>
50  FiniteDiffChainJacobian(Scalar epsfcn = 0., const T &... Values) : Functor(Values...), epsfcn_(epsfcn)
51  {
52  }
53  template <typename... T>
54  FiniteDiffChainJacobian(UpdateFunctionCallbackType update, Scalar epsfcn = 0., const T &... Values) : Functor(Values...), update_(update), epsfcn_(epsfcn)
55  {
56  }
57 #else
58  template <typename T0>
59  FiniteDiffChainJacobian(const T0 &a0, Scalar epsfcn = 0.) : Functor(a0), epsfcn_(epsfcn)
60  {
61  }
62  template <typename T0, typename T1>
63  FiniteDiffChainJacobian(const T0 &a0, const T1 &a1, Scalar epsfcn = 0.) : Functor(a0, a1), epsfcn_(epsfcn)
64  {
65  }
66  template <typename T0, typename T1, typename T2>
67  FiniteDiffChainJacobian(const T0 &a0, const T1 &a1, const T2 &a2, Scalar epsfcn = 0.) : Functor(a0, a1, a2), epsfcn_(epsfcn)
68  {
69  }
70 
71  template <typename T0>
72  FiniteDiffChainJacobian(UpdateFunctionCallbackType update, Scalar epsfcn = 0., const T0 &a0) : Functor(a0), update_(update), epsfcn_(epsfcn)
73  {
74  }
75  template <typename T0, typename T1>
76  FiniteDiffChainJacobian(UpdateFunctionCallbackType update, Scalar epsfcn = 0., const T0 &a0, const T1 &a1) : Functor(a0, a1), update_(update), epsfcn_(epsfcn)
77  {
78  }
79  template <typename T0, typename T1, typename T2>
80  FiniteDiffChainJacobian(UpdateFunctionCallbackType update, Scalar epsfcn = 0., const T0 &a0, const T1 &a1, const T2 &a2) : Functor(a0, a1, a2), update_(update), epsfcn_(epsfcn)
81  {
82  }
83 #endif
84 
85 #if EIGEN_HAS_VARIADIC_TEMPLATES
86  // Some compilers don't accept variadic parameters after a default parameter,
87  // i.e., we can't just write _jac=0 but we need to overload operator():
88  EIGEN_STRONG_INLINE
89  int operator()(const InputJacobianRowType &_jx, ValueType &v) const
90  {
91  InputType x;
92  update_(_jx, x);
93  this->operator()(x, v);
94  return 1;
95  }
96 
97  template <typename... ParamsType>
98  int operator()(const InputJacobianRowType &_jx, ValueType &v, const ParamsType &... Params) const
99  {
100  InputType x;
101  update_(_jx, x);
102  this->operator()(x, v, Params...);
103  return 1;
104  }
105 
106  template <typename... ParamsType>
107  int operator()(const InputJacobianRowType &_jx, ValueType &v, JacobianType &jac, const ParamsType &... Params) const
108 #else
109  EIGEN_STRONG_INLINE
110  int operator()(const InputJacobianRowType &_jx, ValueType &v) const
111  {
112  InputType x;
113  update_(_jx, x);
114  this->operator()(x, v);
115  return 1;
116  }
117 
118  int operator()(const InputJacobianRowType &_jx, ValueType &v, JacobianType &jac) const
119 #endif
120  {
121  using std::abs;
122  using std::sqrt;
123  // Local variables
124  Scalar h;
125  int nfev = 0;
126  const typename InputJacobianRowType::Index n = _jx.size();
127  const Scalar eps = sqrt(((std::max)(epsfcn_, NumTraits<Scalar>::epsilon())));
128  ValueType val1, val2;
129  InputJacobianRowType jx = _jx;
130  InputType x;
131  if (ValuesAtCompileTime == Dynamic)
132  {
133  val1.resize(v.rows());
134  val2.resize(v.rows());
135  }
136 
137 #if EIGEN_HAS_VARIADIC_TEMPLATES
138  update_(jx, x);
139  Functor::operator()(x, v, Params...);
140  ++nfev;
141 #else
142  update_(jx, x);
143  Functor::operator()(x, v);
144  ++nfev;
145 #endif
146 
147  switch (mode)
148  {
149  case Forward:
150  // copy f(x)
151  val1 = v;
152  break;
153  case Central:
154  // do nothing
155  break;
156  default:
157  eigen_assert(false);
158  };
159 
160  // Function Body
161  for (int j = 0; j < n; ++j)
162  {
163  h = eps * abs(jx[j]);
164  if (h == 0.)
165  {
166  h = eps;
167  }
168  switch (mode)
169  {
170  case Forward:
171  jx[j] += h;
172 #if EIGEN_HAS_VARIADIC_TEMPLATES
173  update_(jx, x);
174  Functor::operator()(x, val2, Params...);
175  ++nfev;
176 #else
177  update_(jx, x);
178  Functor::operator()(x, val2);
179  ++nfev;
180 #endif
181  jx[j] = _jx[j];
182  jac.col(j) = (val2 - val1) / h;
183  break;
184  case Central:
185  jx[j] += h;
186 #if EIGEN_HAS_VARIADIC_TEMPLATES
187  update_(jx, x);
188  Functor::operator()(x, val2, Params...);
189  ++nfev;
190 #else
191  update_(jx, x);
192  Functor::operator()(x, val2);
193  ++nfev;
194 #endif
195  jx[j] -= 2 * h;
196 #if EIGEN_HAS_VARIADIC_TEMPLATES
197  update_(jx, x);
198  Functor::operator()(x, val1, Params...);
199  ++nfev;
200 #else
201  update_(jx, x);
202  Functor::operator()(x, val1);
203  ++nfev;
204 #endif
205  jx[j] = _jx[j];
206  jac.col(j) = (val2 - val1) / (2 * h);
207  break;
208  default:
209  eigen_assert(false);
210  };
211  }
212  return nfev;
213  }
214 };
215 } // namespace Eigen
216 
217 #endif // EIGEN_FINITEDIFF_CHAIN_JACOBIAN_H_
Eigen::Forward
@ Forward
Definition: finitediff_common.h:37
Eigen::Central
@ Central
Definition: finitediff_common.h:38
Eigen
Definition: autodiff_chain_hessian.h:16
Eigen::FiniteDiffChainJacobian::UpdateFunctionCallbackType
std::function< void(const InputJacobianRowType &, InputType &)> UpdateFunctionCallbackType
Definition: finitediff_chain_jacobian.h:39
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(const Functor &f, Scalar epsfcn=0.)
Definition: finitediff_chain_jacobian.h:45
Eigen::FiniteDiffChainJacobian::Index
JacobianType::Index Index
Definition: finitediff_chain_jacobian.h:37
exotica::Functor
FunctorBase< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic > Functor
Definition: functor.h:49
Eigen::FiniteDiffChainJacobian::Scalar
ValueType::Scalar Scalar
Definition: finitediff_chain_jacobian.h:26
Eigen::FiniteDiffChainJacobian::JacobianType
Matrix< Scalar, ValuesAtCompileTime, JacobianInputsAtCompileTime > JacobianType
Definition: finitediff_chain_jacobian.h:35
Eigen::FiniteDiffChainJacobian::ValueType
Functor::ValueType ValueType
Definition: finitediff_chain_jacobian.h:25
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(const T0 &a0, Scalar epsfcn=0.)
Definition: finitediff_chain_jacobian.h:59
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(UpdateFunctionCallbackType update, Scalar epsfcn=0., const T0 &a0, const T1 &a1, const T2 &a2)
Definition: finitediff_chain_jacobian.h:80
Eigen::FiniteDiffChainJacobian::InputType
Functor::InputType InputType
Definition: finitediff_chain_jacobian.h:24
Eigen::FiniteDiffChainJacobian::operator()
int operator()(const InputJacobianRowType &_jx, ValueType &v, JacobianType &jac) const
Definition: finitediff_chain_jacobian.h:118
Eigen::FiniteDiffChainJacobian::InputsAtCompileTime
@ InputsAtCompileTime
Definition: finitediff_chain_jacobian.h:30
Eigen::FiniteDiffChainJacobian::InputJacobianRowType
Matrix< Scalar, JacobianInputsAtCompileTime, 1 > InputJacobianRowType
Definition: finitediff_chain_jacobian.h:36
Eigen::FiniteDiffChainJacobian::epsfcn_
Scalar epsfcn_
Definition: finitediff_chain_jacobian.h:42
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(const T0 &a0, const T1 &a1, const T2 &a2, Scalar epsfcn=0.)
Definition: finitediff_chain_jacobian.h:67
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(const Functor &f, UpdateFunctionCallbackType update, Scalar epsfcn=0.)
Definition: finitediff_chain_jacobian.h:46
Eigen::FiniteDiffChainJacobian::operator()
EIGEN_STRONG_INLINE int operator()(const InputJacobianRowType &_jx, ValueType &v) const
Definition: finitediff_chain_jacobian.h:110
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(UpdateFunctionCallbackType update, Scalar epsfcn=0., const T0 &a0, const T1 &a1)
Definition: finitediff_chain_jacobian.h:76
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(UpdateFunctionCallbackType update, Scalar epsfcn=0., const T0 &a0)
Definition: finitediff_chain_jacobian.h:72
Eigen::FiniteDiffChainJacobian
Definition: finitediff_chain_jacobian.h:21
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(const T0 &a0, const T1 &a1, Scalar epsfcn=0.)
Definition: finitediff_chain_jacobian.h:63
Eigen::FiniteDiffChainJacobian::update_
UpdateFunctionCallbackType update_
Definition: finitediff_chain_jacobian.h:41
Eigen::FiniteDiffChainJacobian::ValuesAtCompileTime
@ ValuesAtCompileTime
Definition: finitediff_chain_jacobian.h:31
finitediff_common.h
Eigen::FiniteDiffChainJacobian::JacobianInputsAtCompileTime
@ JacobianInputsAtCompileTime
Definition: finitediff_chain_jacobian.h:32
functor.h
Eigen::FiniteDiffChainJacobian::FiniteDiffChainJacobian
FiniteDiffChainJacobian(Scalar epsfcn=0.)
Definition: finitediff_chain_jacobian.h:44