Home | History | Annotate | Download | only in test 
      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 (at) 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 void test_autodiff_scalar()
    131 {
    132   std::cerr << foo<float>(1,2) << "\n";
    133   typedef AutoDiffScalar<Vector2f> AD;
    134   AD ax(1,Vector2f::UnitX());
    135   AD ay(2,Vector2f::UnitY());
    136   AD res = foo<AD>(ax,ay);
    137   std::cerr << res.value() << " <> "
    138             << res.derivatives().transpose() << "\n\n";
    139 }
    140 
    141 void test_autodiff_vector()
    142 {
    143   std::cerr << foo<Vector2f>(Vector2f(1,2)) << "\n";
    144   typedef AutoDiffScalar<Vector2f> AD;
    145   typedef Matrix<AD,2,1> VectorAD;
    146   VectorAD p(AD(1),AD(-1));
    147   p.x().derivatives() = Vector2f::UnitX();
    148   p.y().derivatives() = Vector2f::UnitY();
    149 
    150   AD res = foo<VectorAD>(p);
    151   std::cerr << res.value() << " <> "
    152             << res.derivatives().transpose() << "\n\n";
    153 }
    154 
    155 void test_autodiff_jacobian()
    156 {
    157   for(int i = 0; i < g_repeat; i++) {
    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 
    166 void test_autodiff()
    167 {
    168     test_autodiff_scalar();
    169     test_autodiff_vector();
    170 //     test_autodiff_jacobian();
    171 }
    172 
    173