11 #include <unsupported/Eigen/AutoDiff> 13 template<
typename Scalar>
25 template<
typename Vector>
32 template<
typename _Scalar,
int NX=Dynamic,
int NY=Dynamic>
37 InputsAtCompileTime = NX,
38 ValuesAtCompileTime = NY
46 TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
49 int inputs()
const {
return m_inputs; }
50 int values()
const {
return m_values; }
57 v[0] = 2 * x[0] * x[0] + x[0] * x[1];
58 v[1] = 3 * x[1] * x[0] + 0.5 * x[1] * x[1];
66 v[2] = 3 * x[1] * x[0] * x[0];
68 if (inputs()>2 &&
values()>2)
72 void operator() (
const InputType&
x, ValueType*
v, JacobianType* _j)
const 78 JacobianType&
j = *_j;
80 j(0,0) = 4 * x[0] + x[1];
84 j(1,1) = 3 * x[0] + 2 * 0.5 * x[1];
93 j(2,0) = 3 * x[1] * 2 * x[0];
94 j(2,1) = 3 * x[0] * x[0];
96 if (inputs()>2 &&
values()>2)
101 j(2,2) = 3 * x[1] * x[0] * x[0];
102 j(2,2) = 3 * x[1] * x[0] * x[0];
109 #if EIGEN_HAS_VARIADIC_TEMPLATES 111 template <
typename Scalar>
112 struct integratorFunctor
120 integratorFunctor(
const Scalar gain) : _gain(gain) {}
121 integratorFunctor(
const integratorFunctor&
f) : _gain(f._gain) {}
124 template <
typename T1,
typename T2>
125 void operator() (
const T1 &input,
T2 *output,
const Scalar dt)
const 130 o[0] = input[0] + input[1] * dt * _gain;
131 o[1] = input[1] * _gain;
135 template <
typename T1,
typename T2,
typename T3>
136 void operator() (
const T1 &input,
T2 *output,
T3 *jacobian,
const Scalar dt)
const 141 o[0] = input[0] + input[1] * dt * _gain;
142 o[1] = input[1] * _gain;
149 j(0, 1) = dt * _gain;
157 template<
typename Func>
void forward_jacobian_cpp11(
const Func&
f)
160 typedef typename Func::ValueType
ValueType;
161 typedef typename Func::InputType
InputType;
164 InputType
x = InputType::Random(InputType::RowsAtCompileTime);
166 JacobianType
j, jref;
168 const Scalar
dt = internal::random<double>();
172 f(x, &yref, &jref, dt);
180 autoj(x, &y, &j, dt);
193 typename Func::InputType
x = Func::InputType::Random(f.inputs());
194 typename Func::ValueType
y(f.values()), yref(f.values());
195 typename Func::JacobianType
j(f.values(),f.inputs()), jref(f.values(),f.inputs());
218 Vector2f
p = Vector2f::Random();
220 AD ax(p.x(),Vector2f::UnitX());
221 AD ay(p.y(),Vector2f::UnitY());
222 AD
res = foo<AD>(ax,ay);
231 Vector2f
p = Vector2f::Random();
234 VectorAD ap = p.cast<AD>();
235 ap.x().derivatives() = Vector2f::UnitX();
236 ap.y().derivatives() = Vector2f::UnitY();
238 AD
res = foo<VectorAD>(ap);
250 #if EIGEN_HAS_VARIADIC_TEMPLATES 251 CALL_SUBTEST(( forward_jacobian_cpp11(integratorFunctor<double>(10)) ));
264 double s1 = internal::random<double>(), s2 = internal::random<double>(), s3 = internal::random<double>(), s4 = internal::random<double>();
269 x(0).derivatives().resize(2);
270 x(0).derivatives().setZero();
271 x(0).derivatives()(0)= 1;
272 x(1).derivatives().resize(2);
273 x(1).derivatives().setZero();
274 x(1).derivatives()(1)=1;
277 x(0).value().derivatives() = VectorXd::Unit(2,0);
278 x(1).value().derivatives() = VectorXd::Unit(2,1);
281 for(
int idx=0; idx<2; idx++) {
282 x(0).derivatives()(idx).derivatives() = VectorXd::Zero(2);
283 x(1).derivatives()(idx).derivatives() = VectorXd::Zero(2);
286 ADD
y =
sin(AD(s3)*
x(0) + AD(s4)*
x(1));
302 const double _cv1_3 = 1.0;
303 const AD chi_3 = 1.0;
305 const AD denom = chi_3 + _cv1_3;
306 return denom.
value();
309 #ifdef EIGEN_TEST_PART_5 315 const double _cv1_3 = 1.0;
316 const AD chi_3 = 1.0;
317 const AD denom = 1.0;
321 #define EIGEN_TEST_SPACE 322 const AD
t =
min EIGEN_TEST_SPACE (denom / chi_3, 1.0);
324 const AD t2 =
min EIGEN_TEST_SPACE (denom / (chi_3 * _cv1_3), 1.0);
326 return t.value() + t2.value();
331 Matrix4d
A = Matrix4d::Ones();
332 Vector4d
v = Vector4d::Ones();
342 const AD maxVal = v.maxCoeff();
343 const AD minVal = v.minCoeff();
344 return maxVal.value() + minVal.value();
352 return v2(0).value();
360 CALL_SUBTEST_1( test_autodiff_scalar<1>() );
361 CALL_SUBTEST_2( test_autodiff_vector<1>() );
362 CALL_SUBTEST_3( test_autodiff_jacobian<1>() );
363 CALL_SUBTEST_4( test_autodiff_hessian<1>() );
367 CALL_SUBTEST_5( bug_1223() );
368 CALL_SUBTEST_5( bug_1260() );
369 CALL_SUBTEST_5( bug_1261() );
Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3))
A scalar type replacement with automatic differentation capability.
void test_autodiff_hessian()
EIGEN_DEVICE_FUNC const ExpReturnType exp() const
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Pose2 T2(M_PI/2.0, Point2(0.0, 2.0))
#define EIGEN_ASM_COMMENT(X)
void test_autodiff_scalar()
EIGEN_DONT_INLINE Scalar foo(const Scalar &x, const Scalar &y)
void forward_jacobian(const Func &f)
TestFunc1(int inputs, int values)
#define EIGEN_DONT_INLINE
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
EIGEN_DEVICE_FUNC const CosReturnType cos() const
#define VERIFY_IS_APPROX(a, b)
Matrix< Scalar, InputsAtCompileTime, 1 > InputType
Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
void test_autodiff_vector()
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
const mpreal sum(const mpreal tab[], const unsigned long int n, int &status, mp_rnd_t mode=mpreal::get_default_rnd())
void test_autodiff_jacobian()
#define CALL_SUBTEST(FUNC)
const Scalar & value() const
EIGEN_DEVICE_FUNC const SinReturnType sin() const
Jet< T, N > pow(const Jet< T, N > &f, double g)
Pose2 T1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)))
The matrix class, also used for vectors and row-vectors.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector