diff --git a/unsupported/Eigen/AutoDiff b/unsupported/Eigen/AutoDiff new file mode 100644 index 000000000..f47bb0228 --- /dev/null +++ b/unsupported/Eigen/AutoDiff @@ -0,0 +1,54 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008-2009 Gael Guennebaud +// +// 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 . + +#ifndef EIGEN_AUTODIFF_MODULE +#define EIGEN_AUTODIFF_MODULE + +#include + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup AutoDiff_Module Auto Diff module + * + * This module features forward automatic differentation via a simple + * templated scalar type wrapper AutoDiffScalar. + * + * \code + * #include + * \endcode + */ +//@{ + +} + +#include "src/AutoDiff/AutoDiffScalar.h" +// #include "src/AutoDiff/AutoDiffVector.h" +#include "src/AutoDiff/AutoDiffJacobian.h" + +namespace Eigen { +//@} +} + +#endif // EIGEN_AUTODIFF_MODULE diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h new file mode 100644 index 000000000..2b7987975 --- /dev/null +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h @@ -0,0 +1,100 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2009 Gael Guennebaud +// +// 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 . + +#ifndef EIGEN_AUTODIFF_JACOBIAN_H +#define EIGEN_AUTODIFF_JACOBIAN_H + +namespace Eigen +{ + +template class AutoDiffJacobian : public Functor +{ +public: + AutoDiffJacobian() : Functor() {} + AutoDiffJacobian(const Functor& f) : Functor(f) {} + + // forward constructors + template + AutoDiffJacobian(const T0& a0) : Functor(a0) {} + template + AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {} + template + AutoDiffJacobian(const T0& a0, const T1& a1, const T1& a2) : Functor(a0, a1, a2) {} + + enum { + InputsAtCompileTime = Functor::InputsAtCompileTime, + ValuesAtCompileTime = Functor::ValuesAtCompileTime + }; + + typedef typename Functor::InputType InputType; + typedef typename Functor::ValueType ValueType; + typedef typename Functor::JacobianType JacobianType; + + typedef AutoDiffScalar > ActiveScalar; + + typedef Matrix ActiveInput; + typedef Matrix ActiveValue; + + void operator() (const InputType& x, ValueType* v, JacobianType* _jac) const + { + ei_assert(v!=0); + if (!_jac) + { + Functor::operator()(x, v); + return; + } + + JacobianType& jac = *_jac; + + ActiveInput ax = x.template cast(); + ActiveValue av(jac.rows()); + + if(InputsAtCompileTime==Dynamic) + { + for (int j=0; jinputs()); + for (int j=0; jinputs()); + } + + for (int j=0; j +// +// 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 . + +#ifndef EIGEN_AUTODIFF_SCALAR_H +#define EIGEN_AUTODIFF_SCALAR_H + +namespace Eigen { + +/** \class AutoDiffScalar + * \brief A scalar type replacement with automatic differentation capability + * + * \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f) + * + * This class represents a scalar value while tracking its respective derivatives. + * + * It supports the following list of global math function: + * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos, + * - ei_abs, ei_sqrt, ei_pow, ei_exp, ei_log, ei_sin, ei_cos, + * - ei_conj, ei_real, ei_imag, ei_abs2. + * + * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However, + * in that case, the expression template mechanism only occurs at the top Matrix level, + * while derivatives are computed right away. + * + */ +template +class AutoDiffScalar +{ + public: + typedef typename ei_traits::Scalar Scalar; + + inline AutoDiffScalar() {} + + inline AutoDiffScalar(const Scalar& value) + : m_value(value) + { + if(m_derivatives.size()>0) + m_derivatives.setZero(); + } + + inline AutoDiffScalar(const Scalar& value, const DerType& der) + : m_value(value), m_derivatives(der) + {} + + template + inline AutoDiffScalar(const AutoDiffScalar& other) + : m_value(other.value()), m_derivatives(other.derivatives()) + {} + + inline AutoDiffScalar(const AutoDiffScalar& other) + : m_value(other.value()), m_derivatives(other.derivatives()) + {} + + template + inline AutoDiffScalar& operator=(const AutoDiffScalar& other) + { + m_value = other.value(); + m_derivatives = other.derivatives(); + return *this; + } + + inline AutoDiffScalar& operator=(const AutoDiffScalar& other) + { + m_value = other.value(); + m_derivatives = other.derivatives(); + return *this; + } + +// inline operator const Scalar& () const { return m_value; } +// inline operator Scalar& () { return m_value; } + + inline const Scalar& value() const { return m_value; } + inline Scalar& value() { return m_value; } + + inline const DerType& derivatives() const { return m_derivatives; } + inline DerType& derivatives() { return m_derivatives; } + + template + inline const AutoDiffScalar,DerType,OtherDerType> > + operator+(const AutoDiffScalar& other) const + { + return AutoDiffScalar,DerType,OtherDerType> >( + m_value + other.value(), + m_derivatives + other.derivatives()); + } + + template + inline AutoDiffScalar& + operator+=(const AutoDiffScalar& other) + { + (*this) = (*this) + other; + return *this; + } + + template + inline const AutoDiffScalar, DerType,OtherDerType> > + operator-(const AutoDiffScalar& other) const + { + return AutoDiffScalar, DerType,OtherDerType> >( + m_value - other.value(), + m_derivatives - other.derivatives()); + } + + template + inline AutoDiffScalar& + operator-=(const AutoDiffScalar& other) + { + *this = *this - other; + return *this; + } + + template + inline const AutoDiffScalar, DerType> > + operator-() const + { + return AutoDiffScalar, DerType> >( + -m_value, + -m_derivatives); + } + + inline const AutoDiffScalar, DerType> > + operator*(const Scalar& other) const + { + return AutoDiffScalar, DerType> >( + m_value * other, + (m_derivatives * other)); + } + + friend inline const AutoDiffScalar, DerType> > + operator*(const Scalar& other, const AutoDiffScalar& a) + { + return AutoDiffScalar, DerType> >( + a.value() * other, + a.derivatives() * other); + } + + inline const AutoDiffScalar, DerType> > + operator/(const Scalar& other) const + { + return AutoDiffScalar, DerType> >( + m_value / other, + (m_derivatives * (Scalar(1)/other))); + } + + friend inline const AutoDiffScalar, DerType> > + operator/(const Scalar& other, const AutoDiffScalar& a) + { + return AutoDiffScalar, DerType> >( + other / a.value(), + a.derivatives() * (-Scalar(1)/other)); + } + + template + inline const AutoDiffScalar, + NestByValue, + NestByValue, DerType> >, + NestByValue, OtherDerType> > > > > > + operator/(const AutoDiffScalar& other) const + { + return AutoDiffScalar, + NestByValue, + NestByValue, DerType> >, + NestByValue, OtherDerType> > > > > >( + m_value / other.value(), + ((m_derivatives * other.value()).nestByValue() - (m_value * other.derivatives()).nestByValue()).nestByValue() + * (Scalar(1)/(other.value()*other.value()))); + } + + template + inline const AutoDiffScalar, + NestByValue, DerType> >, + NestByValue, OtherDerType> > > > + operator*(const AutoDiffScalar& other) const + { + return AutoDiffScalar, + NestByValue, DerType> >, + NestByValue, OtherDerType> > > >( + m_value * other.value(), + (m_derivatives * other.value()).nestByValue() + (m_value * other.derivatives()).nestByValue()); + } + + inline AutoDiffScalar& operator*=(const Scalar& other) + { + *this = *this * other; + return *this; + } + + template + inline AutoDiffScalar& operator*=(const AutoDiffScalar& other) + { + *this = *this * other; + return *this; + } + + protected: + Scalar m_value; + DerType m_derivatives; + +}; + +} + +#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \ + template \ + inline const AutoDiffScalar::Scalar>, DerType> > \ + FUNC(const AutoDiffScalar& x) { \ + typedef typename ei_traits::Scalar Scalar; \ + typedef AutoDiffScalar, DerType> > ReturnType; \ + CODE; \ + } + +namespace std +{ + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs, + return ReturnType(std::abs(x.value()), x.derivatives() * (sign(x.value())));) + + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt, + Scalar sqrtx = std::sqrt(x.value()); + return ReturnType(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));) + + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos, + return ReturnType(std::cos(x.value()), x.derivatives() * (-std::sin(x.value())));) + + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin, + return ReturnType(std::sin(x.value()),x.derivatives() * std::cos(x.value()));) + + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp, + Scalar expx = std::exp(x.value()); + return ReturnType(expx,x.derivatives() * expx);) + + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_log, + return ReturnType(std::log(x.value),x.derivatives() * (Scalar(1).x.value()));) + + template + inline const AutoDiffScalar::Scalar>, DerType> > + pow(const AutoDiffScalar& x, typename ei_traits::Scalar y) + { + typedef typename ei_traits::Scalar Scalar; + return AutoDiffScalar, DerType> >( + std::pow(x.value(),y), + x.derivatives() * (y * std::pow(x.value(),y-1))); + } + +} + +namespace Eigen { + +template +inline const AutoDiffScalar& ei_conj(const AutoDiffScalar& x) { return x; } +template +inline const AutoDiffScalar& ei_real(const AutoDiffScalar& x) { return x; } +template +inline typename DerType::Scalar ei_imag(const AutoDiffScalar&) { return 0.; } + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_abs, + return ReturnType(ei_abs(x.value()), x.derivatives() * (sign(x.value())));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_abs2, + return ReturnType(ei_abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_sqrt, + Scalar sqrtx = ei_sqrt(x.value()); + return ReturnType(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_cos, + return ReturnType(ei_cos(x.value()), x.derivatives() * (-ei_sin(x.value())));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_sin, + return ReturnType(ei_sin(x.value()),x.derivatives() * ei_cos(x.value()));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_exp, + Scalar expx = ei_exp(x.value()); + return ReturnType(expx,x.derivatives() * expx);) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_log, + return ReturnType(ei_log(x.value),x.derivatives() * (Scalar(1).x.value()));) + +template +inline const AutoDiffScalar::Scalar>, DerType> > +ei_pow(const AutoDiffScalar& x, typename ei_traits::Scalar y) +{ return std::pow(x,y);} + +#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY + +template struct NumTraits > +{ + typedef typename DerType::Scalar Real; + typedef AutoDiffScalar FloatingPoint; + enum { + IsComplex = 0, + HasFloatingPoint = 1, + ReadCost = 1, + AddCost = 1, + MulCost = 1 + }; +}; + +} + +#endif // EIGEN_AUTODIFF_SCALAR_H diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h new file mode 100644 index 000000000..a58bb6b2e --- /dev/null +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h @@ -0,0 +1,214 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2009 Gael Guennebaud +// +// 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 . + +#ifndef EIGEN_AUTODIFF_VECTOR_H +#define EIGEN_AUTODIFF_VECTOR_H + +namespace Eigen { + +/* \class AutoDiffScalar + * \brief A scalar type replacement with automatic differentation capability + * + * \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f) + * + * This class represents a scalar value while tracking its respective derivatives. + * + * It supports the following list of global math function: + * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos, + * - ei_abs, ei_sqrt, ei_pow, ei_exp, ei_log, ei_sin, ei_cos, + * - ei_conj, ei_real, ei_imag, ei_abs2. + * + * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However, + * in that case, the expression template mechanism only occurs at the top Matrix level, + * while derivatives are computed right away. + * + */ +template +class AutoDiffVector +{ + public: + typedef typename ei_traits::Scalar Scalar; + + inline AutoDiffVector() {} + + inline AutoDiffVector(const ValueType& values) + : m_values(values) + { + m_jacobian.setZero(); + } + + inline AutoDiffVector(const ValueType& values, const JacobianType& jac) + : m_values(values), m_jacobian(jac) + {} + + template + inline AutoDiffVector(const AutoDiffVector& other) + : m_values(other.values()), m_jacobian(other.jacobian()) + {} + + inline AutoDiffVector(const AutoDiffVector& other) + : m_values(other.values()), m_jacobian(other.jacobian()) + {} + + template + inline AutoDiffScalar& operator=(const AutoDiffVector& other) + { + m_values = other.values(); + m_jacobian = other.jacobian(); + return *this; + } + + inline AutoDiffVector& operator=(const AutoDiffVector& other) + { + m_values = other.values(); + m_jacobian = other.jacobian(); + return *this; + } + + inline const ValueType& values() const { return m_values; } + inline ValueType& values() { return m_values; } + + inline const JacobianType& jacobian() const { return m_jacobian; } + inline JacobianType& jacobian() { return m_jacobian; } + + template + inline const AutoDiffVector< + CwiseBinaryOp,ValueType,OtherValueType> > + CwiseBinaryOp,JacobianType,OtherJacobianType> > + operator+(const AutoDiffScalar& other) const + { + return AutoDiffVector< + CwiseBinaryOp,ValueType,OtherValueType> > + CwiseBinaryOp,JacobianType,OtherJacobianType> >( + m_values + other.values(), + m_jacobian + other.jacobian()); + } + + template + inline AutoDiffVector& + operator+=(const AutoDiffVector& other) + { + m_values += other.values(); + m_jacobian += other.jacobian(); + return *this; + } + + template + inline const AutoDiffVector< + CwiseBinaryOp,ValueType,OtherValueType> > + CwiseBinaryOp,JacobianType,OtherJacobianType> > + operator-(const AutoDiffScalar& other) const + { + return AutoDiffVector< + CwiseBinaryOp,ValueType,OtherValueType> > + CwiseBinaryOp,JacobianType,OtherJacobianType> >( + m_values - other.values(), + m_jacobian - other.jacobian()); + } + + template + inline AutoDiffVector& + operator-=(const AutoDiffVector& other) + { + m_values -= other.values(); + m_jacobian -= other.jacobian(); + return *this; + } + + inline const AutoDiffVector< + CwiseUnaryOp, ValueType> + CwiseUnaryOp, JacobianType> > + operator-() const + { + return AutoDiffVector< + CwiseUnaryOp, ValueType> + CwiseUnaryOp, JacobianType> >( + -m_values, + -m_jacobian); + } + + inline const AutoDiffVector< + CwiseUnaryOp, ValueType> + CwiseUnaryOp, JacobianType> > + operator*(const Scalar& other) const + { + return AutoDiffVector< + CwiseUnaryOp, ValueType> + CwiseUnaryOp, JacobianType> >( + m_values * other, + (m_jacobian * other)); + } + + friend inline const AutoDiffVector< + CwiseUnaryOp, ValueType> + CwiseUnaryOp, JacobianType> > + operator*(const Scalar& other, const AutoDiffVector& v) + { + return AutoDiffVector< + CwiseUnaryOp, ValueType> + CwiseUnaryOp, JacobianType> >( + v.values() * other, + v.jacobian() * other); + } + +// template +// inline const AutoDiffVector< +// CwiseBinaryOp, ValueType, OtherValueType> +// CwiseBinaryOp, +// NestByValue, JacobianType> >, +// NestByValue, OtherJacobianType> > > > +// operator*(const AutoDiffVector& other) const +// { +// return AutoDiffVector< +// CwiseBinaryOp, ValueType, OtherValueType> +// CwiseBinaryOp, +// NestByValue, JacobianType> >, +// NestByValue, OtherJacobianType> > > >( +// m_values.cwise() * other.values(), +// (m_jacobian * other.values()).nestByValue() + (m_values * other.jacobian()).nestByValue()); +// } + + inline AutoDiffVector& operator*=(const Scalar& other) + { + m_values *= other; + m_jacobian *= other; + return *this; + } + + template + inline AutoDiffVector& operator*=(const AutoDiffVector& other) + { + *this = *this * other; + return *this; + } + + protected: + ValueType m_values; + JacobianType m_jacobian; + +}; + +} + +#endif // EIGEN_AUTODIFF_VECTOR_H diff --git a/unsupported/Eigen/src/CMakeLists.txt b/unsupported/Eigen/src/CMakeLists.txt index 727b18cf5..c2f63db20 100644 --- a/unsupported/Eigen/src/CMakeLists.txt +++ b/unsupported/Eigen/src/CMakeLists.txt @@ -1,2 +1,3 @@ ADD_SUBDIRECTORY(IterativeSolvers) ADD_SUBDIRECTORY(BVH) +ADD_SUBDIRECTORY(AutoDiff) diff --git a/unsupported/test/CMakeLists.txt b/unsupported/test/CMakeLists.txt index 2e4bde3e1..16ad1e7a1 100644 --- a/unsupported/test/CMakeLists.txt +++ b/unsupported/test/CMakeLists.txt @@ -15,4 +15,5 @@ else(ADOLC_FOUND) ei_add_property(EIGEN_MISSING_BACKENDS "Adolc") endif(ADOLC_FOUND) +ei_add_test(autodiff) ei_add_test(BVH) diff --git a/unsupported/test/autodiff.cpp b/unsupported/test/autodiff.cpp new file mode 100644 index 000000000..7d619897c --- /dev/null +++ b/unsupported/test/autodiff.cpp @@ -0,0 +1,156 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2009 Gael Guennebaud +// +// 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 . + +#include "main.h" +#include + +template +EIGEN_DONT_INLINE Scalar foo(const Scalar& x, const Scalar& y) +{ +// return x+std::sin(y); + asm("#mybegin"); + return x*2 - std::pow(x,2) + 2*std::sqrt(y*y) - 4 * std::sin(x) + 2 * std::cos(y) - std::exp(-0.5*x*x); +// return y/x;// - y*2; + asm("#myend"); +} + +template +struct TestFunc1 +{ + typedef _Scalar Scalar; + enum { + InputsAtCompileTime = NX, + ValuesAtCompileTime = NY + }; + typedef Matrix InputType; + typedef Matrix ValueType; + typedef Matrix JacobianType; + + int m_inputs, m_values; + + TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} + TestFunc1(int inputs, int values) : m_inputs(inputs), m_values(values) {} + + int inputs() const { return m_inputs; } + int values() const { return m_values; } + + template + void operator() (const Matrix& x, Matrix* _v) const + { + Matrix& v = *_v; + + v[0] = 2 * x[0] * x[0] + x[0] * x[1]; + v[1] = 3 * x[1] * x[0] + 0.5 * x[1] * x[1]; + if(inputs()>2) + { + v[0] += 0.5 * x[2]; + v[1] += x[2]; + } + if(values()>2) + { + v[2] = 3 * x[1] * x[0] * x[0]; + } + if (inputs()>2 && values()>2) + v[2] *= x[2]; + } + + void operator() (const InputType& x, ValueType* v, JacobianType* _j) const + { + (*this)(x, v); + + if(_j) + { + JacobianType& j = *_j; + + j(0,0) = 4 * x[0] + x[1]; + j(1,0) = 3 * x[1]; + + j(0,1) = x[0]; + j(1,1) = 3 * x[0] + 2 * 0.5 * x[1]; + + if (inputs()>2) + { + j(0,2) = 0.5; + j(1,2) = 1; + } + if(values()>2) + { + j(2,0) = 3 * x[1] * 2 * x[0]; + j(2,1) = 3 * x[0] * x[0]; + } + if (inputs()>2 && values()>2) + { + j(2,0) *= x[2]; + j(2,1) *= x[2]; + + j(2,2) = 3 * x[1] * x[0] * x[0]; + j(2,2) = 3 * x[1] * x[0] * x[0]; + } + } + } +}; + +template void adolc_forward_jacobian(const Func& f) +{ + typename Func::InputType x = Func::InputType::Random(f.inputs()); + typename Func::ValueType y(f.values()), yref(f.values()); + typename Func::JacobianType j(f.values(),f.inputs()), jref(f.values(),f.inputs()); + + jref.setZero(); + yref.setZero(); + f(x,&yref,&jref); +// std::cerr << y.transpose() << "\n\n";; +// std::cerr << j << "\n\n";; + + j.setZero(); + y.setZero(); + AutoDiffJacobian autoj(f); + autoj(x, &y, &j); +// std::cerr << y.transpose() << "\n\n";; +// std::cerr << j << "\n\n";; + + VERIFY_IS_APPROX(y, yref); + VERIFY_IS_APPROX(j, jref); +} + +void test_autodiff() +{ + std::sqrt(3); + std::sin(3); + std::cerr << foo(1,2) << "\n"; + AutoDiffScalar ax(1,Vector2f::UnitX()); + AutoDiffScalar ay(2,Vector2f::UnitY()); + std::cerr << foo >(ax,ay).value() << " <> " + << foo >(ax,ay).derivatives().transpose() << "\n\n"; + + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1(3,3)) )); + } + +// exit(1); +} diff --git a/unsupported/test/forward_adolc.cpp b/unsupported/test/forward_adolc.cpp index 016e20cdb..aa7618bff 100644 --- a/unsupported/test/forward_adolc.cpp +++ b/unsupported/test/forward_adolc.cpp @@ -28,7 +28,7 @@ int adtl::ADOLC_numDir; -template +template struct TestFunc1 { typedef _Scalar Scalar; @@ -39,6 +39,14 @@ struct TestFunc1 typedef Matrix InputType; typedef Matrix ValueType; typedef Matrix JacobianType; + + int m_inputs, m_values; + + TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} + TestFunc1(int inputs, int values) : m_inputs(inputs), m_values(values) {} + + int inputs() const { return m_inputs; } + int values() const { return m_values } template void operator() (const Matrix& x, Matrix* _v) const @@ -47,16 +55,16 @@ struct TestFunc1 v[0] = 2 * x[0] * x[0] + x[0] * x[1]; v[1] = 3 * x[1] * x[0] + 0.5 * x[1] * x[1]; - if(NX>2) + if(inputs()>2) { v[0] += 0.5 * x[2]; v[1] += x[2]; } - if(NY>2) + if(values()>2) { v[2] = 3 * x[1] * x[0] * x[0]; } - if (NX>2 && NY>2) + if (inputs()>2 && values()>2) v[2] *= x[2]; } @@ -74,17 +82,17 @@ struct TestFunc1 j(0,1) = x[0]; j(1,1) = 3 * x[0] + 2 * 0.5 * x[1]; - if (NX>2) + if (inputs()>2) { j(0,2) = 0.5; j(1,2) = 1; } - if(NY>2) + if(values()>2) { j(2,0) = 3 * x[1] * 2 * x[0]; j(2,1) = 3 * x[0] * x[0]; } - if (NX>2 && NY>2) + if (inputs()>2 && values()>2) { j(2,0) *= x[2]; j(2,1) *= x[2]; @@ -128,5 +136,6 @@ void test_forward_adolc() CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1()) )); CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1()) )); CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( adolc_forward_jacobian(TestFunc1(3,3)) )); } }