1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Gael Guennebaud <[email protected]>
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 // 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>
foo(const Vector & p)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
TestFunc1TestFunc146 TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
TestFunc1TestFunc147 TestFunc1(int inputs_, int values_) : m_inputs(inputs_), m_values(values_) {}
48
inputsTestFunc149 int inputs() const { return m_inputs; }
valuesTestFunc150 int values() const { return m_values; }
51
52 template<typename T>
operator ()TestFunc153 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
operator ()TestFunc172 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 */
integratorFunctorintegratorFunctor120 integratorFunctor(const Scalar gain) : _gain(gain) {}
integratorFunctorintegratorFunctor121 integratorFunctor(const integratorFunctor& f) : _gain(f._gain) {}
122 const Scalar _gain;
123
124 template <typename T1, typename T2>
operator ()integratorFunctor125 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>
operator ()integratorFunctor136 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
forward_jacobian_cpp11(const Func & f)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
forward_jacobian(const Func & f)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>
test_autodiff_scalar()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>
test_autodiff_vector()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>
test_autodiff_jacobian()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>
test_autodiff_hessian()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
bug_1222()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 #ifdef EIGEN_TEST_PART_5
310
bug_1223()311 double bug_1223() {
312 using std::min;
313 typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD;
314
315 const double _cv1_3 = 1.0;
316 const AD chi_3 = 1.0;
317 const AD denom = 1.0;
318
319 // failed because implementation of min attempts to construct ADS<DerType&> via constructor AutoDiffScalar(const Real& value)
320 // without initializing m_derivatives (which is a reference in this case)
321 #define EIGEN_TEST_SPACE
322 const AD t = min EIGEN_TEST_SPACE (denom / chi_3, 1.0);
323
324 const AD t2 = min EIGEN_TEST_SPACE (denom / (chi_3 * _cv1_3), 1.0);
325
326 return t.value() + t2.value();
327 }
328
329 // regression test for some compilation issues with specializations of ScalarBinaryOpTraits
bug_1260()330 void bug_1260() {
331 Matrix4d A = Matrix4d::Ones();
332 Vector4d v = Vector4d::Ones();
333 A*v;
334 }
335
336 // check a compilation issue with numext::max
bug_1261()337 double bug_1261() {
338 typedef AutoDiffScalar<Matrix2d> AD;
339 typedef Matrix<AD,2,1> VectorAD;
340
341 VectorAD v(0.,0.);
342 const AD maxVal = v.maxCoeff();
343 const AD minVal = v.minCoeff();
344 return maxVal.value() + minVal.value();
345 }
346
bug_1264()347 double bug_1264() {
348 typedef AutoDiffScalar<Vector2d> AD;
349 const AD s = 0.;
350 const Matrix<AD, 3, 1> v1(0.,0.,0.);
351 const Matrix<AD, 3, 1> v2 = (s + 3.0) * v1;
352 return v2(0).value();
353 }
354
355 // check with expressions on constants
bug_1281()356 double bug_1281() {
357 int n = 2;
358 typedef AutoDiffScalar<VectorXd> AD;
359 const AD c = 1.;
360 AD x0(2,n,0);
361 AD y1 = (AD(c)+AD(c))*x0;
362 y1 = x0 * (AD(c)+AD(c));
363 AD y2 = (-AD(c))+x0;
364 y2 = x0+(-AD(c));
365 AD y3 = (AD(c)*(-AD(c))+AD(c))*x0;
366 y3 = x0 * (AD(c)*(-AD(c))+AD(c));
367 return (y1+y2+y3).value();
368 }
369
370 #endif
371
EIGEN_DECLARE_TEST(autodiff)372 EIGEN_DECLARE_TEST(autodiff)
373 {
374 for(int i = 0; i < g_repeat; i++) {
375 CALL_SUBTEST_1( test_autodiff_scalar<1>() );
376 CALL_SUBTEST_2( test_autodiff_vector<1>() );
377 CALL_SUBTEST_3( test_autodiff_jacobian<1>() );
378 CALL_SUBTEST_4( test_autodiff_hessian<1>() );
379 }
380
381 CALL_SUBTEST_5( bug_1222() );
382 CALL_SUBTEST_5( bug_1223() );
383 CALL_SUBTEST_5( bug_1260() );
384 CALL_SUBTEST_5( bug_1261() );
385 CALL_SUBTEST_5( bug_1281() );
386 }
387
388