@@ -46,41 +46,30 @@ namespace math {
4646 * @param[out] fx function applied to argument
4747 * @param[out] grad_fx gradient of function at argument
4848 */
49- template <typename F, typename VectorT,
49+ template <typename F, typename VectorT, typename GradVectorT,
5050 typename ScalarT = return_type_t <VectorT>>
51- void finite_diff_gradient_auto (const F& f, const VectorT& x, ScalarT& fx,
52- VectorT& grad_fx) {
53- VectorT x_temp (x);
51+ void finite_diff_gradient_auto (const F& f, VectorT&& x, ScalarT& fx,
52+ GradVectorT& grad_fx) {
53+ using EigT = Eigen::Matrix<ScalarT, -1 , 1 >;
54+ static constexpr int h_scale[6 ] = {3 , 2 , 1 , -3 , -2 , -1 };
55+ static constexpr int mults[6 ] = {1 , -9 , 45 , -1 , 9 , -45 };
56+
5457 fx = f (x);
5558 grad_fx.resize (x.size ());
56- for (int i = 0 ; i < x.size (); ++i) {
57- double h = finite_diff_stepsize (value_of_rec (x[i]));
59+ Eigen::Map<EigT> grad_map (grad_fx.data (), grad_fx.size ());
5860
61+ grad_map = EigT::NullaryExpr (x.size (), [&f, &x](Eigen::Index i) {
62+ double h = finite_diff_stepsize (value_of_rec (x[i]));
5963 ScalarT delta_f = 0 ;
60-
61- x_temp[i] = x[i] + 3 * h;
62- delta_f += f (x_temp);
63-
64- x_temp[i] = x[i] + 2 * h;
65- delta_f -= 9 * f (x_temp);
66-
67- x_temp[i] = x[i] + h;
68- delta_f += 45 * f (x_temp);
69-
70- x_temp[i] = x[i] + -3 * h;
71- delta_f -= f (x_temp);
72-
73- x_temp[i] = x[i] + -2 * h;
74- delta_f += 9 * f (x_temp);
75-
76- x_temp[i] = x[i] - h;
77- delta_f -= 45 * f (x_temp);
78-
79- delta_f /= 60 * h;
80-
81- x_temp[i] = x[i];
82- grad_fx[i] = delta_f;
83- }
64+ for (int j = 0 ; j < 6 ; ++j) {
65+ auto x_temp
66+ = EigT::NullaryExpr (x.size (), [&x, &i, &h, &j](Eigen::Index k) {
67+ return k == i ? x[i] + h * h_scale[j] : x[k];
68+ });
69+ delta_f += f (std::move (x_temp)) * mults[j];
70+ }
71+ return delta_f / (60 * h);
72+ });
8473}
8574
8675} // namespace math
0 commit comments