Merged in feature/expressions (pull request #45)

Pull request for debugging Expression<double> issue

.cproject

@@ -592,6 +592,7 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -599,6 +600,7 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -646,6 +648,7 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -653,6 +656,7 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -660,6 +664,7 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -675,6 +680,7 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1122,6 +1128,7 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1351,6 +1358,46 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testFixedVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1433,7 +1480,6 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1473,7 +1519,6 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1481,7 +1526,6 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1495,46 +1539,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFMap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testFixedVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1792,6 +1796,7 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1799,6 +1804,7 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1806,6 +1812,7 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1813,6 +1820,7 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2627,6 +2635,7 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2634,6 +2643,7 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2641,6 +2651,7 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2758,6 +2769,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBasisDecompositions.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testBasisDecompositions.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -3168,7 +3187,6 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>

gtsam/base/Manifold.h

@@ -235,7 +235,7 @@ struct DefaultChart<double> {
   static double retract(double origin, const vector& d) {
     return origin + d[0];
   }
-  static const int getDimension(double) {
+  static int getDimension(double) {
     return 1;
   }
 };

gtsam/geometry/Point3.cpp

@@ -111,6 +111,18 @@ double Point3::norm(boost::optional<Matrix&> H) const {
   return r;
 }
 
+/* ************************************************************************* */
+double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const {
+  double r = norm();
+  if (H) {
+    if (fabs(r) > 1e-10)
+      *H << x_ / r, y_ / r, z_ / r;
+    else
+      *H << 1, 1, 1; // really infinity, why 1 ?
+  }
+  return r;
+}
+
 /* ************************************************************************* */
 Point3 Point3::normalize(boost::optional<Matrix&> H) const {
   Point3 normalized = *this / norm();

gtsam/geometry/Point3.h

@@ -186,6 +186,9 @@ namespace gtsam {
     /** Distance of the point from the origin, with Jacobian */
     double norm(boost::optional<Matrix&> H) const;
 
+    /** Distance of the point from the origin, with Jacobian */
+    double norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const;
+
     /** normalize, with optional Jacobian */
     Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
 

gtsam_unstable/nonlinear/CallRecord.h

@@ -46,7 +46,7 @@ namespace internal {
  * it just passes dense Eigen matrices through.
  */
 template<bool ConvertToDynamicRows>
-struct ConvertToDynamicRowsIf {
+struct ConvertToVirtualFunctionSupportedMatrixType {
   template<typename Derived>
   static Eigen::Matrix<double, Eigen::Dynamic, Derived::ColsAtCompileTime> convert(
       const Eigen::MatrixBase<Derived> & x) {
@@ -55,7 +55,13 @@ struct ConvertToDynamicRowsIf {
 };
 
 template<>
-struct ConvertToDynamicRowsIf<false> {
+struct ConvertToVirtualFunctionSupportedMatrixType<false> {
+  template<typename Derived>
+  static const Eigen::Matrix<double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> convert(
+      const Eigen::MatrixBase<Derived> & x) {
+    return x;
+  }
+  // special treatment of matrices that don't need conversion
   template<int Rows, int Cols>
   static const Eigen::Matrix<double, Rows, Cols> & convert(
       const Eigen::Matrix<double, Rows, Cols> & x) {
@@ -143,11 +149,11 @@ struct CallRecord: virtual private internal::ReverseADInterface<
     _startReverseAD(jacobians);
   }
 
-  template<int Rows>
+  template<typename Derived>
-  inline void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
+  inline void reverseAD(const Eigen::MatrixBase<Derived> & dFdT,
       JacobianMap& jacobians) const {
     _reverseAD(
-        internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert(
+        internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert(
             dFdT), jacobians);
   }
 

gtsam_unstable/nonlinear/Expression-inl.h

@@ -64,18 +64,36 @@ public:
 };
 
 //-----------------------------------------------------------------------------
-/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
+
-template<int ROWS, int COLS>
+namespace internal {
-void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
+
-    JacobianMap& jacobians, Key key) {
+template <bool UseBlock, typename Derived>
-  jacobians(key).block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
+struct UseBlockIf {
-}
+  static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
-/// Handle Leaf Case for Dynamic ROWS Matrix type (slower)
+                            JacobianMap& jacobians, Key key){
-template<int COLS>
+    // block makes HUGE difference
-inline void handleLeafCase(
+    jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(0, 0) += dTdA;
-    const Eigen::Matrix<double, Eigen::Dynamic, COLS>& dTdA,
+  };
+};
+/// Handle Leaf Case for Dynamic Matrix type (slower)
+template <typename Derived>
+struct UseBlockIf<false, Derived> {
+  static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
                             JacobianMap& jacobians, Key key) {
     jacobians(key) += dTdA;
+  }
+};
+}
+
+/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
+template<typename Derived>
+void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
+    JacobianMap& jacobians, Key key) {
+  internal::UseBlockIf<
+      Derived::RowsAtCompileTime != Eigen::Dynamic &&
+      Derived::ColsAtCompileTime != Eigen::Dynamic,
+      Derived>
+    ::addToJacobian(dTdA, jacobians, key);
 }
 
 //-----------------------------------------------------------------------------
@@ -166,9 +184,9 @@ public:
   void reverseAD(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
       JacobianMap& jacobians) const {
     if (kind == Leaf)
-      handleLeafCase(dTdA.eval(), jacobians, content.key);
+      handleLeafCase(dTdA, jacobians, content.key);
     else if (kind == Function)
-      content.ptr->reverseAD(dTdA.eval(), jacobians);
+      content.ptr->reverseAD(dTdA, jacobians);
   }
 
   /// Define type so we can apply it as a meta-function

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -81,6 +81,8 @@ public:
    */
   virtual Vector unwhitenedError(const Values& x,
       boost::optional<std::vector<Matrix>&> H = boost::none) const {
+    // TODO(PTF) Is this a place for custom charts?
+    DefaultChart<T> chart;
     if (H) {
       // H should be pre-allocated
       assert(H->size()==size());
@@ -95,18 +97,19 @@ public:
       T value = expression_.value(x, map); // <<< Reverse AD happens here !
 
       // Copy blocks into the vector of jacobians passed in
-      for (DenseIndex i = 0; i < size(); i++)
+      for (DenseIndex i = 0; i < static_cast<DenseIndex>(size()); i++)
         H->at(i) = Ab(i);
 
-      return measurement_.localCoordinates(value);
+      return chart.local(measurement_, value);
     } else {
       const T& value = expression_.value(x);
-      return measurement_.localCoordinates(value);
+      return chart.local(measurement_, value);
     }
   }
 
   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
-
+    // TODO(PTF) Is this a place for custom charts?
+    DefaultChart<T> chart;
     // Only linearize if the factor is active
     if (!active(x))
       return boost::shared_ptr<JacobianFactor>();
@@ -128,7 +131,7 @@ public:
 
     // Evaluate error to get Jacobians and RHS vector b
     T value = expression_.value(x, map); // <<< Reverse AD happens here !
-    Ab(size()).col(0) = -measurement_.localCoordinates(value);
+    Ab(size()).col(0) = -chart.local(measurement_, value);
 
     // Whiten the corresponding system, Ab already contains RHS
     Vector dummy(Dim);

gtsam_unstable/nonlinear/expressions.h

@@ -0,0 +1,26 @@
+/**
+ * @file expressions.h
+ * @brief Common expressions, both linear and non-linear
+ * @date Nov 23, 2014
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam_unstable/nonlinear/Expression.h>
+#include <boost/bind.hpp>
+
+namespace gtsam {
+
+// Generics
+
+template<class T>
+Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
+  return Expression<T>(t1, &T::between, t2);
+}
+
+typedef Expression<double> double_;
+typedef Expression<Vector3> Vector3_;
+
+} // \namespace gtsam
+

gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp

@@ -0,0 +1,119 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testBasisDecompositions.cpp
+ * @date November 23, 2014
+ * @author Frank Dellaert
+ * @brief unit tests for Basis Decompositions w Expressions
+ */
+
+#include <gtsam_unstable/nonlinear/expressions.h>
+#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/Testable.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/list_of.hpp>
+using boost::assign::list_of;
+
+using namespace std;
+using namespace gtsam;
+
+noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
+
+/// Fourier
+template<int N>
+class Fourier {
+
+public:
+
+  typedef Eigen::Matrix<double, N, 1> Coefficients;
+  typedef Eigen::Matrix<double, 1, N> Jacobian;
+
+private:
+
+  double x_;
+  Jacobian H_;
+
+public:
+
+  /// Constructor
+  Fourier(double x) :
+      x_(x) {
+    H_(0, 0) = 1;
+    for (size_t i = 1; i < N; i += 2) {
+      H_(0, i) = cos(i * x);
+      H_(0, i + 1) = sin(i * x);
+    }
+  }
+
+  /// Given coefficients c, predict value for x
+  double operator()(const Coefficients& c, boost::optional<Jacobian&> H) {
+    if (H)
+      (*H) = H_;
+    return H_ * c;
+  }
+};
+
+//******************************************************************************
+TEST(BasisDecompositions, Fourier) {
+  Fourier<3> fx(0);
+  Eigen::Matrix<double, 1, 3> expectedH, actualH;
+  Vector3 c(1.5661, 1.2717, 1.2717);
+  expectedH = numericalDerivative11<double, Vector3>(
+      boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c);
+  EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9);
+  EXPECT(assert_equal((Matrix)expectedH, actualH));
+}
+
+//******************************************************************************
+TEST(BasisDecompositions, FourierExpression) {
+
+  // Create linear factor graph
+  GaussianFactorGraph g;
+  Key key(1);
+  Vector3_ c(key);
+  for (size_t i = 0; i < 16; i++) {
+    double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
+
+    // Manual JacobianFactor
+    Matrix A(1, 3);
+    A << 1, cos(x), sin(x);
+    Vector b(1);
+    b << y;
+    JacobianFactor f1(key, A, b, model);
+
+    // With ExpressionFactor
+    Expression<double> expression(Fourier<3>(x), c);
+    ExpressionFactor<double> f2(model, y, expression);
+
+    g.add(f1);
+  }
+
+  // Solve
+  VectorValues actual = g.optimize();
+
+  // Check
+  Vector3 expected(1.5661, 1.2717, 1.2717);
+  EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+

gtsam_unstable/nonlinear/tests/testExpression.cpp

@@ -73,27 +73,84 @@ TEST(Expression, Leaves) {
 }
 
 /* ************************************************************************* */
+// Unary(Leaf)
+namespace unary {
+Point2 f0(const Point3& p, boost::optional<Matrix23&> H) {
+  return Point2();
+}
+LieScalar f1(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
+  return LieScalar(0.0);
+}
+double f2(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
+  return 0.0;
+}
+Expression<Point3> p(1);
+set<Key> expected = list_of(1);
+}
+TEST(Expression, Unary0) {
+  using namespace unary;
+  Expression<Point2> e(f0, p);
+  EXPECT(expected == e.keys());
+}
+TEST(Expression, Unary1) {
+  using namespace unary;
+  Expression<double> e(f1, p);
+  EXPECT(expected == e.keys());
+}
+TEST(Expression, Unary2) {
+  using namespace unary;
+  Expression<double> e(f2, p);
+  EXPECT(expected == e.keys());
+}
+/* ************************************************************************* */
+//Nullary Method
+TEST(Expression, NullaryMethod) {
 
-//TEST(Expression, NullaryMethod) {
+  // Create expression
-//  Expression<Point3> p(67);
+  Expression<Point3> p(67);
-//  Expression<LieScalar> norm(p, &Point3::norm);
+  Expression<double> norm(p, &Point3::norm);
-//  Values values;
+
-//  values.insert(67,Point3(3,4,5));
+  // Create Values
-//  Augmented<LieScalar> a = norm.augmented(values);
+  Values values;
-//  EXPECT(a.value() == sqrt(50));
+  values.insert(67, Point3(3, 4, 5));
-//  JacobianMap expected;
+
-//  expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
+  // Pre-allocate JacobianMap
-//  EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
+  FastVector<Key> keys;
-//}
+  keys.push_back(67);
+  FastVector<int> dims;
+  dims.push_back(3);
+  VerticalBlockMatrix Ab(dims, 1);
+  JacobianMap map(keys, Ab);
+
+  // Get value and Jacobian
+  double actual = norm.value(values, map);
+
+  // Check all
+  EXPECT(actual == sqrt(50));
+  Matrix expected(1, 3);
+  expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
+  EXPECT(assert_equal(expected,Ab(0)));
+}
 /* ************************************************************************* */
 // Binary(Leaf,Leaf)
 namespace binary {
 // Create leaves
+double doubleF(const Pose3& pose, const Point3& point,
+    boost::optional<Eigen::Matrix<double, 1, 6>&> H1,
+    boost::optional<Eigen::Matrix<double, 1, 3>&> H2) {
+  return 0.0;
+}
 Expression<Pose3> x(1);
 Expression<Point3> p(2);
 Expression<Point3> p_cam(x, &Pose3::transform_to, p);
 }
 /* ************************************************************************* */
+// Check that creating an expression to double compiles
+TEST(Expression, BinaryToDouble) {
+  using namespace binary;
+  Expression<double> p_cam(doubleF, x, p);
+}
+/* ************************************************************************* */
 // keys
 TEST(Expression, BinaryKeys) {
   set<Key> expected = list_of(1)(2);

gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp

@@ -22,8 +22,6 @@
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/PriorFactor.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/Testable.h>
 
 #include <CppUnitLite/TestHarness.h>

gtsam_unstable/slam/expressions.h

@@ -7,20 +7,12 @@
 
 #pragma once
 
-#include <gtsam_unstable/nonlinear/Expression.h>
+#include <gtsam_unstable/nonlinear/expressions.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
-#include <boost/bind.hpp>
 
 namespace gtsam {
 
-// Generics
-
-template<class T>
-Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
-  return Expression<T>(t1, &T::between, t2);
-}
-
 // 2D Geometry
 
 typedef Expression<Point2> Point2_;