From 6a5e4191a33b0c58eda46b622ddeb7fc23eed047 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 21 Sep 2014 17:43:47 +0200
Subject: [PATCH] UnaryExpression

---
 gtsam_unstable/base/tests/testBAD.cpp | 41 +++++++++++++++++++++++----
 1 file changed, 35 insertions(+), 6 deletions(-)

diff --git a/gtsam_unstable/base/tests/testBAD.cpp b/gtsam_unstable/base/tests/testBAD.cpp
index 0d6b6ca0f..2414b7a76 100644
--- a/gtsam_unstable/base/tests/testBAD.cpp
+++ b/gtsam_unstable/base/tests/testBAD.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/Testable.h>
@@ -71,6 +72,34 @@ public:
   }
 };
 
+//-----------------------------------------------------------------------------
+/// Unary Expression
+template<class T, class E>
+class UnaryExpression {
+
+public:
+
+  typedef T (*function)(const typename E::type&);
+
+private:
+
+  const E expression_;
+  function f_;
+
+public:
+
+  typedef T type;
+
+  /// Constructor with a single key
+  UnaryExpression(function f, const E& expression) :
+      expression_(expression), f_(f) {
+  }
+
+  T value(const Values& values) const {
+    return f_(expression_.value(values));
+  }
+};
+
 //-----------------------------------------------------------------------------
 /// Binary Expression
 template<class T, class E1, class E2>
@@ -106,10 +135,8 @@ Point3 transformTo(const Pose3& x, const Point3& p) {
   return x.transform_to(p);
 }
 
-/// Expression version of project
-template<class E>
-LeafExpression<Point2> project(const E& p) {
-  return LeafExpression<Point2>(0);
+Point2 project(const Point3& p) {
+  return PinholeCamera<Cal3_S2>::project_to_camera(p);
 }
 
 /// Expression version of uncalibrate
@@ -206,8 +233,10 @@ TEST(BAD, test) {
 
   // Create expression tree
   typedef BinaryExpression<Point3, LeafExpression<Pose3>, LeafExpression<Point3> > Binary1;
-  Binary1 p_cam = Binary1(transformTo, x, p);
-  LeafExpression<Point2> projection = project(p_cam);
+  Binary1 p_cam(transformTo, x, p);
+
+  typedef UnaryExpression<Point2, Binary1> Unary1;
+  Unary1 projection(project, p_cam);
   LeafExpression<Point2> uv_hat = uncalibrate(K, projection);
 
   // Create factor