1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10#include "main.h"
11#include <unsupported/Eigen/AutoDiff>
12
13template<typename Scalar>
14EIGEN_DONT_INLINE Scalar foo(const Scalar& x, const Scalar& y)
15{
16  using namespace std;
17//   return x+std::sin(y);
18  EIGEN_ASM_COMMENT("mybegin");
19  return static_cast<Scalar>(x*2 - pow(x,2) + 2*sqrt(y*y) - 4 * sin(x) + 2 * cos(y) - exp(-0.5*x*x));
20  //return x+2*y*x;//x*2 -std::pow(x,2);//(2*y/x);// - y*2;
21  EIGEN_ASM_COMMENT("myend");
22}
23
24template<typename Vector>
25EIGEN_DONT_INLINE typename Vector::Scalar foo(const Vector& p)
26{
27  typedef typename Vector::Scalar Scalar;
28  return (p-Vector(Scalar(-1),Scalar(1.))).norm() + (p.array() * p.array()).sum() + p.dot(p);
29}
30
31template<typename _Scalar, int NX=Dynamic, int NY=Dynamic>
32struct TestFunc1
33{
34  typedef _Scalar Scalar;
35  enum {
36    InputsAtCompileTime = NX,
37    ValuesAtCompileTime = NY
38  };
39  typedef Matrix<Scalar,InputsAtCompileTime,1> InputType;
40  typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
41  typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
42
43  int m_inputs, m_values;
44
45  TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
46  TestFunc1(int inputs, int values) : m_inputs(inputs), m_values(values) {}
47
48  int inputs() const { return m_inputs; }
49  int values() const { return m_values; }
50
51  template<typename T>
52  void operator() (const Matrix<T,InputsAtCompileTime,1>& x, Matrix<T,ValuesAtCompileTime,1>* _v) const
53  {
54    Matrix<T,ValuesAtCompileTime,1>& v = *_v;
55
56    v[0] = 2 * x[0] * x[0] + x[0] * x[1];
57    v[1] = 3 * x[1] * x[0] + 0.5 * x[1] * x[1];
58    if(inputs()>2)
59    {
60      v[0] += 0.5 * x[2];
61      v[1] += x[2];
62    }
63    if(values()>2)
64    {
65      v[2] = 3 * x[1] * x[0] * x[0];
66    }
67    if (inputs()>2 && values()>2)
68      v[2] *= x[2];
69  }
70
71  void operator() (const InputType& x, ValueType* v, JacobianType* _j) const
72  {
73    (*this)(x, v);
74
75    if(_j)
76    {
77      JacobianType& j = *_j;
78
79      j(0,0) = 4 * x[0] + x[1];
80      j(1,0) = 3 * x[1];
81
82      j(0,1) = x[0];
83      j(1,1) = 3 * x[0] + 2 * 0.5 * x[1];
84
85      if (inputs()>2)
86      {
87        j(0,2) = 0.5;
88        j(1,2) = 1;
89      }
90      if(values()>2)
91      {
92        j(2,0) = 3 * x[1] * 2 * x[0];
93        j(2,1) = 3 * x[0] * x[0];
94      }
95      if (inputs()>2 && values()>2)
96      {
97        j(2,0) *= x[2];
98        j(2,1) *= x[2];
99
100        j(2,2) = 3 * x[1] * x[0] * x[0];
101        j(2,2) = 3 * x[1] * x[0] * x[0];
102      }
103    }
104  }
105};
106
107template<typename Func> void forward_jacobian(const Func& f)
108{
109    typename Func::InputType x = Func::InputType::Random(f.inputs());
110    typename Func::ValueType y(f.values()), yref(f.values());
111    typename Func::JacobianType j(f.values(),f.inputs()), jref(f.values(),f.inputs());
112
113    jref.setZero();
114    yref.setZero();
115    f(x,&yref,&jref);
116//     std::cerr << y.transpose() << "\n\n";;
117//     std::cerr << j << "\n\n";;
118
119    j.setZero();
120    y.setZero();
121    AutoDiffJacobian<Func> autoj(f);
122    autoj(x, &y, &j);
123//     std::cerr << y.transpose() << "\n\n";;
124//     std::cerr << j << "\n\n";;
125
126    VERIFY_IS_APPROX(y, yref);
127    VERIFY_IS_APPROX(j, jref);
128}
129
130
131// TODO also check actual derivatives!
132void test_autodiff_scalar()
133{
134  Vector2f p = Vector2f::Random();
135  typedef AutoDiffScalar<Vector2f> AD;
136  AD ax(p.x(),Vector2f::UnitX());
137  AD ay(p.y(),Vector2f::UnitY());
138  AD res = foo<AD>(ax,ay);
139  VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y()));
140}
141
142// TODO also check actual derivatives!
143void test_autodiff_vector()
144{
145  Vector2f p = Vector2f::Random();
146  typedef AutoDiffScalar<Vector2f> AD;
147  typedef Matrix<AD,2,1> VectorAD;
148  VectorAD ap = p.cast<AD>();
149  ap.x().derivatives() = Vector2f::UnitX();
150  ap.y().derivatives() = Vector2f::UnitY();
151
152  AD res = foo<VectorAD>(ap);
153  VERIFY_IS_APPROX(res.value(), foo(p));
154}
155
156void test_autodiff_jacobian()
157{
158  CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) ));
159  CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) ));
160  CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) ));
161  CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) ));
162  CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) ));
163}
164
165void test_autodiff()
166{
167  for(int i = 0; i < g_repeat; i++) {
168    CALL_SUBTEST_1( test_autodiff_scalar() );
169    CALL_SUBTEST_2( test_autodiff_vector() );
170    CALL_SUBTEST_3( test_autodiff_jacobian() );
171  }
172}
173
174