mirror of
https://gitlab.com/libeigen/eigen.git
synced 2024-12-21 07:19:46 +08:00
282 lines
7.9 KiB
C++
282 lines
7.9 KiB
C++
// This file is part of Eigen, a lightweight C++ template library
|
|
// for linear algebra.
|
|
//
|
|
// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
|
|
//
|
|
// This Source Code Form is subject to the terms of the Mozilla
|
|
// Public License v. 2.0. If a copy of the MPL was not distributed
|
|
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
|
|
#include "main.h"
|
|
#include <unsupported/Eigen/AutoDiff>
|
|
|
|
template<typename Scalar>
|
|
EIGEN_DONT_INLINE Scalar foo(const Scalar& x, const Scalar& y)
|
|
{
|
|
using namespace std;
|
|
// return x+std::sin(y);
|
|
EIGEN_ASM_COMMENT("mybegin");
|
|
// pow(float, int) promotes to pow(double, double)
|
|
return x*2 - 1 + static_cast<Scalar>(pow(1+x,2)) + 2*sqrt(y*y+0) - 4 * sin(0+x) + 2 * cos(y+0) - exp(Scalar(-0.5)*x*x+0);
|
|
//return x+2*y*x;//x*2 -std::pow(x,2);//(2*y/x);// - y*2;
|
|
EIGEN_ASM_COMMENT("myend");
|
|
}
|
|
|
|
template<typename Vector>
|
|
EIGEN_DONT_INLINE typename Vector::Scalar foo(const Vector& p)
|
|
{
|
|
typedef typename Vector::Scalar Scalar;
|
|
return (p-Vector(Scalar(-1),Scalar(1.))).norm() + (p.array() * p.array()).sum() + p.dot(p);
|
|
}
|
|
|
|
template<typename _Scalar, int NX=Dynamic, int NY=Dynamic>
|
|
struct TestFunc1
|
|
{
|
|
typedef _Scalar Scalar;
|
|
enum {
|
|
InputsAtCompileTime = NX,
|
|
ValuesAtCompileTime = NY
|
|
};
|
|
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; }
|
|
|
|
template<typename T>
|
|
void operator() (const Matrix<T,InputsAtCompileTime,1>& x, Matrix<T,ValuesAtCompileTime,1>* _v) const
|
|
{
|
|
Matrix<T,ValuesAtCompileTime,1>& 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<typename Func> void 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<Func> 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);
|
|
}
|
|
|
|
|
|
// TODO also check actual derivatives!
|
|
template <int>
|
|
void test_autodiff_scalar()
|
|
{
|
|
Vector2f p = Vector2f::Random();
|
|
typedef AutoDiffScalar<Vector2f> AD;
|
|
AD ax(p.x(),Vector2f::UnitX());
|
|
AD ay(p.y(),Vector2f::UnitY());
|
|
AD res = foo<AD>(ax,ay);
|
|
VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y()));
|
|
}
|
|
|
|
// TODO also check actual derivatives!
|
|
template <int>
|
|
void test_autodiff_vector()
|
|
{
|
|
Vector2f p = Vector2f::Random();
|
|
typedef AutoDiffScalar<Vector2f> AD;
|
|
typedef Matrix<AD,2,1> VectorAD;
|
|
VectorAD ap = p.cast<AD>();
|
|
ap.x().derivatives() = Vector2f::UnitX();
|
|
ap.y().derivatives() = Vector2f::UnitY();
|
|
|
|
AD res = foo<VectorAD>(ap);
|
|
VERIFY_IS_APPROX(res.value(), foo(p));
|
|
}
|
|
|
|
template <int>
|
|
void test_autodiff_jacobian()
|
|
{
|
|
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) ));
|
|
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) ));
|
|
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) ));
|
|
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) ));
|
|
CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) ));
|
|
}
|
|
|
|
|
|
template <int>
|
|
void test_autodiff_hessian()
|
|
{
|
|
typedef AutoDiffScalar<VectorXd> AD;
|
|
typedef Matrix<AD,Eigen::Dynamic,1> VectorAD;
|
|
typedef AutoDiffScalar<VectorAD> ADD;
|
|
typedef Matrix<ADD,Eigen::Dynamic,1> VectorADD;
|
|
VectorADD x(2);
|
|
double s1 = internal::random<double>(), s2 = internal::random<double>(), s3 = internal::random<double>(), s4 = internal::random<double>();
|
|
x(0).value()=s1;
|
|
x(1).value()=s2;
|
|
|
|
//set unit vectors for the derivative directions (partial derivatives of the input vector)
|
|
x(0).derivatives().resize(2);
|
|
x(0).derivatives().setZero();
|
|
x(0).derivatives()(0)= 1;
|
|
x(1).derivatives().resize(2);
|
|
x(1).derivatives().setZero();
|
|
x(1).derivatives()(1)=1;
|
|
|
|
//repeat partial derivatives for the inner AutoDiffScalar
|
|
x(0).value().derivatives() = VectorXd::Unit(2,0);
|
|
x(1).value().derivatives() = VectorXd::Unit(2,1);
|
|
|
|
//set the hessian matrix to zero
|
|
for(int idx=0; idx<2; idx++) {
|
|
x(0).derivatives()(idx).derivatives() = VectorXd::Zero(2);
|
|
x(1).derivatives()(idx).derivatives() = VectorXd::Zero(2);
|
|
}
|
|
|
|
ADD y = sin(AD(s3)*x(0) + AD(s4)*x(1));
|
|
|
|
VERIFY_IS_APPROX(y.value().derivatives()(0), y.derivatives()(0).value());
|
|
VERIFY_IS_APPROX(y.value().derivatives()(1), y.derivatives()(1).value());
|
|
VERIFY_IS_APPROX(y.value().derivatives()(0), s3*std::cos(s1*s3+s2*s4));
|
|
VERIFY_IS_APPROX(y.value().derivatives()(1), s4*std::cos(s1*s3+s2*s4));
|
|
VERIFY_IS_APPROX(y.derivatives()(0).derivatives(), -std::sin(s1*s3+s2*s4)*Vector2d(s3*s3,s4*s3));
|
|
VERIFY_IS_APPROX(y.derivatives()(1).derivatives(), -std::sin(s1*s3+s2*s4)*Vector2d(s3*s4,s4*s4));
|
|
|
|
ADD z = x(0)*x(1);
|
|
VERIFY_IS_APPROX(z.derivatives()(0).derivatives(), Vector2d(0,1));
|
|
VERIFY_IS_APPROX(z.derivatives()(1).derivatives(), Vector2d(1,0));
|
|
}
|
|
|
|
double bug_1222() {
|
|
typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD;
|
|
const double _cv1_3 = 1.0;
|
|
const AD chi_3 = 1.0;
|
|
// this line did not work, because operator+ returns ADS<DerType&>, which then cannot be converted to ADS<DerType>
|
|
const AD denom = chi_3 + _cv1_3;
|
|
return denom.value();
|
|
}
|
|
|
|
double bug_1223() {
|
|
using std::min;
|
|
typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD;
|
|
|
|
const double _cv1_3 = 1.0;
|
|
const AD chi_3 = 1.0;
|
|
const AD denom = 1.0;
|
|
|
|
// failed because implementation of min attempts to construct ADS<DerType&> via constructor AutoDiffScalar(const Real& value)
|
|
// without initializing m_derivatives (which is a reference in this case)
|
|
#define EIGEN_TEST_SPACE
|
|
const AD t = min EIGEN_TEST_SPACE (denom / chi_3, 1.0);
|
|
|
|
const AD t2 = min EIGEN_TEST_SPACE (denom / (chi_3 * _cv1_3), 1.0);
|
|
|
|
return t.value() + t2.value();
|
|
}
|
|
|
|
// regression test for some compilation issues with specializations of ScalarBinaryOpTraits
|
|
void bug_1260() {
|
|
Matrix4d A;
|
|
Vector4d v;
|
|
A*v;
|
|
}
|
|
|
|
// check a compilation issue with numext::max
|
|
double bug_1261() {
|
|
typedef AutoDiffScalar<Matrix2d> AD;
|
|
typedef Matrix<AD,2,1> VectorAD;
|
|
|
|
VectorAD v;
|
|
const AD maxVal = v.maxCoeff();
|
|
const AD minVal = v.minCoeff();
|
|
return maxVal.value() + minVal.value();
|
|
}
|
|
|
|
double bug_1264() {
|
|
typedef AutoDiffScalar<Vector2d> AD;
|
|
const AD s;
|
|
const Matrix<AD, 3, 1> v1;
|
|
const Matrix<AD, 3, 1> v2 = (s + 3.0) * v1;
|
|
return v2(0).value();
|
|
}
|
|
|
|
void test_autodiff()
|
|
{
|
|
for(int i = 0; i < g_repeat; i++) {
|
|
CALL_SUBTEST_1( test_autodiff_scalar<1>() );
|
|
CALL_SUBTEST_2( test_autodiff_vector<1>() );
|
|
CALL_SUBTEST_3( test_autodiff_jacobian<1>() );
|
|
CALL_SUBTEST_4( test_autodiff_hessian<1>() );
|
|
}
|
|
|
|
bug_1222();
|
|
bug_1223();
|
|
bug_1260();
|
|
bug_1261();
|
|
}
|
|
|