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 | 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>
|
---|
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 |
|
---|
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 |
|
---|
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!
|
---|
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!
|
---|
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 |
|
---|
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 |
|
---|
165 | double bug_1222() {
|
---|
166 | typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD;
|
---|
167 | const double _cv1_3 = 1.0;
|
---|
168 | const AD chi_3 = 1.0;
|
---|
169 | // this line did not work, because operator+ returns ADS<DerType&>, which then cannot be converted to ADS<DerType>
|
---|
170 | const AD denom = chi_3 + _cv1_3;
|
---|
171 | return denom.value();
|
---|
172 | }
|
---|
173 |
|
---|
174 | void test_autodiff()
|
---|
175 | {
|
---|
176 | for(int i = 0; i < g_repeat; i++) {
|
---|
177 | CALL_SUBTEST_1( test_autodiff_scalar() );
|
---|
178 | CALL_SUBTEST_2( test_autodiff_vector() );
|
---|
179 | CALL_SUBTEST_3( test_autodiff_jacobian() );
|
---|
180 | }
|
---|
181 |
|
---|
182 | bug_1222();
|
---|
183 | }
|
---|
184 |
|
---|