diff options
| -rw-r--r-- | unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h | 23 | ||||
| -rw-r--r-- | unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h | 132 | ||||
| -rw-r--r-- | unsupported/test/autodiff.cpp | 8 |
3 files changed, 110 insertions, 53 deletions
diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h index d42197345..a5e881487 100644 --- a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h @@ -46,13 +46,14 @@ public: InputsAtCompileTime = Functor::InputsAtCompileTime, ValuesAtCompileTime = Functor::ValuesAtCompileTime }; - + typedef typename Functor::InputType InputType; typedef typename Functor::ValueType ValueType; typedef typename Functor::JacobianType JacobianType; - typedef AutoDiffScalar<Matrix<double,InputsAtCompileTime,1> > ActiveScalar; - + typedef Matrix<double,InputsAtCompileTime,1> DerivativeType; + typedef AutoDiffScalar<DerivativeType> ActiveScalar; + typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput; typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> ActiveValue; @@ -69,26 +70,20 @@ public: ActiveInput ax = x.template cast<ActiveScalar>(); ActiveValue av(jac.rows()); - + if(InputsAtCompileTime==Dynamic) - { - for (int j=0; j<jac.cols(); j++) - ax[j].derivatives().resize(this->inputs()); for (int j=0; j<jac.rows(); j++) av[j].derivatives().resize(this->inputs()); - } - - for (int j=0; j<jac.cols(); j++) - for (int i=0; i<jac.cols(); i++) - ax[i].derivatives().coeffRef(j) = i==j ? 1 : 0; + + for (int i=0; i<jac.cols(); i++) + ax[i].derivatives() = DerivativeType::Unit(this->inputs(),i); Functor::operator()(ax, &av); for (int i=0; i<jac.rows(); i++) { (*v)[i] = av[i].value(); - for (int j=0; j<jac.cols(); j++) - jac.coeffRef(i,j) = av[i].derivatives().coeff(j); + jac.row(i) = av[i].derivatives(); } } protected: diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h index f82e5e7c6..888aa5c8c 100644 --- a/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h @@ -27,6 +27,18 @@ namespace Eigen { +template<typename A, typename B> +struct ei_make_coherent_impl { + static void run(A& a, B& b) {} +}; + +// resize a to match b is a.size()==0, and conversely. +template<typename A, typename B> +void ei_make_coherent(const A& a, const B&b) +{ + ei_make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived()); +} + /** \class AutoDiffScalar * \brief A scalar type replacement with automatic differentation capability * @@ -35,7 +47,7 @@ namespace Eigen { * 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, + * - 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. * @@ -49,29 +61,29 @@ class AutoDiffScalar { public: typedef typename ei_traits<DerType>::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<typename OtherDerType> inline AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other) : m_value(other.value()), m_derivatives(other.derivatives()) {} - + inline AutoDiffScalar(const AutoDiffScalar& other) : m_value(other.value()), m_derivatives(other.derivatives()) {} - + template<typename OtherDerType> inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other) { @@ -79,32 +91,33 @@ class AutoDiffScalar 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<typename OtherDerType> inline const AutoDiffScalar<CwiseBinaryOp<ei_scalar_sum_op<Scalar>,DerType,OtherDerType> > operator+(const AutoDiffScalar<OtherDerType>& other) const { + ei_make_coherent(m_derivatives, other.derivatives()); return AutoDiffScalar<CwiseBinaryOp<ei_scalar_sum_op<Scalar>,DerType,OtherDerType> >( m_value + other.value(), m_derivatives + other.derivatives()); } - + template<typename OtherDerType> inline AutoDiffScalar& operator+=(const AutoDiffScalar<OtherDerType>& other) @@ -112,16 +125,17 @@ class AutoDiffScalar (*this) = (*this) + other; return *this; } - + template<typename OtherDerType> inline const AutoDiffScalar<CwiseBinaryOp<ei_scalar_difference_op<Scalar>, DerType,OtherDerType> > operator-(const AutoDiffScalar<OtherDerType>& other) const { + ei_make_coherent(m_derivatives, other.derivatives()); return AutoDiffScalar<CwiseBinaryOp<ei_scalar_difference_op<Scalar>, DerType,OtherDerType> >( m_value - other.value(), m_derivatives - other.derivatives()); } - + template<typename OtherDerType> inline AutoDiffScalar& operator-=(const AutoDiffScalar<OtherDerType>& other) @@ -129,7 +143,7 @@ class AutoDiffScalar *this = *this - other; return *this; } - + template<typename OtherDerType> inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_opposite_op<Scalar>, DerType> > operator-() const @@ -138,7 +152,7 @@ class AutoDiffScalar -m_value, -m_derivatives); } - + inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > operator*(const Scalar& other) const { @@ -146,7 +160,7 @@ class AutoDiffScalar m_value * other, (m_derivatives * other)); } - + friend inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > operator*(const Scalar& other, const AutoDiffScalar& a) { @@ -154,7 +168,7 @@ class AutoDiffScalar a.value() * other, a.derivatives() * other); } - + inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > operator/(const Scalar& other) const { @@ -162,7 +176,7 @@ class AutoDiffScalar m_value / other, (m_derivatives * (Scalar(1)/other))); } - + friend inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > operator/(const Scalar& other, const AutoDiffScalar& a) { @@ -170,7 +184,7 @@ class AutoDiffScalar other / a.value(), a.derivatives() * (-Scalar(1)/other)); } - + template<typename OtherDerType> inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, NestByValue<CwiseBinaryOp<ei_scalar_difference_op<Scalar>, @@ -178,6 +192,7 @@ class AutoDiffScalar NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, OtherDerType> > > > > > operator/(const AutoDiffScalar<OtherDerType>& other) const { + ei_make_coherent(m_derivatives, other.derivatives()); return AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, NestByValue<CwiseBinaryOp<ei_scalar_difference_op<Scalar>, NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >, @@ -186,45 +201,91 @@ class AutoDiffScalar ((m_derivatives * other.value()).nestByValue() - (m_value * other.derivatives()).nestByValue()).nestByValue() * (Scalar(1)/(other.value()*other.value()))); } - + template<typename OtherDerType> inline const AutoDiffScalar<CwiseBinaryOp<ei_scalar_sum_op<Scalar>, NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >, NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, OtherDerType> > > > operator*(const AutoDiffScalar<OtherDerType>& other) const { + ei_make_coherent(m_derivatives, other.derivatives()); return AutoDiffScalar<CwiseBinaryOp<ei_scalar_sum_op<Scalar>, NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >, NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, 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<typename OtherDerType> inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other) { *this = *this * other; return *this; } - + protected: Scalar m_value; DerType m_derivatives; - + +}; + +template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B> +struct ei_make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> { + typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A; + static void run(A& a, B& b) { + if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0)) + { + a.resize(b.size()); + a.setZero(); + } + } +}; + +template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols> +struct ei_make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > { + typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B; + static void run(A& a, B& b) { + if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0)) + { + b.resize(a.size()); + b.setZero(); + } + } +}; + +template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, + typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols> +struct ei_make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, + Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > { + typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A; + typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B; + static void run(A& a, B& b) { + if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0)) + { + a.resize(b.size()); + a.setZero(); + } + else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0)) + { + b.resize(a.size()); + b.setZero(); + } + } }; } #define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \ template<typename DerType> \ - inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<DerType>::Scalar>, DerType> > \ - FUNC(const AutoDiffScalar<DerType>& x) { \ + inline const Eigen::AutoDiffScalar<Eigen::CwiseUnaryOp<Eigen::ei_scalar_multiple_op<typename Eigen::ei_traits<DerType>::Scalar>, DerType> > \ + FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \ + using namespace Eigen; \ typedef typename ei_traits<DerType>::Scalar Scalar; \ typedef AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > ReturnType; \ CODE; \ @@ -234,34 +295,35 @@ 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<typename DerType> - inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<DerType>::Scalar>, DerType> > - pow(const AutoDiffScalar<DerType>& x, typename ei_traits<DerType>::Scalar y) + inline const Eigen::AutoDiffScalar<Eigen::CwiseUnaryOp<Eigen::ei_scalar_multiple_op<typename Eigen::ei_traits<DerType>::Scalar>, DerType> > + pow(const Eigen::AutoDiffScalar<DerType>& x, typename Eigen::ei_traits<DerType>::Scalar y) { + using namespace Eigen; typedef typename ei_traits<DerType>::Scalar Scalar; return AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >( std::pow(x.value(),y), x.derivatives() * (y * std::pow(x.value(),y-1))); } - + } namespace Eigen { diff --git a/unsupported/test/autodiff.cpp b/unsupported/test/autodiff.cpp index b1164897c..a96927b41 100644 --- a/unsupported/test/autodiff.cpp +++ b/unsupported/test/autodiff.cpp @@ -46,12 +46,12 @@ struct TestFunc1 typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> 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; } @@ -142,7 +142,7 @@ void test_autodiff_scalar() std::cerr << foo<AutoDiffScalar<Vector2f> >(ax,ay).value() << " <> " << foo<AutoDiffScalar<Vector2f> >(ax,ay).derivatives().transpose() << "\n\n"; } - + void test_autodiff_jacobian() { for(int i = 0; i < g_repeat; i++) { |