mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-01-24 14:45:14 +08:00
autodiff:
* fix namespace issue * simplify Jacobian code * fix issue with "Dynamic derivatives"
This commit is contained in:
Gael Guennebaud
parent
0927ba1fd3
commit
1503043981
@ -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:
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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++) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user