11 #include <unsupported/Eigen/AutoDiff> 13 template<
typename Scalar>
25 template<
typename Vector>
29 return (p-
Vector(
Scalar(-1),
Scalar(1.))).norm() + (p.array() * p.array()).sum() + p.dot(p);
32 template<
typename _Scalar,
int NX=Dynamic,
int NY=Dynamic>
37 InputsAtCompileTime = NX,
38 ValuesAtCompileTime = NY
46 TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
47 TestFunc1(
int inputs_,
int values_) : m_inputs(inputs_), m_values(values_) {}
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>
130 o[0] = input[0] + input[1] * dt * _gain;
131 o[1] = input[1] * _gain;
135 template <
typename T1,
typename T2,
typename T3>
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();
361 AD y1 = (AD(c)+AD(c))*x0;
362 y1 = x0 * (AD(c)+AD(c));
365 AD y3 = (AD(c)*(-AD(c))+AD(c))*x0;
366 y3 = x0 * (AD(c)*(-AD(c))+AD(c));
367 return (y1+y2+y3).value();
Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
#define CALL_SUBTEST_4(FUNC)
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Jet< T, N > cos(const Jet< T, N > &f)
static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3))
A scalar type replacement with automatic differentiation capability.
void test_autodiff_hessian()
#define CALL_SUBTEST_3(FUNC)
EIGEN_DECLARE_TEST(autodiff)
Jet< T, N > sin(const Jet< T, N > &f)
#define EIGEN_ASM_COMMENT(X)
void test_autodiff_scalar()
EIGEN_DONT_INLINE Scalar foo(const Scalar &x, const Scalar &y)
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
void forward_jacobian(const Func &f)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
TestFunc1(int inputs_, int values_)
#define EIGEN_DONT_INLINE
EIGEN_DEVICE_FUNC const ExpReturnType exp() const
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
#define VERIFY_IS_APPROX(a, b)
Matrix< Scalar, InputsAtCompileTime, 1 > InputType
#define CALL_SUBTEST_1(FUNC)
Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
void test_autodiff_vector()
Array< int, Dynamic, 1 > v
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
void test_autodiff_jacobian()
const Scalar & value() const
#define CALL_SUBTEST_5(FUNC)
#define CALL_SUBTEST(FUNC)
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
#define CALL_SUBTEST_2(FUNC)
Jet< T, N > sqrt(const Jet< T, N > &f)
Jet< T, N > pow(const Jet< T, N > &f, double g)
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
internal::enable_if< internal::valid_indexed_view_overload< RowIndices, ColIndices >::value &&internal::traits< typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::ReturnAsIndexedView, typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::type operator()(const RowIndices &rowIndices, const ColIndices &colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector