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