Moved AdaptAutoDiff template in its own header file

gtsam_unstable/nonlinear/AdaptAutoDiff.h

@@ -0,0 +1,94 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file AdaptAutoDiff.h
+ * @date October 22, 2014
+ * @author Frank Dellaert
+ * @brief Adaptor for Ceres style auto-differentiated functions
+ */
+
+#pragma once
+
+#include <gtsam_unstable/nonlinear/ceres_autodiff.h>
+#include <gtsam/base/Manifold.h>
+
+namespace gtsam {
+
+/// Adapt ceres-style autodiff
+template<typename F, typename T, typename A1, typename A2>
+class AdaptAutoDiff {
+
+  static const int N = traits::dimension<T>::value;
+  static const int M1 = traits::dimension<A1>::value;
+  static const int M2 = traits::dimension<A2>::value;
+
+  typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
+  typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;
+
+  typedef Canonical<T> CanonicalT;
+  typedef Canonical<A1> Canonical1;
+  typedef Canonical<A2> Canonical2;
+
+  typedef typename CanonicalT::vector VectorT;
+  typedef typename Canonical1::vector Vector1;
+  typedef typename Canonical2::vector Vector2;
+
+  // Instantiate function and charts
+  CanonicalT chartT;
+  Canonical1 chart1;
+  Canonical2 chart2;
+  F f;
+
+public:
+
+  typedef Eigen::Matrix<double, N, M1> JacobianTA1;
+  typedef Eigen::Matrix<double, N, M2> JacobianTA2;
+
+  T operator()(const A1& a1, const A2& a2, boost::optional<JacobianTA1&> H1 =
+      boost::none, boost::optional<JacobianTA2&> H2 = boost::none) {
+
+    using ceres::internal::AutoDiff;
+
+    // Make arguments
+    Vector1 v1 = chart1.apply(a1);
+    Vector2 v2 = chart2.apply(a2);
+
+    bool success;
+    VectorT result;
+
+    if (H1 || H2) {
+
+      // Get derivatives with AutoDiff
+      double *parameters[] = { v1.data(), v2.data() };
+      double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack
+      double *jacobians[] = { rowMajor1, rowMajor2 };
+      success = AutoDiff<F, double, 9, 3>::Differentiate(f, parameters, 2,
+          result.data(), jacobians);
+
+      // Convert from row-major to columnn-major
+      // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major
+      *H1 = Eigen::Map<RowMajor1>(rowMajor1);
+      *H2 = Eigen::Map<RowMajor2>(rowMajor2);
+
+    } else {
+      // Apply the mapping, to get result
+      success = f(v1.data(), v2.data(), result.data());
+    }
+    if (!success)
+      throw std::runtime_error(
+          "AdaptAutoDiff: function call resulted in failure");
+    return chartT.retract(result);
+  }
+
+};
+
+}

gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp

@@ -17,16 +17,16 @@
  * @brief unit tests for Block Automatic Differentiation
  */
 
+#include <gtsam_unstable/nonlinear/AdaptAutoDiff.h>
+#include <gtsam_unstable/nonlinear/Expression.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
-#include <gtsam_unstable/nonlinear/Expression.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/LieScalar.h>
 
-#include <gtsam_unstable/nonlinear/ceres_autodiff.h>
 #include <gtsam_unstable/nonlinear/ceres_example.h>
 
 #undef CHECK
@@ -164,75 +164,6 @@ TEST(Expression, AutoDiff2) {
   EXPECT(assert_equal(E2,H2,1e-8));
 }
 
-/* ************************************************************************* */
-// Adapt ceres-style autodiff
-template<typename F, typename T, typename A1, typename A2>
-class AdaptAutoDiff {
-
-  static const int N = traits::dimension<T>::value;
-  static const int M1 = traits::dimension<A1>::value;
-  static const int M2 = traits::dimension<A2>::value;
-
-  typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
-  typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;
-
-  typedef Canonical<T> CanonicalT;
-  typedef Canonical<A1> Canonical1;
-  typedef Canonical<A2> Canonical2;
-
-  typedef typename CanonicalT::vector VectorT;
-  typedef typename Canonical1::vector Vector1;
-  typedef typename Canonical2::vector Vector2;
-
-  // Instantiate function and charts
-  CanonicalT chartT;
-  Canonical1 chart1;
-  Canonical2 chart2;
-  F f;
-
-public:
-
-  typedef Eigen::Matrix<double, N, M1> JacobianTA1;
-  typedef Eigen::Matrix<double, N, M2> JacobianTA2;
-
-  T operator()(const A1& a1, const A2& a2, boost::optional<JacobianTA1&> H1 =
-      boost::none, boost::optional<JacobianTA2&> H2 = boost::none) {
-
-    using ceres::internal::AutoDiff;
-
-    // Make arguments
-    Vector1 v1 = chart1.apply(a1);
-    Vector2 v2 = chart2.apply(a2);
-
-    bool success;
-    VectorT result;
-
-    if (H1 || H2) {
-
-      // Get derivatives with AutoDiff
-      double *parameters[] = { v1.data(), v2.data() };
-      double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack
-      double *jacobians[] = { rowMajor1, rowMajor2 };
-      success = AutoDiff<F, double, 9, 3>::Differentiate(f, parameters, 2,
-          result.data(), jacobians);
-
-      // Convert from row-major to columnn-major
-      // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major
-      *H1 = Eigen::Map<RowMajor1>(rowMajor1);
-      *H2 = Eigen::Map<RowMajor2>(rowMajor2);
-
-    } else {
-      // Apply the mapping, to get result
-      success = f(v1.data(), v2.data(), result.data());
-    }
-    if (!success)
-      throw std::runtime_error(
-          "AdaptAutoDiff: function call resulted in failure");
-    return chartT.retract(result);
-  }
-
-};
-
 /* ************************************************************************* */
 // Test AutoDiff wrapper Snavely
 TEST(Expression, AutoDiff3) {