improve a bit AutoDiffVector, but it still not working
diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h index 69ea914..03c82b7 100644 --- a/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
@@ -35,7 +35,7 @@ * 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. * @@ -48,130 +48,150 @@ class AutoDiffVector { public: - typedef typename ei_traits<ValueType>::Scalar Scalar; - + //typedef typename ei_traits<ValueType>::Scalar Scalar; + typedef typename ei_traits<ValueType>::Scalar BaseScalar; + typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar; + typedef ActiveScalar Scalar; + typedef AutoDiffScalar<typename JacobianType::ColXpr> CoeffType; + inline AutoDiffVector() {} - + inline AutoDiffVector(const ValueType& values) : m_values(values) { m_jacobian.setZero(); } - + + + CoeffType operator[] (int i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType operator[] (int i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + CoeffType operator() (int i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType operator() (int i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + CoeffType coeffRef(int i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType coeffRef(int i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + int size() const { return m_values.size(); } + + // FIXME here we could return an expression of the sum + Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); } + + inline AutoDiffVector(const ValueType& values, const JacobianType& jac) : m_values(values), m_jacobian(jac) {} - + template<typename OtherValueType, typename OtherJacobianType> inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other) : m_values(other.values()), m_jacobian(other.jacobian()) {} - + inline AutoDiffVector(const AutoDiffVector& other) : m_values(other.values()), m_jacobian(other.jacobian()) {} - + template<typename OtherValueType, typename OtherJacobianType> - inline AutoDiffScalar& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& other) + inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& 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<typename OtherValueType,typename OtherJacobianType> inline const AutoDiffVector< - CwiseBinaryOp<ei_scalar_sum_op<Scalar>,ValueType,OtherValueType> > - CwiseBinaryOp<ei_scalar_sum_op<Scalar>,JacobianType,OtherJacobianType> > - operator+(const AutoDiffScalar<OtherDerType>& other) const + typename MakeCwiseBinaryOp<ei_scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp<ei_scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type > + operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const { return AutoDiffVector< - CwiseBinaryOp<ei_scalar_sum_op<Scalar>,ValueType,OtherValueType> > - CwiseBinaryOp<ei_scalar_sum_op<Scalar>,JacobianType,OtherJacobianType> >( + typename MakeCwiseBinaryOp<ei_scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp<ei_scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >( m_values + other.values(), m_jacobian + other.jacobian()); } - + template<typename OtherValueType, typename OtherJacobianType> inline AutoDiffVector& - operator+=(const AutoDiffVector<OtherValueType,OtherDerType>& other) + operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) { m_values += other.values(); m_jacobian += other.jacobian(); return *this; } - + template<typename OtherValueType,typename OtherJacobianType> inline const AutoDiffVector< - CwiseBinaryOp<ei_scalar_difference_op<Scalar>,ValueType,OtherValueType> > - CwiseBinaryOp<ei_scalar_difference_op<Scalar>,JacobianType,OtherJacobianType> > - operator-(const AutoDiffScalar<OtherDerType>& other) const + typename MakeCwiseBinaryOp<ei_scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp<ei_scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type > + operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const { return AutoDiffVector< - CwiseBinaryOp<ei_scalar_difference_op<Scalar>,ValueType,OtherValueType> > - CwiseBinaryOp<ei_scalar_difference_op<Scalar>,JacobianType,OtherJacobianType> >( - m_values - other.values(), - m_jacobian - other.jacobian()); + typename MakeCwiseBinaryOp<ei_scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp<ei_scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >( + m_values - other.values(), + m_jacobian - other.jacobian()); } - + template<typename OtherValueType, typename OtherJacobianType> inline AutoDiffVector& - operator-=(const AutoDiffVector<OtherValueType,OtherDerType>& other) + operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) { m_values -= other.values(); m_jacobian -= other.jacobian(); return *this; } - + inline const AutoDiffVector< - CwiseUnaryOp<ei_scalar_opposite_op<Scalar>, ValueType> - CwiseUnaryOp<ei_scalar_opposite_op<Scalar>, JacobianType> > + typename MakeCwiseUnaryOp<ei_scalar_opposite_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<ei_scalar_opposite_op<Scalar>, JacobianType>::Type > operator-() const { return AutoDiffVector< - CwiseUnaryOp<ei_scalar_opposite_op<Scalar>, ValueType> - CwiseUnaryOp<ei_scalar_opposite_op<Scalar>, JacobianType> >( - -m_values, - -m_jacobian); + typename MakeCwiseUnaryOp<ei_scalar_opposite_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<ei_scalar_opposite_op<Scalar>, JacobianType>::Type >( + -m_values, + -m_jacobian); } - + inline const AutoDiffVector< - CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, ValueType> - CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, JacobianType> > - operator*(const Scalar& other) const + typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, JacobianType>::Type> + operator*(const BaseScalar& other) const { return AutoDiffVector< - CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, ValueType> - CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, JacobianType> >( + typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, JacobianType>::Type >( m_values * other, - (m_jacobian * other)); + m_jacobian * other); } - + friend inline const AutoDiffVector< - CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, ValueType> - CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, JacobianType> > + typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, JacobianType>::Type > operator*(const Scalar& other, const AutoDiffVector& v) { return AutoDiffVector< - CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, ValueType> - CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, JacobianType> >( + typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, JacobianType>::Type >( v.values() * other, v.jacobian() * other); } - + // template<typename OtherValueType,typename OtherJacobianType> // inline const AutoDiffVector< // CwiseBinaryOp<ei_scalar_multiple_op<Scalar>, ValueType, OtherValueType> @@ -188,25 +208,25 @@ // 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<typename OtherValueType,typename OtherJacobianType> inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) { *this = *this * other; return *this; } - + protected: ValueType m_values; JacobianType m_jacobian; - + }; }