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 
13 template<typename Scalar>
foo(const Scalar & x,const Scalar & y)14 EIGEN_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 
24 template<typename Vector>
foo(const Vector & p)25 EIGEN_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 
31 template<typename _Scalar, int NX=Dynamic, int NY=Dynamic>
32 struct 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 
TestFunc1TestFunc145   TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
TestFunc1TestFunc146   TestFunc1(int inputs, int values) : m_inputs(inputs), m_values(values) {}
47 
inputsTestFunc148   int inputs() const { return m_inputs; }
valuesTestFunc149   int values() const { return m_values; }
50 
51   template<typename T>
operator ()TestFunc152   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 
operator ()TestFunc171   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 
forward_jacobian(const Func & f)107 template<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!
test_autodiff_scalar()132 void 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!
test_autodiff_vector()143 void 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 
test_autodiff_jacobian()156 void 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 
test_autodiff()165 void 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