From f21fe5043a742641adb78ded58a61543d96f59ba Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 23 Nov 2014 17:31:33 +0100
Subject: [PATCH 1/9] Created a new expressions.h file in nonlinear

---
 .cproject                              | 14 +++-----------
 .project                               |  4 ++--
 gtsam_unstable/nonlinear/expressions.h | 25 +++++++++++++++++++++++++
 gtsam_unstable/slam/expressions.h      | 10 +---------
 4 files changed, 31 insertions(+), 22 deletions(-)
 create mode 100644 gtsam_unstable/nonlinear/expressions.h

diff --git a/.cproject b/.cproject
index c21bfd8e9..b3709d422 100644
--- a/.cproject
+++ b/.cproject
@@ -21,7 +21,7 @@
 					<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
-							<builder arguments="-j8" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
+							<builder buildPath="${ProjDirPath}/build" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="4" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
 								<option id="macosx.cpp.link.option.paths.451252615" name="Library search path (-L)" superClass="macosx.cpp.link.option.paths"/>
@@ -178,16 +178,8 @@
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
 		<project id="gtsam.null.1344331286" name="gtsam"/>
 	</storageModule>
-	<storageModule moduleId="refreshScope" versionNumber="2">
-		<configuration configurationName="Timing">
-			<resource resourceType="PROJECT" workspacePath="/gtsam"/>
-		</configuration>
-		<configuration configurationName="fast">
-			<resource resourceType="PROJECT" workspacePath="/gtsam"/>
-		</configuration>
-		<configuration configurationName="MacOSX GCC">
-			<resource resourceType="PROJECT" workspacePath="/gtsam"/>
-		</configuration>
+	<storageModule moduleId="refreshScope" versionNumber="1">
+		<resource resourceType="PROJECT" workspacePath="/gtsam"/>
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
 	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
diff --git a/.project b/.project
index 9856df2ea..9589ace56 100644
--- a/.project
+++ b/.project
@@ -23,7 +23,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value>-j5</value>
+					<value>-j4</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildCommand</key>
@@ -63,7 +63,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
-					<value>false</value>
+					<value>true</value>
 				</dictionary>
 			</arguments>
 		</buildCommand>
diff --git a/gtsam_unstable/nonlinear/expressions.h b/gtsam_unstable/nonlinear/expressions.h
new file mode 100644
index 000000000..18f05e441
--- /dev/null
+++ b/gtsam_unstable/nonlinear/expressions.h
@@ -0,0 +1,25 @@
+/**
+ * @file expressions.h
+ * @brief Common expressions, both linear and non-linear
+ * @date Oct 1, 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_;
+
+} // \namespace gtsam
+
diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h
index d9cbd5716..4a63a7de0 100644
--- a/gtsam_unstable/slam/expressions.h
+++ b/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_;

From 83abb4c6ed1b68f290b4f1c770939af0f90150fa Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 23 Nov 2014 18:04:54 +0100
Subject: [PATCH 2/9] unit test with manual Fourier

---
 .cproject                                     | 106 ++++++++++--------
 .../tests/testBasisDecompositions.cpp         |  64 +++++++++++
 2 files changed, 126 insertions(+), 44 deletions(-)
 create mode 100644 gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp

diff --git a/.cproject b/.cproject
index b3709d422..1dcc51dfe 100644
--- a/.cproject
+++ b/.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>
diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
new file mode 100644
index 000000000..db6023629
--- /dev/null
+++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
@@ -0,0 +1,64 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/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);
+
+//******************************************************************************
+TEST(BasisDecompositions, Fourier) {
+
+  // Create linear factor graph
+  GaussianFactorGraph g;
+  Key key(1);
+  for (size_t i = 0; i < 16; i++) {
+    double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
+    Matrix A(1, 3);
+    A << 1, cos(x), sin(x);
+    Vector b(1);
+    b << y;
+    g.add(key, A, b, model);
+  }
+
+  // Solve
+  VectorValues actual = g.optimize();
+
+  // Check
+  Vector3 expected(1.5661, 1.2717, 1.2717);
+  CHECK(assert_equal((Vector) expected, actual.at(key),1e-4));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+

From 149bfe1729929c3393ad14263a3e88f7360a22d8 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 23 Nov 2014 23:15:10 +0100
Subject: [PATCH 3/9] Chery-picked Paul's changes

---
 gtsam/base/Manifold.h                       |  2 +-
 gtsam_unstable/nonlinear/ExpressionFactor.h | 13 ++++++++-----
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index a5a3d5f34..1d7eda12f 100644
--- a/gtsam/base/Manifold.h
+++ b/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;
   }
 };
diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index 37a89af6b..f609071cb 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/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);

From f40940dffec99929e94fe733863dfd184a402ddf Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 23 Nov 2014 23:15:25 +0100
Subject: [PATCH 4/9] Tiny changes

---
 gtsam_unstable/nonlinear/expressions.h                  | 3 ++-
 gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp | 2 --
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/gtsam_unstable/nonlinear/expressions.h b/gtsam_unstable/nonlinear/expressions.h
index 18f05e441..eba85c33c 100644
--- a/gtsam_unstable/nonlinear/expressions.h
+++ b/gtsam_unstable/nonlinear/expressions.h
@@ -1,7 +1,7 @@
 /**
  * @file expressions.h
  * @brief Common expressions, both linear and non-linear
- * @date Oct 1, 2014
+ * @date Nov 23, 2014
  * @author Frank Dellaert
  */
 
@@ -20,6 +20,7 @@ Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
 }
 
 typedef Expression<double> double_;
