finite_differences.hpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2020,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  *
22  * Authors: Christoph Rösmann
23  *********************************************************************/
24 
25 #include <corbo-numerics/finite_differences.h>
26 
27 namespace corbo {
28 
29 template <typename IncFun, typename EvalFun>
30 void ForwardDifferences::jacobian(IncFun inc_fun, EvalFun eval_fun, Eigen::Ref<Eigen::MatrixXd> jacobian)
31 {
32  constexpr const double delta = 1e-9; // discretization width
33  constexpr const double scalar = 1.0 / delta; // auxillary variable to avoid some repeated flops.
34 
35  int dim_x = jacobian.cols();
36  int dim_val = jacobian.rows();
37 
38  Eigen::VectorXd f0(dim_val);
39  Eigen::VectorXd f1(dim_val);
40 
41  eval_fun(f0); // TODO(roesmann) should we support caching?
42  for (int i = 0; i < dim_x; ++i)
43  {
44  inc_fun(i, delta);
45  eval_fun(f1);
46  inc_fun(i, -delta);
47  jacobian.col(i).noalias() = scalar * (f1 - f0);
48  }
49 }
50 
51 template <typename IncFun, typename EvalFun>
52 void ForwardDifferences::hessian(IncFun inc_fun, EvalFun eval_fun, int dim_f, Eigen::Ref<Eigen::MatrixXd> hessian, const double* multipliers)
53 {
54  constexpr const double delta = 1e-5; // discretization width
55  constexpr const double scalar = 1 / (delta * delta); // auxillary variable to avoid some repeated flops.
56 
57  assert(hessian.rows() == hessian.cols());
58 
59  int dim_x = hessian.cols();
60 
61  // http://www.okstate.edu/sas/v8/sashtml/ormp/chap5/sect28.htm
62  // http://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf <- higher order approximations
63 
64  Eigen::VectorXd f0(dim_f); // f(x, y)
65  Eigen::VectorXd f1(dim_f); // f(x+h, y)
66  Eigen::VectorXd f2(dim_f); // f(x, y+h)
67  Eigen::VectorXd f3(dim_f); // f(x+h, y+h)
68 
69  // df(x,y)/(dxdy) = 1/(h^2) ( f3 - f1 - f2 + f0 )
70 
71  // TODO(roesmann) symmetry?
72  for (int i = 0; i < dim_x; ++i)
73  {
74  for (int j = 0; j < dim_x; ++j)
75  {
76  inc_fun(i, delta);
77  eval_fun(f1);
78  inc_fun(j, delta);
79  eval_fun(f3);
80  inc_fun(i, -delta);
81  eval_fun(f2);
82  inc_fun(j, -delta);
83  eval_fun(f0);
84 
85  if (multipliers)
86  {
87  hessian(i, j) = scalar * (f3[0] - f1[0] - f2[0] + f0[0]) * multipliers[0];
88  for (int v = 1; v < dim_f; ++v)
89  {
90  hessian(i, j) += scalar * (f3[v] - f1[v] - f2[v] + f0[v]) * multipliers[v];
91  }
92  }
93  else
94  {
95  hessian(i, j) = scalar * (f3[0] - f1[0] - f2[0] + f0[0]);
96  for (int v = 1; v < dim_f; ++v)
97  {
98  hessian(i, j) += scalar * (f3[v] - f1[v] - f2[v] + f0[v]);
99  }
100  }
101  }
102  }
103 }
104 
105 template <typename IncFun, typename EvalFun>
106 void ForwardDifferences::jacobianHessian(IncFun inc_fun, EvalFun eval_fun, Eigen::Ref<Eigen::MatrixXd> jacobian, Eigen::Ref<Eigen::MatrixXd> hessian,
107  const double* multipliers)
108 {
109  constexpr const double delta = 1e-5; // discretization width
110  constexpr const double scalar_x = 1 / (delta); // auxillary variable to avoid some repeated flops.
111  constexpr const double scalar_xy = 1 / (delta * delta); // auxillary variable to avoid some repeated flops.
112 
113  assert(hessian.rows() == hessian.cols());
114  assert(jacobian.cols() == hessian.cols());
115 
116  int dim_x = hessian.cols();
117  int dim_f = jacobian.rows();
118 
119  Eigen::Map<const Eigen::VectorXd> multipliers_vec(multipliers, dim_f);
120 
121  // http://www.okstate.edu/sas/v8/sashtml/ormp/chap5/sect28.htm
122 
123  Eigen::VectorXd f0(dim_f); // f(x, y)
124  Eigen::VectorXd f1(dim_f); // f(x+h, y)
125  Eigen::VectorXd f2(dim_f); // f(x, y+h)
126  Eigen::VectorXd f3(dim_f); // f(x+h, y+h)
127 
128  // df(x,y)/(dxdy) = 1/(h^2) ( f3 - f1 - f2 + f0 )
129 
130  // TODO(roesmann) symmetry?
131  for (int i = 0; i < dim_x; ++i)
132  {
133  for (int j = 0; j < dim_x; ++j)
134  {
135  inc_fun(i, delta);
136  eval_fun(f1);
137  inc_fun(j, delta);
138  eval_fun(f3);
139  inc_fun(i, -delta);
140  eval_fun(f2);
141  inc_fun(j, -delta);
142  eval_fun(f0);
143 
144  if (multipliers)
145  {
146  hessian(i, j) = scalar_xy * (f3[0] - f1[0] - f2[0] + f0[0]) * multipliers[0];
147  for (int v = 1; v < dim_f; ++v)
148  {
149  hessian(i, j) += scalar_xy * (f3[v] - f1[v] - f2[v] + f0[v]) * multipliers[v];
150  }
151  if (i == j) jacobian.col(i).noalias() = scalar_x * (f1 - f0).cwiseProduct(multipliers_vec);
152  }
153  else
154  {
155  hessian(i, j) = scalar_xy * (f3[0] - f1[0] - f2[0] + f0[0]);
156  for (int v = 1; v < dim_f; ++v)
157  {
158  hessian(i, j) += scalar_xy * (f3[v] - f1[v] - f2[v] + f0[v]);
159  }
160  if (i == j) jacobian.col(i).noalias() = scalar_x * (f1 - f0);
161  }
162  }
163  }
164 }
165 
166 template <typename IncFun, typename EvalFun>
167 void CentralDifferences::jacobian(IncFun inc_fun, EvalFun eval_fun, Eigen::Ref<Eigen::MatrixXd> jacobian)
168 {
169  constexpr const double delta = 1e-9; // discretization width
170  constexpr const double ddelta = 2 * delta; // two times the discretization width
171  constexpr const double scalar = 1.0 / ddelta; // auxillary variable to avoid some repeated flops.
172 
173  int dim_x = jacobian.cols();
174  int dim_val = jacobian.rows();
175 
176  Eigen::VectorXd f0(dim_val);
177  Eigen::VectorXd f1(dim_val);
178 
179  for (int i = 0; i < dim_x; ++i)
180  {
181  inc_fun(i, delta);
182  eval_fun(f1);
183  inc_fun(i, -ddelta);
184  eval_fun(f0);
185  jacobian.col(i).noalias() = scalar * (f1 - f0);
186  inc_fun(i, delta);
187  }
188 }
189 
190 template <typename IncFun, typename EvalFun>
191 void CentralDifferences::hessian(IncFun inc_fun, EvalFun eval_fun, int dim_f, Eigen::Ref<Eigen::MatrixXd> hessian, const double* multipliers)
192 {
193  constexpr const double delta = 1e-5; // discretization width
194  constexpr const double ddelta = 2 * delta; // two times the discretization width
195  constexpr const double scalar_xx = 1 / (delta * delta); // auxillary variable to avoid some repeated flops.
196  constexpr const double scalar_xy = 1 / (4.0 * delta * delta); // auxillary variable to avoid some repeated flops.
197 
198  assert(hessian.rows() == hessian.cols());
199 
200  int dim_x = hessian.cols();
201 
202  Eigen::VectorXd f1(dim_f); // f(x+h, y+h)
203  Eigen::VectorXd f2(dim_f); // f(x+h, y-h)
204  Eigen::VectorXd f3(dim_f); // f(x-h, y+h)
205  Eigen::VectorXd f4(dim_f); // f(x-h, y-h)
206 
207  // df(x,y)/(dxdx) = 1/(h^2) ( f(x+h,y) - 2f(x,y) + f(x-h,y)
208  // df(x,y)/(dxdy) = 1/(4h^2) ( f1 - f2 - f3 + f4 )
209 
210  // TODO(roesmann) symmetry?
211  for (int i = 0; i < dim_x; ++i)
212  {
213  for (int j = 0; j < dim_x; ++j)
214  {
215  if (i == j)
216  {
217  inc_fun(i, delta);
218  eval_fun(f1);
219  inc_fun(i, -ddelta);
220  eval_fun(f3);
221  inc_fun(i, delta);
222  eval_fun(f2);
223 
224  if (multipliers)
225  {
226  hessian(i, j) = scalar_xx * (f1[0] - 2 * f2[0] + f3[0]) * multipliers[0];
227  for (int v = 1; v < dim_f; ++v)
228  {
229  hessian(i, j) += scalar_xx * (f1[v] - 2 * f2[v] + f3[v]) * multipliers[v];
230  }
231  }
232  else
233  {
234  hessian(i, j) = scalar_xx * (f1[0] - 2 * f2[0] + f3[0]);
235  for (int v = 1; v < dim_f; ++v)
236  {
237  hessian(i, j) += scalar_xx * (f1[v] - 2 * f2[v] + f3[v]);
238  }
239  }
240  }
241  else
242  {
243  inc_fun(i, delta);
244  inc_fun(j, delta);
245  eval_fun(f1);
246  inc_fun(j, -ddelta);
247  eval_fun(f2);
248  inc_fun(i, -ddelta);
249  eval_fun(f4);
250  inc_fun(j, ddelta);
251  eval_fun(f3);
252 
253  // reset values
254  inc_fun(i, delta);
255  inc_fun(j, -delta);
256 
257  if (multipliers)
258  {
259  hessian(i, j) = scalar_xy * (f1[0] - f2[0] - f3[0] + f4[0]) * multipliers[0];
260  for (int v = 1; v < dim_f; ++v)
261  {
262  hessian(i, j) += scalar_xy * (f1[v] - f2[v] - f3[v] + f4[v]) * multipliers[v];
263  }
264  }
265  else
266  {
267  hessian(i, j) = scalar_xy * (f1[0] - f2[0] - f3[0] + f4[0]);
268  for (int v = 1; v < dim_f; ++v)
269  {
270  hessian(i, j) += scalar_xy * (f1[v] - f2[v] - f3[v] + f4[v]);
271  }
272  }
273  }
274  }
275  }
276 }
277 
278 template <typename IncFun, typename EvalFun>
279 void CentralDifferences::jacobianHessian(IncFun inc_fun, EvalFun eval_fun, Eigen::Ref<Eigen::MatrixXd> jacobian, Eigen::Ref<Eigen::MatrixXd> hessian,
280  const double* multipliers)
281 {
282  constexpr const double delta = 1e-5; // discretization width
283  constexpr const double ddelta = 2 * delta; // two times the discretization width
284  constexpr const double scalar_x = 1 / ddelta; // auxillary variable to avoid some repeated flops.
285  constexpr const double scalar_xx = 1 / (delta * delta); // auxillary variable to avoid some repeated flops.
286  constexpr const double scalar_xy = 1 / (2.0 * delta * delta); // auxillary variable to avoid some repeated flops.
287 
288  assert(hessian.rows() == hessian.cols());
289  assert(jacobian.cols() == hessian.cols());
290 
291  int dim_x = hessian.cols();
292  int dim_f = jacobian.rows();
293 
294  Eigen::Map<const Eigen::VectorXd> multipliers_vec(multipliers, dim_f);
295 
296  // https://en.wikipedia.org/wiki/Finite_difference
297 
298  Eigen::VectorXd f0(dim_f); // f(x+h,y+h)
299  Eigen::VectorXd f1(dim_f); // f(x+h,y)
300  Eigen::VectorXd f2(dim_f); // f(x, y+h)
301  Eigen::VectorXd f3(dim_f); // f(x,y)
302  Eigen::VectorXd f4(dim_f); // f(x-h,y)
303  Eigen::VectorXd f5(dim_f); // f(x, y-h)
304  Eigen::VectorXd f6(dim_f); // f(x-h,y-h)
305 
306  // TODO(roesmann) symmetry?
307 
308  for (int i = 0; i < dim_x; ++i)
309  {
310  for (int j = 0; j < dim_x; ++j)
311  {
312  if (i == j)
313  {
314  inc_fun(i, delta);
315  eval_fun(f1);
316  inc_fun(i, -ddelta);
317  eval_fun(f4);
318  inc_fun(i, delta);
319  eval_fun(f3);
320 
321  if (multipliers)
322  {
323  hessian(i, j) = scalar_xx * (f1[0] - 2 * f3[0] + f4[0]) * multipliers[0];
324  for (int v = 1; v < dim_f; ++v)
325  {
326  hessian(i, j) += scalar_xx * (f1[v] - 2 * f3[v] + f4[v]) * multipliers[v];
327  }
328  jacobian.col(i).noalias() = scalar_x * (f1 - f4).cwiseProduct(multipliers_vec); // only for i==j
329  }
330  else
331  {
332  hessian(i, j) = scalar_xx * (f1[0] - 2 * f3[0] + f4[0]);
333  for (int v = 1; v < dim_f; ++v)
334  {
335  hessian(i, j) += scalar_xx * (f1[v] - 2 * f3[v] + f4[v]);
336  }
337  jacobian.col(i).noalias() = scalar_x * (f1 - f4); // only for i==j
338  }
339  }
340  else
341  {
342  inc_fun(i, delta);
343  eval_fun(f1);
344  inc_fun(j, delta);
345  eval_fun(f0);
346  inc_fun(i, -delta);
347  eval_fun(f2);
348  inc_fun(j, -ddelta);
349  eval_fun(f5);
350  inc_fun(i, -delta);
351  eval_fun(f6);
352  inc_fun(j, delta);
353  eval_fun(f4);
354  inc_fun(i, delta);
355  eval_fun(f3);
356 
357  if (multipliers)
358  {
359  hessian(i, j) = scalar_xy * (f0[0] - f1[0] - f2[0] + 2 * f3[0] - f4[0] - f5[0] + f6[0]) * multipliers[0];
360  for (int v = 1; v < dim_f; ++v)
361  {
362  hessian(i, j) += scalar_xy * (f0[v] - f1[v] - f2[v] + 2 * f3[v] - f4[v] - f5[v] + f6[v]) * multipliers[v];
363  }
364  }
365  else
366  {
367  hessian(i, j) = scalar_xy * (f0[0] - f1[0] - f2[0] + 2 * f3[0] - f4[0] - f5[0] + f6[0]);
368  for (int v = 1; v < dim_f; ++v)
369  {
370  hessian(i, j) += scalar_xy * (f0[v] - f1[v] - f2[v] + 2 * f3[v] - f4[v] - f5[v] + f6[v]);
371  }
372  }
373  }
374  }
375  }
376 }
377 
378 } // namespace corbo
finite_differences.h
corbo::CentralDifferences::jacobianHessian
void jacobianHessian(IncFun inc_fun, EvalFun eval_fun, Eigen::Ref< Eigen::MatrixXd > jacobian, Eigen::Ref< Eigen::MatrixXd > hessian, const double *multipliers=nullptr)
Definition: finite_differences.hpp:279
corbo::ForwardDifferences::jacobian
static void jacobian(IncFun inc_fun, EvalFun eval_fun, Eigen::Ref< Eigen::MatrixXd > jacobian)
Compute Jacobian of a desired function.
Definition: finite_differences.hpp:30
corbo::CentralDifferences::jacobian
static void jacobian(IncFun inc_fun, EvalFun eval_fun, Eigen::Ref< Eigen::MatrixXd > jacobian)
Compute Jacobian of a desired function.
Definition: finite_differences.hpp:167
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
corbo::ForwardDifferences::jacobianHessian
void jacobianHessian(IncFun inc_fun, EvalFun eval_fun, Eigen::Ref< Eigen::MatrixXd > jacobian, Eigen::Ref< Eigen::MatrixXd > hessian, const double *multipliers=nullptr)
Compute Jacobian and Hessian of a desired function.
Definition: finite_differences.hpp:106
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
corbo::CentralDifferences::hessian
static void hessian(IncFun inc_fun, EvalFun eval_fun, int dim_f, Eigen::Ref< Eigen::MatrixXd > hessian, const double *multipliers=nullptr)
Definition: finite_differences.hpp:191
corbo::ForwardDifferences::hessian
static void hessian(IncFun inc_fun, EvalFun eval_fun, int dim_f, Eigen::Ref< Eigen::MatrixXd > hessian, const double *multipliers=nullptr)
Compute Hessian of a desired function.
Definition: finite_differences.hpp:52

control_box_rst
Author(s): Christoph Rösmann
autogenerated on Mon Feb 28 2022 22:06:51