1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include "main.h" 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include <unsupported/Eigen/AutoDiff> 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_DONT_INLINE Scalar foo(const Scalar& x, const Scalar& y) 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using namespace std; 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// return x+std::sin(y); 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_ASM_COMMENT("mybegin"); 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return static_cast<Scalar>(x*2 - pow(x,2) + 2*sqrt(y*y) - 4 * sin(x) + 2 * cos(y) - exp(-0.5*x*x)); 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath //return x+2*y*x;//x*2 -std::pow(x,2);//(2*y/x);// - y*2; 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_ASM_COMMENT("myend"); 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Vector> 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_DONT_INLINE typename Vector::Scalar foo(const Vector& p) 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename Vector::Scalar Scalar; 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return (p-Vector(Scalar(-1),Scalar(1.))).norm() + (p.array() * p.array()).sum() + p.dot(p); 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar, int NX=Dynamic, int NY=Dynamic> 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct TestFunc1 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath InputsAtCompileTime = NX, 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ValuesAtCompileTime = NY 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int m_inputs, m_values; 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath TestFunc1(int inputs, int values) : m_inputs(inputs), m_values(values) {} 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int inputs() const { return m_inputs; } 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int values() const { return m_values; } 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename T> 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void operator() (const Matrix<T,InputsAtCompileTime,1>& x, Matrix<T,ValuesAtCompileTime,1>* _v) const 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix<T,ValuesAtCompileTime,1>& v = *_v; 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v[0] = 2 * x[0] * x[0] + x[0] * x[1]; 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v[1] = 3 * x[1] * x[0] + 0.5 * x[1] * x[1]; 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(inputs()>2) 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v[0] += 0.5 * x[2]; 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v[1] += x[2]; 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(values()>2) 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v[2] = 3 * x[1] * x[0] * x[0]; 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (inputs()>2 && values()>2) 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v[2] *= x[2]; 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void operator() (const InputType& x, ValueType* v, JacobianType* _j) const 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath (*this)(x, v); 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(_j) 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath JacobianType& j = *_j; 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(0,0) = 4 * x[0] + x[1]; 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(1,0) = 3 * x[1]; 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(0,1) = x[0]; 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(1,1) = 3 * x[0] + 2 * 0.5 * x[1]; 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (inputs()>2) 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(0,2) = 0.5; 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(1,2) = 1; 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(values()>2) 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(2,0) = 3 * x[1] * 2 * x[0]; 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(2,1) = 3 * x[0] * x[0]; 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (inputs()>2 && values()>2) 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(2,0) *= x[2]; 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(2,1) *= x[2]; 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(2,2) = 3 * x[1] * x[0] * x[0]; 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j(2,2) = 3 * x[1] * x[0] * x[0]; 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Func> void forward_jacobian(const Func& f) 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typename Func::InputType x = Func::InputType::Random(f.inputs()); 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typename Func::ValueType y(f.values()), yref(f.values()); 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typename Func::JacobianType j(f.values(),f.inputs()), jref(f.values(),f.inputs()); 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath jref.setZero(); 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath yref.setZero(); 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath f(x,&yref,&jref); 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// std::cerr << y.transpose() << "\n\n";; 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// std::cerr << j << "\n\n";; 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath j.setZero(); 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath y.setZero(); 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AutoDiffJacobian<Func> autoj(f); 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath autoj(x, &y, &j); 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// std::cerr << y.transpose() << "\n\n";; 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// std::cerr << j << "\n\n";; 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(y, yref); 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(j, jref); 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 1307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// TODO also check actual derivatives! 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_autodiff_scalar() 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 1347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Vector2f p = Vector2f::Random(); 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef AutoDiffScalar<Vector2f> AD; 1367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez AD ax(p.x(),Vector2f::UnitX()); 1377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez AD ay(p.y(),Vector2f::UnitY()); 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AD res = foo<AD>(ax,ay); 1397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y())); 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 1427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// TODO also check actual derivatives! 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_autodiff_vector() 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 1457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Vector2f p = Vector2f::Random(); 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef AutoDiffScalar<Vector2f> AD; 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<AD,2,1> VectorAD; 1487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorAD ap = p.cast<AD>(); 1497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ap.x().derivatives() = Vector2f::UnitX(); 1507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ap.y().derivatives() = Vector2f::UnitY(); 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 1527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez AD res = foo<VectorAD>(ap); 1537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX(res.value(), foo(p)); 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_autodiff_jacobian() 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 1587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) )); 1597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) )); 1607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) )); 1617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) )); 1627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) )); 163c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 164c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 165c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_autodiff() 166c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 1677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for(int i = 0; i < g_repeat; i++) { 1687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST_1( test_autodiff_scalar() ); 1697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST_2( test_autodiff_vector() ); 1707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST_3( test_autodiff_jacobian() ); 1717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 172c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 173c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 174