+typedef Expression<Vector3> Vector3_;
 
 } // \namespace gtsam
 
diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
index c63bfeb6f..ea258dc05 100644
--- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
+++ b/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>

From e0f9f7a33e5a70cf002b77a176b76c330c2e6c08 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 23 Nov 2014 23:16:04 +0100
Subject: [PATCH 5/9] Fourier prototype that unfortunately does not compile

---
 .../tests/testBasisDecompositions.cpp         | 43 ++++++++++++++++++-
 1 file changed, 42 insertions(+), 1 deletion(-)

diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
index db6023629..acd325194 100644
--- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
+++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
@@ -32,19 +32,60 @@ 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) {
+    return H_ * c;
+  }
+};
+
 //******************************************************************************
 TEST(BasisDecompositions, Fourier) {
 
   // 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;
-    g.add(key, A, b, model);
+    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

From 9322b3ba8b5791ddc9d3fdbae038b441f09b01eb Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 24 Nov 2014 00:08:34 +0100
Subject: [PATCH 6/9] Unit test verifies Jacobian and value

---
 .../nonlinear/tests/testBasisDecompositions.cpp  | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
index acd325194..f113a4f64 100644
--- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
+++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
@@ -20,6 +20,7 @@
 #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>
@@ -60,12 +61,25 @@ public:
 
   /// 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;
@@ -93,7 +107,7 @@ TEST(BasisDecompositions, Fourier) {
 
   // Check
   Vector3 expected(1.5661, 1.2717, 1.2717);
-  CHECK(assert_equal((Vector) expected, actual.at(key),1e-4));
+  EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4));
 }
 
 //******************************************************************************

From 586ad610f8d7626a18eb7514971481eda6d4de68 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 24 Nov 2014 02:15:48 +0100
Subject: [PATCH 7/9] Added more unit tests that expose compilation issue

---
 gtsam/geometry/Point3.cpp                     | 12 +++
 gtsam/geometry/Point3.h                       |  3 +
 .../nonlinear/tests/testExpression.cpp        | 79 ++++++++++++++++---
 3 files changed, 83 insertions(+), 11 deletions(-)

diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index 42ecd8c74..ce56c78c1 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/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();
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index d490ccf8d..3a4dd08dd 100644
--- a/gtsam/geometry/Point3.h
+++ b/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;
 
diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp
index d660d28b5..4e032b9f2 100644
--- a/gtsam_unstable/nonlinear/tests/testExpression.cpp
+++ b/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) {
-//  Expression<Point3> p(67);
-//  Expression<LieScalar> norm(p, &Point3::norm);
-//  Values values;
-//  values.insert(67,Point3(3,4,5));
-//  Augmented<LieScalar> a = norm.augmented(values);
-//  EXPECT(a.value() == sqrt(50));
-//  JacobianMap expected;
-//  expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
-//  EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
-//}
+  // Create expression
+  Expression<Point3> p(67);
+  Expression<double> norm(p, &Point3::norm);
+
+  // Create Values
+  Values values;
+  values.insert(67, Point3(3, 4, 5));
+
+  // Pre-allocate JacobianMap
+  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);

From 3bf92d1a47e28766a40834f575ebe05487962c56 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 24 Nov 2014 09:58:47 +0100
Subject: [PATCH 8/9] Force eval

---
 gtsam_unstable/nonlinear/Expression-inl.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index a98ab349f..1a855aeb7 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/gtsam_unstable/nonlinear/Expression-inl.h
@@ -507,7 +507,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
     void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
         JacobianMap& jacobians) const {
       Base::Record::reverseAD(dFdT, jacobians);
-      This::trace.reverseAD(dFdT * This::dTdA, jacobians);
+      This::trace.reverseAD((dFdT * This::dTdA).eval(), jacobians);
     }
   };
 

From db6c9ff378d6c19d139a1eba977ad2273c8d9d97 Mon Sep 17 00:00:00 2001
From: HannesSommer <hannes.sommer@mavt.ethz.ch>
Date: Mon, 24 Nov 2014 10:48:15 +0100
Subject: [PATCH 9/9] swtichted to Eigen::MatrixBase<Derived> as far as
 possible for dTdA to gain some performance and not to forget some types at
 some level :).

---
 gtsam_unstable/nonlinear/CallRecord.h     | 16 ++++++---
 gtsam_unstable/nonlinear/Expression-inl.h | 44 ++++++++++++++++-------
 2 files changed, 42 insertions(+), 18 deletions(-)

diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h
index 3806f1803..f1ac0b044 100644
--- a/gtsam_unstable/nonlinear/CallRecord.h
+++ b/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>
-  inline void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
+  template<typename Derived>
+  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);
   }
 
diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index 1a855aeb7..7f59dcb7d 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/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>
-void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
-    JacobianMap& jacobians, Key key) {
-  jacobians(key).block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
+
+namespace internal {
+
+template <bool UseBlock, typename Derived>
+struct UseBlockIf {
+  static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
+                            JacobianMap& jacobians, Key key){
+    // block makes HUGE difference
+    jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(0, 0) += 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 for Dynamic ROWS Matrix type (slower)
-template<int COLS>
-inline void handleLeafCase(
-    const Eigen::Matrix<double, Eigen::Dynamic, COLS>& 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) {
-  jacobians(key) += dTdA;
+  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
@@ -507,7 +525,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
     void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
         JacobianMap& jacobians) const {
       Base::Record::reverseAD(dFdT, jacobians);
-      This::trace.reverseAD((dFdT * This::dTdA).eval(), jacobians);
+      This::trace.reverseAD(dFdT * This::dTdA, jacobians);
     }
   };