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>
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   // pow(float, int) promotes to pow(double, double)
20   return x*2 - 1 + static_cast<Scalar>(pow(1+x,2)) + 2*sqrt(y*y+0) - 4 * sin(0+x) + 2 * cos(y+0) - exp(Scalar(-0.5)*x*x+0);
21   //return x+2*y*x;//x*2 -std::pow(x,2);//(2*y/x);// - y*2;
22   EIGEN_ASM_COMMENT("myend");
23 }
24 
25 template<typename Vector>
26 EIGEN_DONT_INLINE typename Vector::Scalar foo(const Vector& p)
27 {
28   typedef typename Vector::Scalar Scalar;
29   return (p-Vector(Scalar(-1),Scalar(1.))).norm() + (p.array() * p.array()).sum() + p.dot(p);
30 }
31 
32 template<typename _Scalar, int NX=Dynamic, int NY=Dynamic>
33 struct TestFunc1
34 {
35   typedef _Scalar Scalar;
36   enum {
37     InputsAtCompileTime = NX,
38     ValuesAtCompileTime = NY
39   };
40   typedef Matrix<Scalar,InputsAtCompileTime,1> InputType;
41   typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
42   typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
43 
44   int m_inputs, m_values;
45 
46   TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
47   TestFunc1(int inputs, int values) : m_inputs(inputs), m_values(values) {}
48 
49   int inputs() const { return m_inputs; }
50   int values() const { return m_values; }
51 
52   template<typename T>
53   void operator() (const Matrix<T,InputsAtCompileTime,1>& x, Matrix<T,ValuesAtCompileTime,1>* _v) const
54   {
55     Matrix<T,ValuesAtCompileTime,1>& v = *_v;
56 
57     v[0] = 2 * x[0] * x[0] + x[0] * x[1];
58     v[1] = 3 * x[1] * x[0] + 0.5 * x[1] * x[1];
59     if(inputs()>2)
60     {
61       v[0] += 0.5 * x[2];
62       v[1] += x[2];
63     }
64     if(values()>2)
65     {
66       v[2] = 3 * x[1] * x[0] * x[0];
67     }
68     if (inputs()>2 && values()>2)
69       v[2] *= x[2];
70   }
71 
72   void operator() (const InputType& x, ValueType* v, JacobianType* _j) const
73   {
74     (*this)(x, v);
75 
76     if(_j)
77     {
78       JacobianType& j = *_j;
79 
80       j(0,0) = 4 * x[0] + x[1];
81       j(1,0) = 3 * x[1];
82 
83       j(0,1) = x[0];
84       j(1,1) = 3 * x[0] + 2 * 0.5 * x[1];
85 
86       if (inputs()>2)
87       {
88         j(0,2) = 0.5;
89         j(1,2) = 1;
90       }
91       if(values()>2)
92       {
93         j(2,0) = 3 * x[1] * 2 * x[0];
94         j(2,1) = 3 * x[0] * x[0];
95       }
96       if (inputs()>2 && values()>2)
97       {
98         j(2,0) *= x[2];
99         j(2,1) *= x[2];
100 
101         j(2,2) = 3 * x[1] * x[0] * x[0];
102         j(2,2) = 3 * x[1] * x[0] * x[0];
103       }
104     }
105   }
106 };
107 
108 
109 #if EIGEN_HAS_VARIADIC_TEMPLATES
110 /* Test functor for the C++11 features. */
111 template <typename Scalar>
112 struct integratorFunctor
113 {
114     typedef Matrix<Scalar, 2, 1> InputType;
115     typedef Matrix<Scalar, 2, 1> ValueType;
116 
117     /*
118      * Implementation starts here.
119      */
120     integratorFunctor(const Scalar gain) : _gain(gain) {}
121     integratorFunctor(const integratorFunctor& f) : _gain(f._gain) {}
122     const Scalar _gain;
123 
124     template <typename T1, typename T2>
125     void operator() (const T1 &input, T2 *output, const Scalar dt) const
126     {
127         T2 &o = *output;
128 
129         /* Integrator to test the AD. */
130         o[0] = input[0] + input[1] * dt * _gain;
131         o[1] = input[1] * _gain;
132     }
133 
134     /* Only needed for the test */
135     template <typename T1, typename T2, typename T3>
136     void operator() (const T1 &input, T2 *output, T3 *jacobian, const Scalar dt) const
137     {
138         T2 &o = *output;
139 
140         /* Integrator to test the AD. */
141         o[0] = input[0] + input[1] * dt * _gain;
142         o[1] = input[1] * _gain;
143 
144         if (jacobian)
145         {
146             T3 &j = *jacobian;
147 
148             j(0, 0) = 1;
149             j(0, 1) = dt * _gain;
150             j(1, 0) = 0;
151             j(1, 1) = _gain;
152         }
153     }
154 
155 };
156 
157 template<typename Func> void forward_jacobian_cpp11(const Func& f)
158 {
159     typedef typename Func::ValueType::Scalar Scalar;
160     typedef typename Func::ValueType ValueType;
161     typedef typename Func::InputType InputType;
162     typedef typename AutoDiffJacobian<Func>::JacobianType JacobianType;
163 
164     InputType x = InputType::Random(InputType::RowsAtCompileTime);
165     ValueType y, yref;
166     JacobianType j, jref;
167 
168     const Scalar dt = internal::random<double>();
169 
170     jref.setZero();
171     yref.setZero();
172     f(x, &yref, &jref, dt);
173 
174     //std::cerr << "y, yref, jref: " << "\n";
175     //std::cerr << y.transpose() << "\n\n";
176     //std::cerr << yref << "\n\n";
177     //std::cerr << jref << "\n\n";
178 
179     AutoDiffJacobian<Func> autoj(f);
180     autoj(x, &y, &j, dt);
181 
182     //std::cerr << "y j (via autodiff): " << "\n";
183     //std::cerr << y.transpose() << "\n\n";
184     //std::cerr << j << "\n\n";
185 
186     VERIFY_IS_APPROX(y, yref);
187     VERIFY_IS_APPROX(j, jref);
188 }
189 #endif
190 
191 template<typename Func> void forward_jacobian(const Func& f)
192 {
193     typename Func::InputType x = Func::InputType::Random(f.inputs());
194     typename Func::ValueType y(f.values()), yref(f.values());
195     typename Func::JacobianType j(f.values(),f.inputs()), jref(f.values(),f.inputs());
196 
197     jref.setZero();
198     yref.setZero();
199     f(x,&yref,&jref);
200 //     std::cerr << y.transpose() << "\n\n";;
201 //     std::cerr << j << "\n\n";;
202 
203     j.setZero();
204     y.setZero();
205     AutoDiffJacobian<Func> autoj(f);
206     autoj(x, &y, &j);
207 //     std::cerr << y.transpose() << "\n\n";;
208 //     std::cerr << j << "\n\n";;
209 
210     VERIFY_IS_APPROX(y, yref);
211     VERIFY_IS_APPROX(j, jref);
212 }
213 
214 // TODO also check actual derivatives!
215 template <int>
216 void test_autodiff_scalar()
217 {
218   Vector2f p = Vector2f::Random();
219   typedef AutoDiffScalar<Vector2f> AD;
220   AD ax(p.x(),Vector2f::UnitX());
221   AD ay(p.y(),Vector2f::UnitY());
222   AD res = foo<AD>(ax,ay);
223   VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y()));
224 }
225 
226 
227 // TODO also check actual derivatives!
228 template <int>
229 void test_autodiff_vector()
230 {
231   Vector2f p = Vector2f::Random();
232   typedef AutoDiffScalar<Vector2f> AD;
233   typedef Matrix<AD,2,1> VectorAD;
234   VectorAD ap = p.cast<AD>();
235   ap.x().derivatives() = Vector2f::UnitX();
236   ap.y().derivatives() = Vector2f::UnitY();
237 
238   AD res = foo<VectorAD>(ap);
239   VERIFY_IS_APPROX(res.value(), foo(p));
240 }
241 
242 template <int>
243 void test_autodiff_jacobian()
244 {
245   CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) ));
246   CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) ));
247   CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) ));
248   CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) ));
249   CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) ));
250 #if EIGEN_HAS_VARIADIC_TEMPLATES
251   CALL_SUBTEST(( forward_jacobian_cpp11(integratorFunctor<double>(10)) ));
252 #endif
253 }
254 
255 
256 template <int>
257 void test_autodiff_hessian()
258 {
259   typedef AutoDiffScalar<VectorXd> AD;
260   typedef Matrix<AD,Eigen::Dynamic,1> VectorAD;
261   typedef AutoDiffScalar<VectorAD> ADD;
262   typedef Matrix<ADD,Eigen::Dynamic,1> VectorADD;
263   VectorADD x(2);
264   double s1 = internal::random<double>(), s2 = internal::random<double>(), s3 = internal::random<double>(), s4 = internal::random<double>();
265   x(0).value()=s1;
266   x(1).value()=s2;
267 
268   //set unit vectors for the derivative directions (partial derivatives of the input vector)
269   x(0).derivatives().resize(2);
270   x(0).derivatives().setZero();
271   x(0).derivatives()(0)= 1;
272   x(1).derivatives().resize(2);
273   x(1).derivatives().setZero();
274   x(1).derivatives()(1)=1;
275 
276   //repeat partial derivatives for the inner AutoDiffScalar
277   x(0).value().derivatives() = VectorXd::Unit(2,0);
278   x(1).value().derivatives() = VectorXd::Unit(2,1);
279 
280   //set the hessian matrix to zero
281   for(int idx=0; idx<2; idx++) {
282       x(0).derivatives()(idx).derivatives()  = VectorXd::Zero(2);
283       x(1).derivatives()(idx).derivatives()  = VectorXd::Zero(2);
284   }
285 
286   ADD y = sin(AD(s3)*x(0) + AD(s4)*x(1));
287 
288   VERIFY_IS_APPROX(y.value().derivatives()(0), y.derivatives()(0).value());
289   VERIFY_IS_APPROX(y.value().derivatives()(1), y.derivatives()(1).value());
290   VERIFY_IS_APPROX(y.value().derivatives()(0), s3*std::cos(s1*s3+s2*s4));
291   VERIFY_IS_APPROX(y.value().derivatives()(1), s4*std::cos(s1*s3+s2*s4));
292   VERIFY_IS_APPROX(y.derivatives()(0).derivatives(), -std::sin(s1*s3+s2*s4)*Vector2d(s3*s3,s4*s3));
293   VERIFY_IS_APPROX(y.derivatives()(1).derivatives(),  -std::sin(s1*s3+s2*s4)*Vector2d(s3*s4,s4*s4));
294 
295   ADD z = x(0)*x(1);
296   VERIFY_IS_APPROX(z.derivatives()(0).derivatives(), Vector2d(0,1));
297   VERIFY_IS_APPROX(z.derivatives()(1).derivatives(), Vector2d(1,0));
298 }
299 
300 double bug_1222() {
301   typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD;
302   const double _cv1_3 = 1.0;
303   const AD chi_3 = 1.0;
304   // this line did not work, because operator+ returns ADS<DerType&>, which then cannot be converted to ADS<DerType>
305   const AD denom = chi_3 + _cv1_3;
306   return denom.value();
307 }
308 
309 double bug_1223() {
310   using std::min;
311   typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD;
312 
313   const double _cv1_3 = 1.0;
314   const AD chi_3 = 1.0;
315   const AD denom = 1.0;
316 
317   // failed because implementation of min attempts to construct ADS<DerType&> via constructor AutoDiffScalar(const Real& value)
318   // without initializing m_derivatives (which is a reference in this case)
319   #define EIGEN_TEST_SPACE
320   const AD t = min EIGEN_TEST_SPACE (denom / chi_3, 1.0);
321 
322   const AD t2 = min EIGEN_TEST_SPACE (denom / (chi_3 * _cv1_3), 1.0);
323 
324   return t.value() + t2.value();
325 }
326 
327 // regression test for some compilation issues with specializations of ScalarBinaryOpTraits
328 void bug_1260() {
329   Matrix4d A;
330   Vector4d v;
331   A*v;
332 }
333 
334 // check a compilation issue with numext::max
335 double bug_1261() {
336   typedef AutoDiffScalar<Matrix2d> AD;
337   typedef Matrix<AD,2,1> VectorAD;
338 
339   VectorAD v;
340   const AD maxVal = v.maxCoeff();
341   const AD minVal = v.minCoeff();
342   return maxVal.value() + minVal.value();
343 }
344 
345 double bug_1264() {
346   typedef AutoDiffScalar<Vector2d> AD;
347   const AD s;
348   const Matrix<AD, 3, 1> v1;
349   const Matrix<AD, 3, 1> v2 = (s + 3.0) * v1;
350   return v2(0).value();
351 }
352 
353 void test_autodiff()
354 {
355   for(int i = 0; i < g_repeat; i++) {
356     CALL_SUBTEST_1( test_autodiff_scalar<1>() );
357     CALL_SUBTEST_2( test_autodiff_vector<1>() );
358     CALL_SUBTEST_3( test_autodiff_jacobian<1>() );
359     CALL_SUBTEST_4( test_autodiff_hessian<1>() );
360   }
361 
362   bug_1222();
363   bug_1223();
364   bug_1260();
365   bug_1261();
366 }
367 
368