NumericalDiff.cpp revision c981c48f5bc9aefeffc0bcb0cc3934c2fae179dd
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 Thomas Capricelli <orzel@freehackers.org> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include <stdio.h> 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include "main.h" 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include <unsupported/Eigen/NumericalDiff> 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Generic functor 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar, int NX=Dynamic, int NY=Dynamic> 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct Functor 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath InputsAtCompileTime = NX, 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ValuesAtCompileTime = NY 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int m_inputs, m_values; 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int inputs() const { return m_inputs; } 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int values() const { return m_values; } 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct my_functor : Functor<double> 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath my_functor(void): Functor<double>(3,15) {} 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int operator()(const VectorXd &x, VectorXd &fvec) const 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath double tmp1, tmp2, tmp3; 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath double y[15] = {1.4e-1, 1.8e-1, 2.2e-1, 2.5e-1, 2.9e-1, 3.2e-1, 3.5e-1, 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 3.9e-1, 3.7e-1, 5.8e-1, 7.3e-1, 9.6e-1, 1.34, 2.1, 4.39}; 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (int i = 0; i < values(); i++) 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp1 = i+1; 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp2 = 16 - i - 1; 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp3 = (i>=8)? tmp2 : tmp1; 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fvec[i] = y[i] - (x[0] + tmp1/(x[1]*tmp2 + x[2]*tmp3)); 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return 0; 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int actual_df(const VectorXd &x, MatrixXd &fjac) const 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath double tmp1, tmp2, tmp3, tmp4; 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (int i = 0; i < values(); i++) 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp1 = i+1; 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp2 = 16 - i - 1; 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp3 = (i>=8)? tmp2 : tmp1; 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp4 = (x[1]*tmp2 + x[2]*tmp3); tmp4 = tmp4*tmp4; 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac(i,0) = -1; 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac(i,1) = tmp1*tmp2/tmp4; 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac(i,2) = tmp1*tmp3/tmp4; 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return 0; 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_forward() 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VectorXd x(3); 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath MatrixXd jac(15,3); 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath MatrixXd actual_jac(15,3); 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath my_functor functor; 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath x << 0.082, 1.13, 2.35; 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // real one 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath functor.actual_df(x, actual_jac); 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// std::cout << actual_jac << std::endl << std::endl; 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // using NumericalDiff 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath NumericalDiff<my_functor> numDiff(functor); 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath numDiff.df(x, jac); 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// std::cout << jac << std::endl; 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(jac, actual_jac); 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_central() 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VectorXd x(3); 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath MatrixXd jac(15,3); 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath MatrixXd actual_jac(15,3); 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath my_functor functor; 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath x << 0.082, 1.13, 2.35; 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // real one 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath functor.actual_df(x, actual_jac); 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // using NumericalDiff 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath NumericalDiff<my_functor,Central> numDiff(functor); 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath numDiff.df(x, jac); 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(jac, actual_jac); 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_NumericalDiff() 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST(test_forward()); 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST(test_central()); 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 115