starting work on a Numerical differenciation module

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

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

unsupported/test/CMakeLists.txt

@@ -16,6 +16,7 @@ else(ADOLC_FOUND)
 endif(ADOLC_FOUND)
 
 ei_add_test(NonLinear)
+ei_add_test(NumericalDiff)
 ei_add_test(autodiff)
 ei_add_test(BVH)
 #ei_add_test(matrixExponential)

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());
+}