1 files changed, 184 insertions, 0 deletions
diff --git a/eigen/unsupported/test/autodiff.cpp b/eigen/unsupported/test/autodiff.cpp
new file mode 100644
index 0000000..7c112a1
--- /dev/null
+++ b/eigen/unsupported/test/autodiff.cpp
@@ -0,0 +1,184 @@
+// 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");
+  return static_cast<Scalar>(x*2 - pow(x,2) + 2*sqrt(y*y) - 4 * sin(x) + 2 * cos(y) - exp(-0.5*x*x));
+  //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!
+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!
+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));
+}
+
+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)) ));
+}
+
+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();
+}
+
+void test_autodiff()
+{
+  for(int i = 0; i < g_repeat; i++) {
+    CALL_SUBTEST_1( test_autodiff_scalar() );
+    CALL_SUBTEST_2( test_autodiff_vector() );
+    CALL_SUBTEST_3( test_autodiff_jacobian() );
+  }
+
+  bug_1222();
+}
+