test_autodiff.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018, University of Edinburgh
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #include <gtest/gtest.h>
31 #include <Eigen/Dense>
32 
33 #if EIGEN_VERSION_AT_LEAST(3, 3, 0)
34 // Autodiff is only supported with Eigen 3.3.0 or higher
35 
36 #include <exotica_core/tools/autodiff_chain_hessian.h>
37 #include <exotica_core/tools/autodiff_chain_hessian_sparse.h>
38 #include <exotica_core/tools/autodiff_chain_jacobian.h>
39 #include <exotica_core/tools/autodiff_chain_jacobian_sparse.h>
40 #include <exotica_core/tools/finitediff_chain_hessian.h>
41 #include <exotica_core/tools/finitediff_chain_jacobian.h>
42 
43 #include <iostream>
44 
45 using namespace exotica;
46 
47 constexpr int N = 1000;
48 constexpr double THRESHOLD_ANALYTIC = 1e-5;
49 constexpr double THRESHOLD_FINITE_DIFFERENCES = 1e-3;
50 constexpr double THRESHOLD_FINITE_DIFFERENCES_HESSIAN = 5e-1;
51 constexpr double INPUT_VECTOR_SCALE = 2.0;
52 
53 // This function combines Function3(Function2(x)).
54 // The result will be used for comparison with passing the derivative of Function2 into the AutoDiff.
55 template <class FunctorType>
56 struct Function1 : public FunctorType
57 {
58  enum
59  {
60  Inputs = 4,
61  Values = 4,
62  JacobianCols = 4
63  };
64 
65  template <typename T>
66  void operator()(const Eigen::Matrix<T, FunctorType::InputType::RowsAtCompileTime, 1> &x, Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> &y) const
67  {
68  Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> tmp(y.rows());
69  // Always cast known scalar type matrices/vectors into the templated type <T>.
70  // This is required for AutoDiff to work properly.
71  for (int i = 0; i < 4; ++i)
72  {
73  y(i, 0) = (Eigen::AngleAxis<T>(x(i, 0), Eigen::Vector3d::UnitZ().cast<T>()).toRotationMatrix() * Eigen::Vector3d::UnitX().cast<T>()).dot(Eigen::Vector3d::UnitX().cast<T>());
74  tmp(i, 0) = y(i, 0);
75  if (i > 0) y(i, 0) += tmp(i - 1, 0);
76  }
77  }
78 };
79 
80 // Function2 rotates a UnitX vector around Z axis.
81 // This is a helper function that will provide input for Function3.
82 template <class FunctorType>
83 struct Function2 : public FunctorType
84 {
85  enum
86  {
87  Inputs = 4,
88  Values = 12,
89  JacobianCols = 4
90  };
91 
92  template <typename T>
93  void operator()(const Eigen::Matrix<T, FunctorType::InputType::RowsAtCompileTime, 1> &x, Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> &y) const
94  {
95  Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> tmp(y.rows());
96  for (int i = 0; i < 4; ++i)
97  {
98  y.block(i * 3, 0, 3, 1) = Eigen::AngleAxis<T>(x(i, 0), Eigen::Vector3d::UnitZ().cast<T>()).toRotationMatrix() * Eigen::Vector3d::UnitX().cast<T>();
99  }
100  }
101 };
102 
103 // This function computes dot product between the input vector and UnitX.
104 // The input vector will be taken from the output of Function2 (including the derivatives).
105 template <class FunctorType>
106 struct Function3 : public FunctorType
107 {
108  enum
109  {
110  Inputs = 12,
111  Values = 4,
112  JacobianCols = 4
113  };
114 
115  template <typename T>
116  void operator()(const Eigen::Matrix<T, FunctorType::InputType::RowsAtCompileTime, 1> &x, Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> &y) const
117  {
118  Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> tmp(y.rows());
119  for (int i = 0; i < 4; ++i)
120  {
121  y(i, 0) = Eigen::Vector3d::UnitX().cast<T>().dot(x.block(i * 3, 0, 3, 1));
122  tmp(i, 0) = y(i, 0);
123  if (i > 0) y(i, 0) += tmp(i - 1, 0);
124  }
125  }
126 };
127 
128 // Typedefs for convenience
129 template <template <typename> class DiffType, typename _FunctorType, template <typename> class FunctionType>
130 struct TestingBase
131 {
132  typedef _FunctorType FunctorType;
133  typedef FunctionType<FunctorType> Function;
134  typedef DiffType<Function> Diff;
135  typedef typename Diff::InputType InputType;
136  typedef typename Diff::ValueType ValueType;
137  typedef typename Diff::JacobianType JacobianType;
138 };
139 
140 // Compute Jacobians using AutoDiff
141 template <class T>
142 struct JacobianFull : public T
143 {
144  void operator()(const typename T::InputType &x, typename T::ValueType &y,
145  typename T::JacobianType &j)
146  {
147  typename T::Function f;
148  typename T::Diff autoj(f);
149 
150  if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
151  y.resize(T::Function::Values, 1);
152  if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
153  j.resize(T::Function::Values, T::Function::JacobianCols);
154 
155  // Compute full Jacobian
156  autoj(x, y, j);
157  }
158 };
159 
160 // Compute Jacobians using AutoDiff given the Jacobian of the input variables
161 template <class T>
162 struct JacobianCompound : public T
163 {
164  void operator()(const typename T::InputType &x, typename T::ValueType &y,
165  typename T::JacobianType &j, const typename T::Diff::InputJacobianType &ij)
166  {
167  typename T::Function f;
168  typename T::Diff autoj(f);
169 
170  if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
171  y.resize(T::Function::Values, 1);
172  if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
173  j.resize(T::Function::Values, T::Function::JacobianCols);
174 
175  // Compute compund Jacobian
176  autoj(x, y, j, ij);
177  }
178 };
179 
180 // Compute Jacobians using FiniteDiff
181 // Requires separate method because of additional template parameters
182 template <class T, Eigen::NumericalDiffMode mode>
183 struct JacobianFullFinite : public T
184 {
185  typedef Eigen::FiniteDiffChainJacobian<typename T::Function, mode> diff;
186 
187  int operator()(const typename T::InputType &x, typename T::ValueType &y,
188  typename T::JacobianType &j)
189  {
190  typename T::Function f;
191  diff autoj(f);
192 
193  if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
194  y.resize(T::Function::Values, 1);
195  if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
196  j.resize(T::Function::Values, T::Function::JacobianCols);
197 
198  // Compute full Jacobian
199  return autoj(x, y, j);
200  }
201 };
202 
203 // Compute Jacobians using FiniteDiff given the Jacobian of the input variables
204 // Requires separate method because of additional template parameters
205 // compute_intermediate is used to provide update of the inputs
206 template <class T, class T2, Eigen::NumericalDiffMode mode>
207 struct JacobianCompoundFinite : public T
208 {
209  typedef Eigen::FiniteDiffChainJacobian<typename T::Function, mode> diff;
210  typedef typename T2::Function F2;
211 
212  int operator()(const typename diff::InputJacobianRowType &x, typename T::ValueType &y,
213  typename T::JacobianType &j)
214  {
215  typename T::Function f;
216 
217  auto compute_intermediate = [](const typename diff::InputJacobianRowType &jx, typename T::InputType &_x) {
218  if (T::InputType::RowsAtCompileTime == Eigen::Dynamic) _x.resize(F2::Values);
219  F2 f;
220  // This is where the chaining of the functions happens
221  f(jx, _x);
222  };
223  diff autoj(f, compute_intermediate);
224 
225  if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
226  y.resize(T::Function::Values, 1);
227  if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
228  j.resize(T::Function::Values, T::Function::JacobianCols);
229 
230  // Compute full Jacobian
231  return autoj(x, y, j);
232  }
233 };
234 
235 // Compute Hessians using AutoDiff
236 template <class T>
237 struct HessianFull : public T
238 {
239  void operator()(const typename T::InputType &x, typename T::ValueType &y,
240  typename T::JacobianType &j, typename T::Diff::HessianType &hess)
241  {
242  typename T::Function f;
243  typename T::Diff autoj(f);
244 
245  if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
246  {
247  y.resize(T::Function::Values, 1);
248  hess.resize(T::Function::Values);
249  }
250  if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
251  {
252  j.resize(T::Function::Values, T::Function::JacobianCols);
253  for (int i = 0; i < hess.rows(); ++i)
254  {
255  hess[i].resize(T::Function::JacobianCols, T::Function::JacobianCols);
256  }
257  }
258 
259  // Compute full Hessian
260  autoj(x, y, j, hess);
261  }
262 };
263 
264 // Compute Hessians using AutoDiff given the Jacobian of the input variables
265 template <class T>
266 struct HessianCompound : public T
267 {
268  void operator()(const typename T::InputType &x, typename T::ValueType &y,
269  typename T::JacobianType &j, typename T::Diff::HessianType &hess,
270  const typename T::Diff::InputJacobianType &ij, const typename T::Diff::InputHessianType &ihess)
271  {
272  typename T::Function f;
273  typename T::Diff autoj(f);
274 
275  if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
276  {
277  y.resize(T::Function::Values, 1);
278  hess.resize(T::Function::Values);
279  }
280  if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
281  {
282  j.resize(T::Function::Values, T::Function::JacobianCols);
283  for (int i = 0; i < hess.rows(); ++i)
284  {
285  hess[i].resize(T::Function::JacobianCols, T::Function::JacobianCols);
286  }
287  }
288 
289  // Compute the Jacobian and Hessian of the compound function.
290  autoj(x, y, j, hess, ij, ihess);
291  }
292 };
293 
294 // Compute Hessians using FiniteDiff
295 // Requires separate method because of additional template parameters
296 template <class T, Eigen::NumericalDiffMode mode>
297 struct HessianFullFinite : public T
298 {
299  typedef Eigen::FiniteDiffChainHessian<typename T::Function, mode> diff;
300 
301  int operator()(const typename T::InputType &x, typename T::ValueType &y,
302  typename T::JacobianType &j, typename T::Diff::HessianType &hess)
303  {
304  typename T::Function f;
305  diff autoj(f);
306 
307  if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
308  {
309  y.resize(T::Function::Values, 1);
310  hess.resize(T::Function::Values);
311  }
312  if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
313  {
314  j.resize(T::Function::Values, T::Function::JacobianCols);
315  for (int i = 0; i < hess.rows(); ++i)
316  {
317  hess[i].resize(T::Function::JacobianCols, T::Function::JacobianCols);
318  }
319  }
320 
321  // Compute the Jacobian and Hessian of the compound function.
322  return autoj(x, y, j, hess);
323  }
324 };
325 
326 // Compute Hessians using FiniteDiff given the Jacobian of the input variables
327 // Requires separate method because of additional template parameters
328 // compute_intermediate is used to provide update of the inputs
329 template <class T, class T2, Eigen::NumericalDiffMode mode>
330 struct HessianCompoundFinite : public T
331 {
332  typedef Eigen::FiniteDiffChainHessian<typename T::Function, mode> diff;
333  typedef typename T2::Function F2;
334 
335  int operator()(const typename diff::InputJacobianRowType &x, typename T::ValueType &y,
336  typename T::JacobianType &j, typename T::Diff::HessianType &hess)
337  {
338  typename T::Function f;
339 
340  auto compute_intermediate = [](const typename diff::InputJacobianRowType &jx, typename T::InputType &_x) {
341  if (T::InputType::RowsAtCompileTime == Eigen::Dynamic) _x.resize(F2::Values);
342  F2 f;
343  // This is where the chaining of the functions happens
344  f(jx, _x);
345  };
346  diff autoj(f, compute_intermediate);
347 
348  if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
349  {
350  y.resize(T::Function::Values, 1);
351  hess.resize(T::Function::Values);
352  }
353  if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
354  {
355  j.resize(T::Function::Values, T::Function::JacobianCols);
356  for (int i = 0; i < hess.rows(); ++i)
357  {
358  hess[i].resize(T::Function::JacobianCols, T::Function::JacobianCols);
359  }
360  }
361 
362  // Compute the Jacobian and Hessian of the compound function.
363  return autoj(x, y, j, hess);
364  }
365 };
366 
367 // Compile time matrix sizes
368 struct TestStaticTrait
369 {
370  enum
371  {
372  Inputs1 = 4,
373  Values1 = 4,
374  JacobianCols1 = 4,
375  Inputs2 = 4,
376  Values2 = 12,
377  JacobianCols2 = 4,
378  Inputs3 = 12,
379  Values3 = 4,
380  JacobianCols3 = 4
381  };
382 };
383 
384 // Dynamic marix sizes
385 struct TestDynamicTrait
386 {
387  enum
388  {
389  Inputs1 = Eigen::Dynamic,
390  Values1 = Eigen::Dynamic,
391  JacobianCols1 = Eigen::Dynamic,
392  Inputs2 = Eigen::Dynamic,
393  Values2 = Eigen::Dynamic,
394  JacobianCols2 = Eigen::Dynamic,
395  Inputs3 = Eigen::Dynamic,
396  Values3 = Eigen::Dynamic,
397  JacobianCols3 = Eigen::Dynamic
398  };
399 };
400 
401 // All tests are comparing Function1(x) == Function3(Function2(x))
402 template <template <typename> class DiffType, typename Dynamic, Eigen::NumericalDiffMode mode>
403 void TestJacobians()
404 {
405  for (int i = 0; i < N; ++i)
406  {
407  // Setup functors
408  typedef FunctorBase<double, Dynamic::Inputs1, Dynamic::Values1, Dynamic::JacobianCols1> MyFunctor1;
409  typedef FunctorBase<double, Dynamic::Inputs2, Dynamic::Values2, Dynamic::JacobianCols2> MyFunctor2;
410  typedef FunctorBase<double, Dynamic::Inputs3, Dynamic::Values3, Dynamic::JacobianCols3> MyFunctor3;
411  // Define helper types
412  typedef TestingBase<DiffType, MyFunctor1, Function1> Test1;
413  typedef TestingBase<DiffType, MyFunctor2, Function2> Test2;
414  typedef TestingBase<DiffType, MyFunctor3, Function3> Test3;
415 
416  typename Test1::InputType x1;
417  if (Test1::InputType::RowsAtCompileTime == Eigen::Dynamic)
418  {
419  x1 = Test1::InputType::Random(4) * INPUT_VECTOR_SCALE;
420  }
421  else
422  {
423  x1 = Test1::InputType::Random() * INPUT_VECTOR_SCALE;
424  }
425 
426  typename Test1::ValueType y1;
427  typename Test1::JacobianType j1;
428 
429  typename Test2::ValueType y2;
430  typename Test2::JacobianType j2;
431 
432  typename Test3::ValueType y3;
433  typename Test3::JacobianType j3;
434 
435  // Compute Function1(x)'
436  JacobianFull<Test1>()(x1, y1, j1);
437  // Compute Function2(x)'
438  JacobianFull<Test2>()(x1, y2, j2);
439  // Compute Function3(Function2(x))'
440  JacobianCompound<Test3>()(y2, y3, j3, j2);
441 
442  typename Test1::ValueType y1fd;
443  typename Test1::JacobianType j1fd;
444  typename Test3::ValueType y3fd;
445  typename Test3::JacobianType j3fd;
446 
447  // Compute Function1(x)' using finite differences
448  JacobianFullFinite<Test1, mode>()(x1, y1fd, j1fd);
449  // Compute Function3(Function2(x))' using finite differences
450  JacobianCompoundFinite<Test3, Test2, mode>()(x1, y3fd, j3fd);
451 
452  EXPECT_TRUE((y1 - y3).norm() < THRESHOLD_ANALYTIC);
453  EXPECT_TRUE((j1 - j1fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
454  EXPECT_TRUE((j3 - j3fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
455  EXPECT_TRUE((y1fd - y3fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
456  EXPECT_TRUE((y1 - y1fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
457  }
458 }
459 
460 // The sparse test is only different because FiniteDiff doesn't work with sparse matrices
461 template <template <typename> class DiffType, typename Dynamic>
462 void TestJacobiansSparse()
463 {
464  for (int i = 0; i < N; ++i)
465  {
466  // Setup functors
467  typedef FunctorBase<double, Dynamic::Inputs1, Dynamic::Values1, Dynamic::JacobianCols1> MyFunctor1;
468  typedef FunctorBase<double, Dynamic::Inputs2, Dynamic::Values2, Dynamic::JacobianCols2> MyFunctor2;
469  typedef FunctorBase<double, Dynamic::Inputs3, Dynamic::Values3, Dynamic::JacobianCols3> MyFunctor3;
470  // Define sparse helper types
471  typedef TestingBase<DiffType, MyFunctor1, Function1> Test1;
472  typedef TestingBase<DiffType, MyFunctor2, Function2> Test2;
473  typedef TestingBase<DiffType, MyFunctor3, Function3> Test3;
474 
475  typename Test1::InputType x1;
476  if (Test1::InputType::RowsAtCompileTime == Eigen::Dynamic)
477  {
478  x1 = Test1::InputType::Random(4) * INPUT_VECTOR_SCALE;
479  }
480  else
481  {
482  x1 = Test1::InputType::Random() * INPUT_VECTOR_SCALE;
483  }
484 
485  typename Test1::ValueType y1;
486  typename Test1::JacobianType j1;
487 
488  typename Test2::ValueType y2;
489  typename Test2::JacobianType j2;
490 
491  typename Test3::ValueType y3;
492  typename Test3::JacobianType j3;
493 
494  // Compute sparse Function1(x)'
495  JacobianFull<Test1>()(x1, y1, j1);
496  // Compute sparse Function2(x)'
497  JacobianFull<Test2>()(x1, y2, j2);
498  // Compute sparse Function3(Function2(x))'
499  JacobianCompound<Test3>()(y2, y3, j3, j2);
500 
501  // Define dense helper types
502  typedef TestingBase<Eigen::AutoDiffChainJacobian, MyFunctor1, Function1> Test1D;
503  typedef TestingBase<Eigen::AutoDiffChainJacobian, MyFunctor2, Function2> Test2D;
504  typedef TestingBase<Eigen::AutoDiffChainJacobian, MyFunctor3, Function3> Test3D;
505  typename Test1D::ValueType y1d;
506  typename Test1D::JacobianType j1d;
507  typename Test2D::ValueType y2d;
508  typename Test2D::JacobianType j2d;
509  typename Test3D::ValueType y3d;
510  typename Test3D::JacobianType j3d;
511 
512  // Compute dense Function1(x)'
513  JacobianFull<Test1D>()(x1, y1d, j1d);
514  // Compute dense Function2(x)'
515  JacobianFull<Test2D>()(x1, y2d, j2d);
516  // Compute dense Function3(Function2(x))'
517  JacobianCompound<Test3D>()(y2d, y3d, j3d, j2d);
518 
519  EXPECT_TRUE((y1 - y3).norm() < THRESHOLD_ANALYTIC);
520  EXPECT_TRUE((j1 - j1d).norm() < THRESHOLD_ANALYTIC);
521  EXPECT_TRUE((j3 - j3d).norm() < THRESHOLD_ANALYTIC);
522  EXPECT_TRUE((y1d - y3d).norm() < THRESHOLD_ANALYTIC);
523  EXPECT_TRUE((y1 - y1d).norm() < THRESHOLD_ANALYTIC);
524  }
525 }
526 
527 // Hessian norm
528 // TODO: define a better norm
529 template <typename HessianType1, typename HessianType2>
530 double diffNorm(const HessianType1 &A, const HessianType2 &B)
531 {
532  double ret = 0;
533  double tmp;
534  for (int i = 0; i < A.rows(); ++i)
535  {
536  tmp = (A[i] - B[i]).norm();
537  ret += tmp * tmp;
538  }
539  return ret / static_cast<double>(A.rows());
540 }
541 
542 template <template <typename> class DiffType, typename Dynamic, Eigen::NumericalDiffMode mode>
543 void TestHessians()
544 {
545  for (int i = 0; i < N; ++i)
546  {
547  // Setup functors
548  typedef FunctorBase<double, Dynamic::Inputs1, Dynamic::Values1, Dynamic::JacobianCols1> MyFunctor1;
549  typedef FunctorBase<double, Dynamic::Inputs2, Dynamic::Values2, Dynamic::JacobianCols2> MyFunctor2;
550  typedef FunctorBase<double, Dynamic::Inputs3, Dynamic::Values3, Dynamic::JacobianCols3> MyFunctor3;
551  // Define helper types
552  typedef TestingBase<DiffType, MyFunctor1, Function1> Test1;
553  typedef TestingBase<DiffType, MyFunctor2, Function2> Test2;
554  typedef TestingBase<DiffType, MyFunctor3, Function3> Test3;
555 
556  typename Test1::InputType x1;
557  if (Test1::InputType::RowsAtCompileTime == Eigen::Dynamic)
558  {
559  x1 = Test1::InputType::Random(4) * INPUT_VECTOR_SCALE;
560  }
561  else
562  {
563  x1 = Test1::InputType::Random() * INPUT_VECTOR_SCALE;
564  }
565 
566  typename Test1::ValueType y1;
567  typename Test1::JacobianType j1;
568  typename Test1::Diff::HessianType hess1;
569 
570  typename Test1::ValueType y1fd;
571  typename Test1::JacobianType j1fd;
572  typename Test1::Diff::HessianType hess1fd;
573 
574  typename Test2::ValueType y2;
575  typename Test2::JacobianType j2;
576  typename Test2::Diff::HessianType hess2;
577 
578  typename Test3::ValueType y3;
579  typename Test3::JacobianType j3;
580  typename Test3::Diff::HessianType hess3;
581 
582  typename Test3::ValueType y3fd;
583  typename Test3::JacobianType j3fd;
584  typename Test3::Diff::HessianType hess3fd;
585 
586  // Compute Function1(x)''
587  HessianFull<Test1>()(x1, y1, j1, hess1);
588  // Compute Function2(x)''
589  HessianFull<Test2>()(x1, y2, j2, hess2);
590  // Compute Function3(Function2(x))''
591  HessianCompound<Test3>()(y2, y3, j3, hess3, j2, hess2);
592 
593  // Compute Function1(x)'' using finite differences
594  HessianFullFinite<Test1, mode>()(x1, y1fd, j1fd, hess1fd);
595  // Compute Function3(Function2(x))'' using finite differences
596  HessianCompoundFinite<Test3, Test2, mode>()(x1, y3fd, j3fd, hess3fd);
597 
598  EXPECT_TRUE((y1 - y3).norm() < THRESHOLD_ANALYTIC);
599  EXPECT_TRUE((j1 - j1fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
600  EXPECT_TRUE((j3 - j3fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
601  EXPECT_TRUE((y1fd - y3fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
602  EXPECT_TRUE((y1 - y1fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
603 
604  EXPECT_TRUE(diffNorm(hess1, hess3) < THRESHOLD_ANALYTIC);
605  EXPECT_TRUE(diffNorm(hess1, hess1fd) < THRESHOLD_FINITE_DIFFERENCES_HESSIAN);
606  EXPECT_TRUE(diffNorm(hess3, hess3fd) < THRESHOLD_FINITE_DIFFERENCES_HESSIAN);
607  EXPECT_TRUE(diffNorm(hess1, hess3fd) < THRESHOLD_FINITE_DIFFERENCES_HESSIAN);
608  }
609 }
610 
611 // The sparse test is only different because FiniteDiff doesn't work with sparse matrices
612 template <template <typename> class DiffType, typename Dynamic>
613 void TestHessiansSparse()
614 {
615  for (int i = 0; i < N; ++i)
616  {
617  // Setup functors
618  typedef FunctorBase<double, Dynamic::Inputs1, Dynamic::Values1, Dynamic::JacobianCols1> MyFunctor1;
619  typedef FunctorBase<double, Dynamic::Inputs2, Dynamic::Values2, Dynamic::JacobianCols2> MyFunctor2;
620  typedef FunctorBase<double, Dynamic::Inputs3, Dynamic::Values3, Dynamic::JacobianCols3> MyFunctor3;
621  // Define sparse helper types
622  typedef TestingBase<DiffType, MyFunctor1, Function1> Test1;
623  typedef TestingBase<DiffType, MyFunctor2, Function2> Test2;
624  typedef TestingBase<DiffType, MyFunctor3, Function3> Test3;
625 
626  typename Test1::InputType x1;
627  if (Test1::InputType::RowsAtCompileTime == Eigen::Dynamic)
628  {
629  x1 = Test1::InputType::Random(4) * INPUT_VECTOR_SCALE;
630  }
631  else
632  {
633  x1 = Test1::InputType::Random() * INPUT_VECTOR_SCALE;
634  }
635 
636  typename Test1::ValueType y1;
637  typename Test1::JacobianType j1;
638  typename Test1::Diff::HessianType hess1;
639 
640  typename Test2::ValueType y2;
641  typename Test2::JacobianType j2;
642  typename Test2::Diff::HessianType hess2;
643 
644  typename Test3::ValueType y3;
645  typename Test3::JacobianType j3;
646  typename Test3::Diff::HessianType hess3;
647 
648  // Compute sparse Function1(x)''
649  HessianFull<Test1>()(x1, y1, j1, hess1);
650  // Compute sparse Function2(x)''
651  HessianFull<Test2>()(x1, y2, j2, hess2);
652  // Compute sparse Function3(Function2(x))''
653  HessianCompound<Test3>()(y2, y3, j3, hess3, j2, hess2);
654 
655  // Define dense helper types
656  typedef TestingBase<Eigen::AutoDiffChainHessian, MyFunctor1, Function1> Test1D;
657  typedef TestingBase<Eigen::AutoDiffChainHessian, MyFunctor2, Function2> Test2D;
658  typedef TestingBase<Eigen::AutoDiffChainHessian, MyFunctor3, Function3> Test3D;
659 
660  typename Test1D::ValueType y1d;
661  typename Test1D::JacobianType j1d;
662  typename Test1D::Diff::HessianType hess1d;
663 
664  typename Test2D::ValueType y2d;
665  typename Test2D::JacobianType j2d;
666  typename Test2D::Diff::HessianType hess2d;
667 
668  typename Test3D::ValueType y3d;
669  typename Test3D::JacobianType j3d;
670  typename Test3D::Diff::HessianType hess3d;
671 
672  // Compute dense Function1(x)''
673  HessianFull<Test1D>()(x1, y1d, j1d, hess1d);
674  // Compute dense Function2(x)''
675  HessianFull<Test2D>()(x1, y2d, j2d, hess2d);
676  // Compute dense Function3(Function2(x))''
677  HessianCompound<Test3D>()(y2d, y3d, j3d, hess3d, j2d, hess2d);
678 
679  EXPECT_TRUE((y1 - y3).norm() < THRESHOLD_ANALYTIC);
680  EXPECT_TRUE((j1 - j1d).norm() < THRESHOLD_ANALYTIC);
681  EXPECT_TRUE((j3 - j3d).norm() < THRESHOLD_ANALYTIC);
682  EXPECT_TRUE((y1d - y3d).norm() < THRESHOLD_ANALYTIC);
683  EXPECT_TRUE((y1 - y1d).norm() < THRESHOLD_ANALYTIC);
684 
685  EXPECT_TRUE(diffNorm(hess1, hess3) < THRESHOLD_ANALYTIC);
686  EXPECT_TRUE(diffNorm(hess1, hess1d) < THRESHOLD_ANALYTIC);
687  EXPECT_TRUE(diffNorm(hess3, hess3d) < THRESHOLD_ANALYTIC);
688  EXPECT_TRUE(diffNorm(hess1, hess3d) < THRESHOLD_ANALYTIC);
689  }
690 }
691 
692 // Through magic of tempalting, test:
693 // 1. AutoDiffChainJacobian Jacobian computation against finite differences
694 // 2. AutoDiffChainHessian Jacobian computation against finite differences
695 // 3. AutoDiffChainHessian Hessian computation against finite differences
696 // 4. Run 1 with Central differencing
697 // 5. Run 2 with Central differencing
698 // 6. Run 3 with Central differencing
699 // 7. AutoDiffChainJacobianSparse Jacobian computation against AutoDiffChainJacobian
700 // 8. AutoDiffChainHessianSparse Hessian computation against AutoDiffChainHessian
701 // 9. Run 1-7 with matrix sizes fixed at compile time
702 
703 TEST(AutoDiffJacobian, JacobianComputationDynamicMatrix)
704 {
705  TestJacobians<Eigen::AutoDiffChainJacobian, TestDynamicTrait, Eigen::Forward>();
706 }
707 
708 TEST(AutoDiffJacobian, JacobianComputationDynamicMatrixCentralDifferences)
709 {
710  TestJacobians<Eigen::AutoDiffChainJacobian, TestDynamicTrait, Eigen::Central>();
711 }
712 
713 TEST(AutoDiffJacobian, JacobianComputationTemplatedMatrix)
714 {
715  TestJacobians<Eigen::AutoDiffChainJacobian, TestStaticTrait, Eigen::Forward>();
716 }
717 
718 TEST(AutoDiffJacobian, JacobianComputationTemplatedMatrixCentralDifferences)
719 {
720  TestJacobians<Eigen::AutoDiffChainJacobian, TestStaticTrait, Eigen::Central>();
721 }
722 
723 TEST(AutoDiffHessian, JacobianComputationDynamicMatrix)
724 {
725  TestJacobians<Eigen::AutoDiffChainHessian, TestDynamicTrait, Eigen::Forward>();
726 }
727 
728 TEST(AutoDiffHessian, JacobianComputationDynamicMatrixCentralDifferences)
729 {
730  TestJacobians<Eigen::AutoDiffChainHessian, TestDynamicTrait, Eigen::Central>();
731 }
732 
733 TEST(AutoDiffHessian, JacobianComputationTemplatedMatrix)
734 {
735  TestJacobians<Eigen::AutoDiffChainHessian, TestStaticTrait, Eigen::Forward>();
736 }
737 
738 TEST(AutoDiffHessian, JacobianComputationTemplatedMatrixCentralDifferences)
739 {
740  TestJacobians<Eigen::AutoDiffChainHessian, TestStaticTrait, Eigen::Central>();
741 }
742 
743 TEST(AutoDiffHessian, HessianComputationDynamicMatrix)
744 {
745  TestHessians<Eigen::AutoDiffChainHessian, TestDynamicTrait, Eigen::Forward>();
746 }
747 
748 TEST(AutoDiffHessian, HessianComputationDynamicMatrixCentralDifferences)
749 {
750  TestHessians<Eigen::AutoDiffChainHessian, TestDynamicTrait, Eigen::Central>();
751 }
752 
753 TEST(AutoDiffHessian, HessianComputationTemplatedMatrix)
754 {
755  TestHessians<Eigen::AutoDiffChainHessian, TestStaticTrait, Eigen::Forward>();
756 }
757 
758 TEST(AutoDiffHessian, HessianComputationTemplatedMatrixCentralDifferences)
759 {
760  TestHessians<Eigen::AutoDiffChainHessian, TestStaticTrait, Eigen::Central>();
761 }
762 
763 TEST(AutoDiffJacobianSparse, JacobianComputationDynamicMatrix)
764 {
765  TestJacobiansSparse<Eigen::AutoDiffChainJacobianSparse, TestDynamicTrait>();
766 }
767 
768 TEST(AutoDiffJacobianSparse, JacobianComputationTemplatedMatrix)
769 {
770  TestJacobiansSparse<Eigen::AutoDiffChainJacobianSparse, TestStaticTrait>();
771 }
772 
773 TEST(AutoDiffHessianSparse, HessianComputationDynamicMatrix)
774 {
775  TestHessiansSparse<Eigen::AutoDiffChainHessianSparse, TestDynamicTrait>();
776 }
777 
778 TEST(AutoDiffHessianSparse, HessianComputationTemplatedMatrix)
779 {
780  TestHessiansSparse<Eigen::AutoDiffChainHessianSparse, TestStaticTrait>();
781 }
782 
783 #endif
784 
785 int main(int argc, char **argv)
786 {
787  testing::InitGoogleTest(&argc, argv);
788  int ret = RUN_ALL_TESTS();
789  return ret;
790 }
autodiff_chain_hessian.h
f
f
diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
autodiff_chain_jacobian_sparse.h
main
int main(int argc, char **argv)
Definition: test_autodiff.cpp:785
autodiff_chain_hessian_sparse.h
tf::dot
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
TEST
TEST(ExoticaCore, testKinematicJacobian)
Definition: test_kinematics.cpp:182
finitediff_chain_hessian.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
autodiff_chain_jacobian.h
Eigen::y
const T & y
Definition: autodiff_scalar.h:644
finitediff_chain_jacobian.h

exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Sat Apr 10 2021 02:34:49