diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h index 1a61e3367..33b6c393f 100644 --- a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h @@ -20,37 +20,60 @@ public: AutoDiffJacobian(const Functor& f) : Functor(f) {} // forward constructors +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + AutoDiffJacobian(const T& ...Values) : Functor(Values...) {} +#else 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 T2& a2) : Functor(a0, a1, a2) {} - - enum { - InputsAtCompileTime = Functor::InputsAtCompileTime, - ValuesAtCompileTime = Functor::ValuesAtCompileTime - }; +#endif typedef typename Functor::InputType InputType; typedef typename Functor::ValueType ValueType; - typedef typename Functor::JacobianType JacobianType; - typedef typename JacobianType::Scalar Scalar; + typedef typename ValueType::Scalar Scalar; + + enum { + InputsAtCompileTime = InputType::RowsAtCompileTime, + ValuesAtCompileTime = ValueType::RowsAtCompileTime + }; + + typedef Matrix JacobianType; typedef typename JacobianType::Index Index; - typedef Matrix DerivativeType; + typedef Matrix DerivativeType; typedef AutoDiffScalar ActiveScalar; - typedef Matrix ActiveInput; typedef Matrix ActiveValue; +#if EIGEN_HAS_VARIADIC_TEMPLATES + // Some compilers don't accept variadic parameters after a default parameter, + // i.e., we can't just write _jac=0 but we need to overload operator(): + EIGEN_STRONG_INLINE + void operator() (const InputType& x, ValueType* v) const + { + this->operator()(x, v, 0); + } + template + void operator() (const InputType& x, ValueType* v, JacobianType* _jac, + const ParamsType&... Params) const +#else void operator() (const InputType& x, ValueType* v, JacobianType* _jac=0) const +#endif { eigen_assert(v!=0); + if (!_jac) { +#if EIGEN_HAS_VARIADIC_TEMPLATES + Functor::operator()(x, v, Params...); +#else Functor::operator()(x, v); +#endif return; } @@ -61,12 +84,16 @@ public: if(InputsAtCompileTime==Dynamic) for (Index j=0; jinputs()); + av[j].derivatives().resize(x.rows()); for (Index i=0; iinputs(),i); + ax[i].derivatives() = DerivativeType::Unit(x.rows(),i); +#if EIGEN_HAS_VARIADIC_TEMPLATES + Functor::operator()(ax, &av, Params...); +#else Functor::operator()(ax, &av); +#endif for (Index i=0; i +struct integratorFunctor +{ + typedef Matrix InputType; + typedef Matrix ValueType; + + /* + * Implementation starts here. + */ + integratorFunctor(const Scalar gain) : _gain(gain) {} + integratorFunctor(const integratorFunctor& f) : _gain(f._gain) {} + const Scalar _gain; + + template + void operator() (const T1 &input, T2 *output, const Scalar dt) const + { + T2 &o = *output; + + /* Integrator to test the AD. */ + o[0] = input[0] + input[1] * dt * _gain; + o[1] = input[1] * _gain; + } + + /* Only needed for the test */ + template + void operator() (const T1 &input, T2 *output, T3 *jacobian, const Scalar dt) const + { + T2 &o = *output; + + /* Integrator to test the AD. */ + o[0] = input[0] + input[1] * dt * _gain; + o[1] = input[1] * _gain; + + if (jacobian) + { + T3 &j = *jacobian; + + j(0, 0) = 1; + j(0, 1) = dt * _gain; + j(1, 0) = 0; + j(1, 1) = _gain; + } + } + +}; + +template void forward_jacobian_cpp11(const Func& f) +{ + typedef typename Func::ValueType::Scalar Scalar; + typedef typename Func::ValueType ValueType; + typedef typename Func::InputType InputType; + typedef typename AutoDiffJacobian::JacobianType JacobianType; + + InputType x = InputType::Random(InputType::RowsAtCompileTime); + ValueType y, yref; + JacobianType j, jref; + + const Scalar dt = internal::random(); + + jref.setZero(); + yref.setZero(); + f(x, &yref, &jref, dt); + + //std::cerr << "y, yref, jref: " << "\n"; + //std::cerr << y.transpose() << "\n\n"; + //std::cerr << yref << "\n\n"; + //std::cerr << jref << "\n\n"; + + AutoDiffJacobian autoj(f); + autoj(x, &y, &j, dt); + + //std::cerr << "y j (via autodiff): " << "\n"; + //std::cerr << y.transpose() << "\n\n"; + //std::cerr << j << "\n\n"; + + VERIFY_IS_APPROX(y, yref); + VERIFY_IS_APPROX(j, jref); +} +#endif + template void forward_jacobian(const Func& f) { typename Func::InputType x = Func::InputType::Random(f.inputs()); @@ -128,7 +211,6 @@ template void forward_jacobian(const Func& f) VERIFY_IS_APPROX(j, jref); } - // TODO also check actual derivatives! template void test_autodiff_scalar() @@ -141,6 +223,7 @@ void test_autodiff_scalar() VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y())); } + // TODO also check actual derivatives! template void test_autodiff_vector() @@ -151,7 +234,7 @@ void test_autodiff_vector() VectorAD ap = p.cast(); ap.x().derivatives() = Vector2f::UnitX(); ap.y().derivatives() = Vector2f::UnitY(); - + AD res = foo(ap); VERIFY_IS_APPROX(res.value(), foo(p)); } @@ -164,6 +247,9 @@ void test_autodiff_jacobian() CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); CALL_SUBTEST(( forward_jacobian(TestFunc1(3,3)) )); +#if EIGEN_HAS_VARIADIC_TEMPLATES + CALL_SUBTEST(( forward_jacobian_cpp11(integratorFunctor(10)) )); +#endif }