starting work on a Numerical differenciation module
diff --git a/unsupported/Eigen/NumericalDiff b/unsupported/Eigen/NumericalDiff new file mode 100644 index 0000000..991ce7c --- /dev/null +++ b/unsupported/Eigen/NumericalDiff
@@ -0,0 +1,50 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// Eigen is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see <http://www.gnu.org/licenses/>. + +#ifndef EIGEN_NUMERICALDIFF_MODULE +#define EIGEN_NUMERICALDIFF_MODULE + +#include <Eigen/Core> + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup NumericalDiff_Module Support for numerical differenciation. + * See http://en.wikipedia.org/wiki/Numerical_differentiation + * + * \code + * #include <unsupported/Eigen/NumericalDiff> + * \endcode + */ +//@{ + +} + +#include "src/NumericalDiff/NumericalDiff.h" + +namespace Eigen { +//@} +} + +#endif // EIGEN_NUMERICALDIFF_MODULE
diff --git a/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h b/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h new file mode 100644 index 0000000..223cf9e --- /dev/null +++ b/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h
@@ -0,0 +1,95 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// Eigen is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see <http://www.gnu.org/licenses/>. + +#ifndef EIGEN_NUMERICAL_DIFF_H +#define EIGEN_NUMERICAL_DIFF_H + +namespace Eigen +{ + +template<typename Functor> class NumericalDiff : public Functor +{ +public: + typedef typename Functor::Scalar Scalar; + typedef typename Functor::InputType InputType; + typedef typename Functor::ValueType ValueType; + typedef typename Functor::JacobianType JacobianType; + + NumericalDiff(Scalar _epsfcn=0.) : Functor(), epsfcn(_epsfcn) {} + NumericalDiff(const Functor& f, Scalar _epsfcn=0.) : Functor(f), epsfcn(_epsfcn) {} + + // forward constructors + template<typename T0> + NumericalDiff(const T0& a0) : Functor(a0), epsfcn(0) {} + template<typename T0, typename T1> + NumericalDiff(const T0& a0, const T1& a1) : Functor(a0, a1), epsfcn(0) {} + template<typename T0, typename T1, typename T2> + NumericalDiff(const T0& a0, const T1& a1, const T1& a2) : Functor(a0, a1, a2), epsfcn(0) {} + + enum { + InputsAtCompileTime = Functor::InputsAtCompileTime, + ValuesAtCompileTime = Functor::ValuesAtCompileTime + }; + + /** + * return the number of evaluation of functor + */ + int df(const InputType& _x, JacobianType &jac) const + { + /* Local variables */ + Scalar h; + int nfev=0; + const int n = _x.size(); + const Scalar eps = ei_sqrt((std::max(epsfcn,epsilon<Scalar>() ))); + ValueType val, fx; + InputType x = _x; + // TODO : we should do this only if the size is not already known + val.resize(Functor::values()); + fx.resize(Functor::values()); + + // compute f(x) + Functor::operator()(x, fx); + + /* Function Body */ + + for (int j = 0; j < n; ++j) { + h = eps * ei_abs(x[j]); + if (h == 0.) { + h = eps; + } + x[j] += h; + Functor::operator()(x, val); + nfev++; + x[j] = _x[j]; + jac.col(j) = (val-fx)/h; + } + return nfev; + } +private: + Scalar epsfcn; +}; + +} // namespace + +#endif // EIGEN_NUMERICAL_DIFF_H
diff --git a/unsupported/test/CMakeLists.txt b/unsupported/test/CMakeLists.txt index 007a9c4..6c02111 100644 --- a/unsupported/test/CMakeLists.txt +++ b/unsupported/test/CMakeLists.txt
@@ -16,6 +16,7 @@ endif(ADOLC_FOUND) ei_add_test(NonLinear) +ei_add_test(NumericalDiff) ei_add_test(autodiff) ei_add_test(BVH) #ei_add_test(matrixExponential)
diff --git a/unsupported/test/NumericalDiff.cpp b/unsupported/test/NumericalDiff.cpp new file mode 100644 index 0000000..1bc9e61 --- /dev/null +++ b/unsupported/test/NumericalDiff.cpp
@@ -0,0 +1,95 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> + +#include <stdio.h> + +#include "main.h" +#include <unsupported/Eigen/NumericalDiff> + +// Generic functor +template<typename _Scalar, int NX=Dynamic, int NY=Dynamic> +struct Functor +{ + typedef _Scalar Scalar; + enum { + InputsAtCompileTime = NX, + ValuesAtCompileTime = NY + }; + typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; + typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; + typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; + + int m_inputs, m_values; + + Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} + Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {} + + int inputs() const { return m_inputs; } + int values() const { return m_values; } + +}; + +struct my_functor : Functor<double> +{ + my_functor(void): Functor<double>(3,15) {} + int operator()(const VectorXd &x, VectorXd &fvec) const + { + double tmp1, tmp2, tmp3; + double y[15] = {1.4e-1, 1.8e-1, 2.2e-1, 2.5e-1, 2.9e-1, 3.2e-1, 3.5e-1, + 3.9e-1, 3.7e-1, 5.8e-1, 7.3e-1, 9.6e-1, 1.34, 2.1, 4.39}; + + for (int i = 0; i < values(); i++) + { + tmp1 = i+1; + tmp2 = 16 - i - 1; + tmp3 = (i>=8)? tmp2 : tmp1; + fvec[i] = y[i] - (x[0] + tmp1/(x[1]*tmp2 + x[2]*tmp3)); + } + return 0; + } + + int df(const VectorXd &x, MatrixXd &fjac) const + { + double tmp1, tmp2, tmp3, tmp4; + for (int i = 0; i < values(); i++) + { + tmp1 = i+1; + tmp2 = 16 - i - 1; + tmp3 = (i>=8)? tmp2 : tmp1; + tmp4 = (x[1]*tmp2 + x[2]*tmp3); tmp4 = tmp4*tmp4; + fjac(i,0) = -1; + fjac(i,1) = tmp1*tmp2/tmp4; + fjac(i,2) = tmp1*tmp3/tmp4; + } + return 0; + } +}; + +void test_forward() +{ + VectorXd x(3); + MatrixXd jac(15,3); + MatrixXd actual_jac(15,3); + my_functor functor; + + x << 0.082, 1.13, 2.35; + + // real one + functor.df(x, actual_jac); +// std::cout << actual_jac << std::endl << std::endl; + + // using NumericalDiff + NumericalDiff<my_functor> numDiff(functor); + numDiff.df(x, jac); +// std::cout << jac << std::endl; + + VERIFY_IS_APPROX(jac, actual_jac); +} + + +void test_NumericalDiff() +{ + CALL_SUBTEST(test_forward()); +}