Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 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 | |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 130 | |
| 131 | // TODO also check actual derivatives! |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 132 | void test_autodiff_scalar() |
| 133 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 134 | Vector2f p = Vector2f::Random(); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 135 | typedef AutoDiffScalar<Vector2f> AD; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 136 | AD ax(p.x(),Vector2f::UnitX()); |
| 137 | AD ay(p.y(),Vector2f::UnitY()); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 138 | AD res = foo<AD>(ax,ay); |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 139 | VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y())); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 140 | } |
| 141 | |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 142 | // TODO also check actual derivatives! |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 143 | void test_autodiff_vector() |
| 144 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 145 | Vector2f p = Vector2f::Random(); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 146 | typedef AutoDiffScalar<Vector2f> AD; |
| 147 | typedef Matrix<AD,2,1> VectorAD; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 148 | VectorAD ap = p.cast<AD>(); |
| 149 | ap.x().derivatives() = Vector2f::UnitX(); |
| 150 | ap.y().derivatives() = Vector2f::UnitY(); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 151 | |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 152 | AD res = foo<VectorAD>(ap); |
| 153 | VERIFY_IS_APPROX(res.value(), foo(p)); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 154 | } |
| 155 | |
| 156 | void test_autodiff_jacobian() |
| 157 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 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)) )); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 163 | } |
| 164 | |
| 165 | void test_autodiff() |
| 166 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 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 | } |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 172 | } |
| 173 | |