From 439f51ec7f051308a5ec08975cd5c189a14f8b3f Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 22 Oct 2014 10:10:58 +0200
Subject: [PATCH] test out invoke

---
 .../nonlinear/tests/testExpressionMeta.cpp    | 105 ++++++++++++++++--
 1 file changed, 97 insertions(+), 8 deletions(-)

diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
index 19a39d52f..ecc343384 100644
--- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
@@ -86,32 +86,31 @@ TEST(ExpressionFactor, JacobiansValue) {
 
 /* ************************************************************************* */
 // Test out polymorphic transform
-
 #include <boost/fusion/include/make_vector.hpp>
 #include <boost/fusion/include/transform.hpp>
 #include <boost/utility/result_of.hpp>
 
 struct triple {
-  template<class> struct result; // says we will provide result
+  template<class > struct result; // says we will provide result
 
   template<class F>
   struct result<F(int)> {
-      typedef double type; // result for int argument
+    typedef double type; // result for int argument
   };
 
   template<class F>
   struct result<F(const int&)> {
-      typedef double type; // result for int argument
+    typedef double type; // result for int argument
   };
 
   template<class F>
   struct result<F(const double &)> {
-      typedef double type; // result for double argument
+    typedef double type; // result for double argument
   };
 
   template<class F>
   struct result<F(double)> {
-      typedef double type; // result for double argument
+    typedef double type; // result for double argument
   };
 
   // actual function
@@ -121,6 +120,7 @@ struct triple {
   }
 };
 
+// Test out polymorphic transform
 TEST(ExpressionFactor, Triple) {
   typedef boost::fusion::vector<int, double> IntDouble;
   IntDouble H = boost::fusion::make_vector(1, 2.0);
@@ -138,10 +138,11 @@ TEST(ExpressionFactor, Triple) {
 /* ************************************************************************* */
 #include <boost/fusion/include/invoke.hpp>
 #include <boost/functional/value_factory.hpp>
+#include <boost/fusion/functional/adapter/fused.hpp>
+#include <boost/fusion/adapted/mpl.hpp>
 
-// Test out polymorphic transform
+// Test out invoke
 TEST(ExpressionFactor, Invoke) {
-  std::plus<int> add;
   assert(invoke(add,boost::fusion::make_vector(1,1)) == 2);
 
   // Creating a Pose3 (is there another way?)
@@ -149,6 +150,94 @@ TEST(ExpressionFactor, Invoke) {
   Pose3 pose = boost::fusion::invoke(boost::value_factory<Pose3>(), pair);
 }
 
+/* ************************************************************************* */
+// debug const issue (how to make read/write arguments for invoke)
+struct test {
+  typedef void result_type;
+  void operator()(int& a, int& b) const {
+    a = 6;
+    b = 7;
+  }
+};
+
+TEST(ExpressionFactor, ConstIssue) {
+  int a, b;
+  boost::fusion::invoke(test(),
+      boost::fusion::make_vector(boost::ref(a), boost::ref(b)));
+  LONGS_EQUAL(6, a);
+  LONGS_EQUAL(7, b);
+}
+
+/* ************************************************************************* */
+// Test out invoke on a given GTSAM function
+// then construct prototype for it's derivatives
+TEST(ExpressionFactor, InvokeDerivatives) {
+  // This is the method in Pose3:
+  //  Point3 transform_to(const Point3& p) const;
+  //  Point3 transform_to(const Point3& p,
+  //    boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
+
+  // Let's assign it it to a boost function object
+  // cast is needed because Pose3::transform_to is overloaded
+  typedef boost::function<Point3(const Pose3&, const Point3&)> F;
+  F f = static_cast<Point3 (Pose3::*)(
+      const Point3&) const >(&Pose3::transform_to);
+
+  // Create arguments
+Pose3  pose;
+  Point3 point;
+  typedef boost::fusion::vector<Pose3, Point3> Arguments;
+  Arguments args = boost::fusion::make_vector(pose, point);
+
+  // Create fused function (takes fusion vector) and call it
+  boost::fusion::fused<F> g(f);
+  Point3 actual = g(args);
+  CHECK(assert_equal(point,actual));
+
+  // We can *immediately* do this using invoke
+  Point3 actual2 = boost::fusion::invoke(f, args);
+  CHECK(assert_equal(point,actual2));
+
+  // Now, let's create the optional Jacobian arguments
+  typedef Point3 T;
+  typedef boost::mpl::vector<Pose3, Point3> TYPES;
+  typedef boost::mpl::transform<TYPES, Optional<T, MPL::_1> >::type Optionals;
+
+  // Unfortunately this is moot: we need a pointer to a function with the
+  // optional derivatives; I don't see a way of calling a function that we
+  // did not get access to by the caller passing us a pointer.
+  // Let's test below whether we can have a proxy object
+}
+
+struct proxy {
+  typedef Point3 result_type;
+  Point3 operator()(const Pose3& pose, const Point3& point) const {
+    return pose.transform_to(point);
+  }
+  Point3 operator()(const Pose3& pose, const Point3& point,
+      boost::optional<Matrix36&> Dpose,
+      boost::optional<Matrix3&> Dpoint) const {
+    return pose.transform_to(point, Dpose, Dpoint);
+  }
+};
+
+TEST(ExpressionFactor, InvokeDerivatives2) {
+  // without derivatives
+  Pose3 pose;
+  Point3 point;
+  Point3 actual = boost::fusion::invoke(proxy(),
+      boost::fusion::make_vector(pose, point));
+  CHECK(assert_equal(point,actual));
+
+  // with derivatives, does not work, const issue again
+  Matrix36 Dpose;
+  Matrix3 Dpoint;
+  Point3 actual2 = boost::fusion::invoke(proxy(),
+      boost::fusion::make_vector(pose, point, boost::ref(Dpose),
+          boost::ref(Dpoint)));
+  CHECK(assert_equal(point,actual2));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;