Merged develop into fix/matlab_wrapper

2015-05-15 17:07:06 -04:00 · 2015-05-15 17:07:06 -04:00 · 9132af9e60
parent e5dce0de0d 0efdf3aad0
commit 9132af9e60
71 changed files with 2570 additions and 2243 deletions
--- a/.cproject
+++ b/.cproject
@ -532,14 +532,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j4</buildArguments>
-				<buildTarget>testSimilarity3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@ -755,6 +747,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testSimilarity3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@ -1301,7 +1301,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>
@ -1341,7 +1340,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>
@ -1349,7 +1347,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>
@ -1453,6 +1450,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>
@ -1763,6 +1761,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>
@ -1770,6 +1769,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>
@ -1777,6 +1777,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>
@ -1784,6 +1785,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>
@ -1975,7 +1977,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>
@ -2037,22 +2038,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testExpressionMeta.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testExpressionMeta.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j4</buildArguments>
-				<buildTarget>testCustomChartExpression.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@ -2127,6 +2112,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>
@ -2134,6 +2120,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>
@ -2181,6 +2168,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>
@ -2188,6 +2176,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>
@ -2195,6 +2184,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>
@ -2210,6 +2200,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>
@ -2783,6 +2774,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testExecutionTrace.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testExecutionTrace.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testIMUSystem.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@ -3329,6 +3328,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>
@ -3336,6 +3336,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>
@ -3343,6 +3344,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>
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@ -98,7 +98,8 @@ if(    NOT cmake_build_type_tolower STREQUAL ""
   AND NOT cmake_build_type_tolower STREQUAL "release"
   AND NOT cmake_build_type_tolower STREQUAL "timing"
   AND NOT cmake_build_type_tolower STREQUAL "profiling"
-   AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
+   AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo"
+   AND NOT cmake_build_type_tolower STREQUAL "minsizerel")
  message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).")
 endif()

--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@ -31,28 +31,29 @@
 *
 */

-#include <gtsam/base/timing.h>
-#include <gtsam/base/treeTraversal-inst.h>
-#include <gtsam/slam/dataset.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/linear/GaussianJunctionTree.h>
-#include <gtsam/linear/GaussianEliminationTree.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
+#include <gtsam/linear/GaussianEliminationTree.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/base/treeTraversal-inst.h>

-#include <fstream>
-#include <iostream>
-#include <boost/archive/binary_oarchive.hpp>
 #include <boost/archive/binary_iarchive.hpp>
-#include <boost/serialization/export.hpp>
+#include <boost/archive/binary_oarchive.hpp>
 #include <boost/program_options.hpp>
 #include <boost/range/algorithm/set_algorithm.hpp>
 #include <boost/random.hpp>
+#include <boost/serialization/export.hpp>
+
+#include <fstream>
+#include <iostream>

 #ifdef GTSAM_USE_TBB
 #include <tbb/tbb.h>
@ -72,23 +73,6 @@ typedef NoiseModelFactor1<Pose> NM1;
 typedef NoiseModelFactor2<Pose,Pose> NM2;
 typedef BearingRangeFactor<Pose,Point2> BR;

-//GTSAM_VALUE_EXPORT(Value);
-//GTSAM_VALUE_EXPORT(Pose);
-//GTSAM_VALUE_EXPORT(Rot2);
-//GTSAM_VALUE_EXPORT(Point2);
-//GTSAM_VALUE_EXPORT(NonlinearFactor);
-//GTSAM_VALUE_EXPORT(NoiseModelFactor);
-//GTSAM_VALUE_EXPORT(NM1);
-//GTSAM_VALUE_EXPORT(NM2);
-//GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
-//GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
-//GTSAM_VALUE_EXPORT(BR);
-//GTSAM_VALUE_EXPORT(noiseModel::Base);
-//GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
-//GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
-//GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
-//GTSAM_VALUE_EXPORT(noiseModel::Unit);
-
 double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
  // Compute degrees of freedom (observations - variables)
  // In ocaml, +1 was added to the observations to account for the prior, but
@ -269,12 +253,12 @@ void runIncremental()
      boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
    {
      Key key1 = measurement->key1(), key2 = measurement->key2();
-      if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) {
+      if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
        // We found an odometry starting at firstStep
        firstPose = std::min(key1, key2);
        break;
      }
-      if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) {
+      if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) {
        // We found an odometry joining firstStep with a previous pose
        havePreviousPose = true;
        firstPose = std::max(key1, key2);
@ -303,7 +287,9 @@ void runIncremental()

  cout << "Playing forward time steps..." << endl;

-  for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step)
+  for (size_t step = firstPose;
+      nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep);
+      ++step)
  {
    Values newVariables;
    NonlinearFactorGraph newFactors;
--- a/gtsam/3rdparty/ceres/eigen.h
+++ b/gtsam/3rdparty/ceres/eigen.h
@ -31,7 +31,7 @@
 #ifndef CERES_INTERNAL_EIGEN_H_
 #define CERES_INTERNAL_EIGEN_H_

-#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <Eigen/Core>

 namespace ceres {

--- a/gtsam/3rdparty/ceres/fixed_array.h
+++ b/gtsam/3rdparty/ceres/fixed_array.h
@ -33,7 +33,7 @@
 #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_

 #include <cstddef>
-#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <Eigen/Core>
 #include <gtsam/3rdparty/ceres/macros.h>
 #include <gtsam/3rdparty/ceres/manual_constructor.h>

--- a/gtsam/3rdparty/ceres/jet.h
+++ b/gtsam/3rdparty/ceres/jet.h
@ -162,7 +162,7 @@
 #include <limits>
 #include <string>

-#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <Eigen/Core>
 #include <gtsam/3rdparty/ceres/fpclassify.h>

 namespace ceres {
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@ -131,12 +131,6 @@ else()
    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 endif()

-# Set dataset paths
-set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp"
-    APPEND PROPERTY COMPILE_DEFINITIONS
-	"SOURCE_TREE_DATASET_DIR=\"${PROJECT_SOURCE_DIR}/examples/Data\""
-	"INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"")
-
 # Special cases
 if(MSVC)
 	set_property(SOURCE
--- a/gtsam/base/OptionalJacobian.h
+++ b/gtsam/base/OptionalJacobian.h
@ -168,5 +168,20 @@ public:
  Jacobian* operator->(){ return pointer_; }
 };

+// forward declare
+template <typename T> struct traits;
+
+/**
+ * @brief: meta-function to generate JacobianTA optional reference
+ * Used mainly by Expressions
+ * @param T return type
+ * @param A argument type
+ */
+template<class T, class A>
+struct MakeOptionalJacobian {
+  typedef OptionalJacobian<traits<T>::dimension,
+      traits<A>::dimension> type;
+};
+
 } // namespace gtsam

--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@ -222,11 +222,7 @@ double norm_2(const Vector& v) {

 /* ************************************************************************* */
 Vector reciprocal(const Vector &a) {
-  size_t n = a.size();
-  Vector b(n);
-  for( size_t i = 0; i < n; i++ )
-    b(i) = 1.0/a(i);
-  return b;
+  return a.array().inverse();
 }

 /* ************************************************************************* */
--- a/gtsam/base/VectorSpace.h
+++ b/gtsam/base/VectorSpace.h
@ -24,13 +24,6 @@ namespace internal {
 template<class Class, int N>
 struct VectorSpaceImpl {

-  /// @name Group
-  /// @{
-  static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
-  static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
-  static Class Inverse(const Class& m) { return -m;}
-  /// @}
-
  /// @name Manifold
  /// @{
  typedef Eigen::Matrix<double, N, 1> TangentVector;
@ -68,21 +61,21 @@ struct VectorSpaceImpl {
    return Class(v);
  }

-  static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
-      ChartJacobian H2) {
+  static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
+      ChartJacobian H2 = boost::none) {
    if (H1) *H1 = Jacobian::Identity();
    if (H2) *H2 = Jacobian::Identity();
    return v1 + v2;
  }

-  static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
-      ChartJacobian H2) {
+  static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
+      ChartJacobian H2 = boost::none) {
    if (H1) *H1 = - Jacobian::Identity();
    if (H2) *H2 =   Jacobian::Identity();
    return v2 - v1;
  }

-  static Class Inverse(const Class& v, ChartJacobian H) {
+  static Class Inverse(const Class& v, ChartJacobian H = boost::none) {
    if (H) *H = - Jacobian::Identity();
    return -v;
  }
--- a/gtsam/base/tests/testTreeTraversal.cpp
+++ b/gtsam/base/tests/testTreeTraversal.cpp
@ -45,11 +45,11 @@ struct TestForest {
 };

 TestForest makeTestForest() {
-  //    0     1
-  //   / \
-  //  2   3
-  //      |
-  //      4
+  //     0     1
+  //   / |
+  //  2  3
+  //     |
+  //     4
  TestForest forest;
  forest.roots_.push_back(boost::make_shared<TestNode>(0));
  forest.roots_.push_back(boost::make_shared<TestNode>(1));
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@ -25,7 +25,7 @@
 #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@"

 // Paths to example datasets distributed with GTSAM
-#define GTSAM_SOURCE_TREE_DATASET_DIR "@CMAKE_SOURCE_DIR@/examples/Data"
+#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data"
 #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data"

 // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices)
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@ -467,7 +467,7 @@ namespace gtsam {

    // find highest label among branches
    boost::optional<L> highestLabel;
-    boost::optional<size_t> nrChoices;
+    size_t nrChoices = 0;
    for (Iterator it = begin; it != end; it++) {
      if (it->root_->isLeaf())
        continue;
@ -475,22 +475,21 @@ namespace gtsam {
          boost::dynamic_pointer_cast<const Choice>(it->root_);
      if (!highestLabel || c->label() > *highestLabel) {
        highestLabel.reset(c->label());
-        nrChoices.reset(c->nrChoices());
+        nrChoices = c->nrChoices();
      }
    }

    // if label is already in correct order, just put together a choice on label
-    if (!highestLabel || !nrChoices || label > *highestLabel) {
+    if (!nrChoices || !highestLabel || label > *highestLabel) {
      boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
      for (Iterator it = begin; it != end; it++)
        choiceOnLabel->push_back(it->root_);
      return Choice::Unique(choiceOnLabel);
    } else {
      // Set up a new choice on the highest label
-      boost::shared_ptr<Choice> choiceOnHighestLabel(
-          new Choice(*highestLabel, *nrChoices));
+      boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, nrChoices));
      // now, for all possible values of highestLabel
-      for (size_t index = 0; index < *nrChoices; index++) {
+      for (size_t index = 0; index < nrChoices; index++) {
        // make a new set of functions for composing by iterating over the given
        // functions, and selecting the appropriate branch.
        std::vector<DecisionTree> functions;
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@ -21,7 +21,6 @@

 #pragma once

-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>

 namespace gtsam {
--- a/gtsam/geometry/OrientedPlane3.cpp
+++ b/gtsam/geometry/OrientedPlane3.cpp
@ -73,7 +73,7 @@ Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const {
 }

 /* ************************************************************************* */
-OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {
+OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
  // Retract the Unit3
  Vector2 n_v(v(0), v(1));
  Unit3 n_retracted = n_.retract(n_v);
@ -83,7 +83,7 @@ OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {

 /* ************************************************************************* */
 Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
-  Vector n_local = n_.localCoordinates(y.n_);
+  Vector2 n_local = n_.localCoordinates(y.n_);
  double d_local = d_ - y.d_;
  Vector3 e;
  e << n_local(0), n_local(1), -d_local;
--- a/gtsam/geometry/OrientedPlane3.h
+++ b/gtsam/geometry/OrientedPlane3.h
@ -51,7 +51,7 @@ public:
      n_(s), d_(d) {
  }

-  OrientedPlane3(Vector vec) :
+  OrientedPlane3(const Vector4& vec) :
      n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
  }

@ -89,7 +89,7 @@ public:
  }

  /// The retract function
-  OrientedPlane3 retract(const Vector& v) const;
+  OrientedPlane3 retract(const Vector3& v) const;

  /// The local coordinates function
  Vector3 localCoordinates(const OrientedPlane3& s) const;
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@ -270,7 +270,10 @@ public:
  /// print
  void print(const std::string& s = "PinholePose") const {
    Base::print(s);
-    K_->print(s + ".calibration");
+    if (!K_)
+      std::cout << "s No calibration given" << std::endl;
+    else
+      K_->print(s + ".calibration");
  }

  /// @}
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@ -112,7 +112,9 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
 /* ************************************************************************* */
 /** Modified from Murray94book version (which assumes w and v normalized?) */
 Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) {
-  if (H) *H = ExpmapDerivative(xi);
+  if (H) {
+    *H = ExpmapDerivative(xi);
+  }

  // get angular velocity omega and translational velocity v from twist xi
  Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
@ -254,6 +256,14 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
  return J;
 }

+/* ************************************************************************* */
+const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
+  if (H) {
+    *H << Z_3x3, rotation().matrix();
+  }
+  return t_;
+}
+
 /* ************************************************************************* */
 Matrix4 Pose3::matrix() const {
  const Matrix3 R = R_.matrix();
@ -280,8 +290,9 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
    Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    (*Dpose) << DR, R;
  }
-  if (Dpoint)
+  if (Dpoint) {
    *Dpoint = R_.matrix();
+  }
  return R_ * p + t_;
 }

@ -299,17 +310,18 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
        +wz, 0.0, -wx, 0.0,-1.0, 0.0,
        -wy, +wx, 0.0, 0.0, 0.0,-1.0;
  }
-  if (Dpoint)
+  if (Dpoint) {
    *Dpoint = Rt;
+  }
  return q;
 }

 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
    OptionalJacobian<1, 3> H2) const {
-  if (!H1 && !H2)
+  if (!H1 && !H2) {
    return transform_to(point).norm();
-  else {
+  } else {
    Matrix36 D1;
    Matrix3 D2;
    Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@ -223,9 +223,7 @@ public:
  }

  /// get translation
-  const Point3& translation() const {
-    return t_;
-  }
+  const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const;

  /// get x
  double x() const {
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@ -119,7 +119,7 @@ Matrix3 Unit3::skew() const {
 }

 /* ************************************************************************* */
-Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
+Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
  // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
  Matrix23 Bt = basis().transpose();
  Vector2 xi = Bt * q.p_.vector();
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@ -108,7 +108,7 @@ public:
  }

  /// Return unit-norm Vector
-  Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
+  Vector3 unitVector(boost::optional<Matrix&> H = boost::none) const {
    if (H)
      *H = basis();
    return (p_.vector());
@ -120,7 +120,7 @@ public:
  }

  /// Signed, vector-valued error between two directions
-  Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
+  Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;

  /// Distance between two directions
  double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
--- a/gtsam/geometry/tests/testCalibratedCamera.cpp
+++ b/gtsam/geometry/tests/testCalibratedCamera.cpp
@ -90,11 +90,11 @@ TEST( CalibratedCamera, project)
 /* ************************************************************************* */
 TEST( CalibratedCamera, Dproject_to_camera1) {
  Point3 pp(155,233,131);
-  Matrix test1;
-  CalibratedCamera::project_to_camera(pp, test1);
-  Matrix test2 = numericalDerivative11<Point2,Point3>(
+  Matrix actual;
+  CalibratedCamera::project_to_camera(pp, actual);
+  Matrix expected_numerical = numericalDerivative11<Point2,Point3>(
      boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp);
-  CHECK(assert_equal(test1, test2));
+  CHECK(assert_equal(expected_numerical, actual));
 }

 /* ************************************************************************* */
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@ -186,6 +186,17 @@ TEST(Pose3, expmaps_galore_full)
  EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));
 }

+/* ************************************************************************* */
+// Check translation and its pushforward
+TEST(Pose3, translation) {
+  Matrix actualH;
+  EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
+
+  Matrix numericalH = numericalDerivative11<Point3, Pose3>(
+      boost::bind(&Pose3::translation, _1, boost::none), T);
+  EXPECT(assert_equal(numericalH, actualH, 1e-6));
+}
+
 /* ************************************************************************* */
 TEST(Pose3, Adjoint_compose_full)
 {
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@ -23,14 +23,8 @@

 namespace gtsam {

-/**
- * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
- * @param projection_matrices Projection matrices (K*P^-1)
- * @param measurements 2D measurements
- * @param rank_tol SVD rank tolerance
- * @return Triangulated Point3
- */
-Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
+Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34>& projection_matrices,
    const std::vector<Point2>& measurements, double rank_tol) {

  // number of cameras
@ -57,7 +51,16 @@ Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
  if (rank < 3)
    throw(TriangulationUnderconstrainedException());

-  // Create 3D point from eigenvector
+  return v;
+}
+
+Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
+    const std::vector<Point2>& measurements, double rank_tol) {
+
+  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
+      rank_tol);
+
+  // Create 3D point from homogeneous coordinates
  return Point3(sub((v / v(3)), 0, 3));
 }

@ -89,6 +92,5 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
  return result.at<Point3>(landmarkKey);
 }

-
 } // \namespace gtsam

--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@ -18,7 +18,6 @@

 #pragma once

-
 #include <gtsam/slam/TriangulationFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -43,6 +42,17 @@ public:
  }
 };

+/**
+ * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
+ * @param projection_matrices Projection matrices (K*P^-1)
+ * @param measurements 2D measurements
+ * @param rank_tol SVD rank tolerance
+ * @return Triangulated point, in homogeneous coordinates
+ */
+GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34>& projection_matrices,
+    const std::vector<Point2>& measurements, double rank_tol = 1e-9);
+
 /**
 * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
 * @param projection_matrices Projection matrices (K*P^-1)
@ -52,7 +62,7 @@ public:
 */
 GTSAM_EXPORT Point3 triangulateDLT(
    const std::vector<Matrix34>& projection_matrices,
-    const std::vector<Point2>& measurements, double rank_tol);
+    const std::vector<Point2>& measurements, double rank_tol = 1e-9);

 ///
 /**
@ -164,6 +174,25 @@ Point3 triangulateNonlinear(
  return optimize(graph, values, Symbol('p', 0));
 }

+/**
+ * Create a 3*4 camera projection matrix from calibration and pose.
+ * Functor for partial application on calibration
+ * @param pose The camera pose
+ * @param cal  The calibration
+ * @return Returns a Matrix34
+ */
+template<class CALIBRATION>
+struct CameraProjectionMatrix {
+  CameraProjectionMatrix(const CALIBRATION& calibration) :
+      K_(calibration.K()) {
+  }
+  Matrix34 operator()(const Pose3& pose) const {
+    return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
+  }
+private:
+  const Matrix3 K_;
+};
+
 /**
 * Function to triangulate 3D landmark point from an arbitrary number
 * of poses (at least 2) using the DLT. The function checks that the
@ -188,10 +217,10 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,

  // construct projection matrices from poses & calibration
  std::vector<Matrix34> projection_matrices;
-  BOOST_FOREACH(const Pose3& pose, poses) {
-    projection_matrices.push_back(
-        sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0));
-  }
+  CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
+  BOOST_FOREACH(const Pose3& pose, poses)
+    projection_matrices.push_back(createP(pose));
+
  // Triangulate linearly
  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);

@ -204,7 +233,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
  BOOST_FOREACH(const Pose3& pose, poses) {
    const Point3& p_local = pose.transform_to(point);
    if (p_local.z() <= 0)
-    throw(TriangulationCheiralityException());
+      throw(TriangulationCheiralityException());
  }
 #endif

@ -230,7 +259,7 @@ Point3 triangulatePoint3(
    bool optimize = false) {

  size_t m = cameras.size();
-  assert(measurements.size()==m);
+  assert(measurements.size() == m);

  if (m < 2)
    throw(TriangulationUnderconstrainedException());
@ -238,10 +267,10 @@ Point3 triangulatePoint3(
  // construct projection matrices from poses & calibration
  typedef PinholeCamera<CALIBRATION> Camera;
  std::vector<Matrix34> projection_matrices;
-  BOOST_FOREACH(const Camera& camera, cameras) {
-  Matrix P_ = (camera.pose().inverse().matrix());
-    projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) );
-   }
+  BOOST_FOREACH(const Camera& camera, cameras)
+    projection_matrices.push_back(
+        CameraProjectionMatrix<CALIBRATION>(camera.calibration())(
+            camera.pose()));
  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);

  // The n refine using non-linear optimization
@ -253,7 +282,7 @@ Point3 triangulatePoint3(
  BOOST_FOREACH(const Camera& camera, cameras) {
    const Point3& p_local = camera.pose().transform_to(point);
    if (p_local.z() <= 0)
-    throw(TriangulationCheiralityException());
+      throw(TriangulationCheiralityException());
  }
 #endif

--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@ -365,32 +365,50 @@ void JacobianFactor::print(const string& s,
 /* ************************************************************************* */
 // Check if two linear factors are equal
 bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const {
-  if (!dynamic_cast<const JacobianFactor*>(&f_))
+  static const bool verbose = false;
+  if (!dynamic_cast<const JacobianFactor*>(&f_)) {
+    if (verbose)
+      cout << "JacobianFactor::equals: Incorrect type" << endl;
    return false;
-  else {
+  } else {
    const JacobianFactor& f(static_cast<const JacobianFactor&>(f_));

    // Check keys
-    if (keys() != f.keys())
+    if (keys() != f.keys()) {
+      if (verbose)
+        cout << "JacobianFactor::equals: keys do not match" << endl;
      return false;
+    }

    // Check noise model
-    if ((model_ && !f.model_) || (!model_ && f.model_))
+    if ((model_ && !f.model_) || (!model_ && f.model_)) {
+      if (verbose)
+        cout << "JacobianFactor::equals: noise model mismatch" << endl;
      return false;
-    if (model_ && f.model_ && !model_->equals(*f.model_, tol))
+    }
+    if (model_ && f.model_ && !model_->equals(*f.model_, tol)) {
+      if (verbose)
+        cout << "JacobianFactor::equals: noise modesl are not equal" << endl;
      return false;
+    }

    // Check matrix sizes
-    if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols()))
+    if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) {
+      if (verbose)
+        cout << "JacobianFactor::equals: augmented size mismatch" << endl;
      return false;
+    }

    // Check matrix contents
    constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
    constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
    for (size_t row = 0; row < (size_t) Ab1.rows(); ++row)
      if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol)
-          && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol))
+          && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) {
+        if (verbose)
+          cout << "JacobianFactor::equals: matrix mismatch at row " << row << endl;
        return false;
+      }

    return true;
  }
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@ -28,6 +28,7 @@
 #include <iostream>
 #include <typeinfo>
 #include <stdexcept>
+#include <cmath>

 using namespace std;

@ -70,6 +71,11 @@ boost::optional<Vector> checkIfDiagonal(const Matrix M) {
  }
 }

+/* ************************************************************************* */
+Vector Base::sigmas() const {
+  throw("Base::sigmas: sigmas() not implemented for this noise model");
+}
+
 /* ************************************************************************* */
 Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
  size_t m = R.rows(), n = R.cols();
@ -79,24 +85,25 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
  if (smart)
    diagonal = checkIfDiagonal(R);
  if (diagonal)
-    return Diagonal::Sigmas(reciprocal(*diagonal), true);
+    return Diagonal::Sigmas(diagonal->array().inverse(), true);
  else
    return shared_ptr(new Gaussian(R.rows(), R));
 }

 /* ************************************************************************* */
-Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) {
-  size_t m = M.rows(), n = M.cols();
+Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart) {
+  size_t m = information.rows(), n = information.cols();
  if (m != n)
    throw invalid_argument("Gaussian::Information: R not square");
  boost::optional<Vector> diagonal = boost::none;
  if (smart)
-    diagonal = checkIfDiagonal(M);
+    diagonal = checkIfDiagonal(information);
  if (diagonal)
    return Diagonal::Precisions(*diagonal, true);
  else {
-    Matrix R = RtR(M);
-    return shared_ptr(new Gaussian(R.rows(), R));
+    Eigen::LLT<Matrix> llt(information);
+    Matrix R = llt.matrixU();
+    return shared_ptr(new Gaussian(n, R));
  }
 }

@ -111,13 +118,15 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
    variances = checkIfDiagonal(covariance);
  if (variances)
    return Diagonal::Variances(*variances, true);
-  else
-    return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
+  else {
+    // TODO: can we do this more efficiently and still get an upper triangular nmatrix??
+    return Information(covariance.inverse(), false);
+  }
 }

 /* ************************************************************************* */
 void Gaussian::print(const string& name) const {
-  gtsam::print(thisR(), "Gaussian");
+  gtsam::print(thisR(), name + "Gaussian");
 }

 /* ************************************************************************* */
@ -129,6 +138,12 @@ bool Gaussian::equals(const Base& expected, double tol) const {
  return equal_with_abs_tol(R(), p->R(), sqrt(tol));
 }

+/* ************************************************************************* */
+Vector Gaussian::sigmas() const {
+  // TODO(frank): can this be done faster?
+  return Vector((thisR().transpose() * thisR()).inverse().diagonal()).cwiseSqrt();
+}
+
 /* ************************************************************************* */
 Vector Gaussian::whiten(const Vector& v) const {
  return thisR() * v;
@ -221,9 +236,11 @@ Diagonal::Diagonal() :
 }

 /* ************************************************************************* */
-Diagonal::Diagonal(const Vector& sigmas) :
-    Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_(
-        emul(invsigmas_, invsigmas_)) {
+Diagonal::Diagonal(const Vector& sigmas)
+    : Gaussian(sigmas.size()),
+      sigmas_(sigmas),
+      invsigmas_(sigmas.array().inverse()),
+      precisions_(invsigmas_.array().square()) {
 }

 /* ************************************************************************* */
@ -262,12 +279,12 @@ void Diagonal::print(const string& name) const {

 /* ************************************************************************* */
 Vector Diagonal::whiten(const Vector& v) const {
-  return emul(v, invsigmas());
+  return v.cwiseProduct(invsigmas_);
 }

 /* ************************************************************************* */
 Vector Diagonal::unwhiten(const Vector& v) const {
-  return emul(v, sigmas_);
+  return v.cwiseProduct(sigmas_);
 }

 /* ************************************************************************* */
@ -293,7 +310,7 @@ namespace internal {
 // switch precisions and invsigmas to finite value
 // TODO: why?? And, why not just ask s==0.0 below ?
 static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
-  for (size_t i = 0; i < sigmas.size(); ++i)
+  for (Vector::Index i = 0; i < sigmas.size(); ++i)
    if (!std::isfinite(1. / sigmas[i])) {
      precisions[i] = 0.0;
      invsigmas[i] = 0.0;
@ -342,7 +359,7 @@ Vector Constrained::whiten(const Vector& v) const {
  assert (b.size()==a.size());
  Vector c(n);
  for( size_t i = 0; i < n; i++ ) {
-    const double& ai = a(i), &bi = b(i);
+    const double& ai = a(i), bi = b(i);
    c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
  }
  return c;
@ -404,8 +421,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
  list<Triple> Rd;

  Vector pseudo(m); // allocate storage for pseudo-inverse
-  Vector invsigmas = reciprocal(sigmas_);
-  Vector weights = emul(invsigmas,invsigmas); // calculate weights once
+  Vector invsigmas = sigmas_.array().inverse();
+  Vector weights = invsigmas.array().square(); // calculate weights once

  // We loop over all columns, because the columns that can be eliminated
  // are not necessarily contiguous. For each one, estimate the corresponding
@ -542,16 +559,6 @@ Vector Base::weight(const Vector &error) const {
  return w;
 }

-/** square root version of the weight function */
-Vector Base::sqrtWeight(const Vector &error) const {
-  const size_t n = error.rows();
-  Vector w(n);
-  for ( size_t i = 0 ; i < n ; ++i )
-    w(i) = sqrtWeight(error(i));
-  return w;
-}
-
-
 /** The following three functions reweight block matrices and a vector
 * according to their weight implementation */

@ -560,8 +567,7 @@ void Base::reweight(Vector& error) const {
    const double w = sqrtWeight(error.norm());
    error *= w;
  } else {
-    const Vector w = sqrtWeight(error);
-    error.array() *= w.array();
+    error.array() *= weight(error).cwiseSqrt().array();
  }
 }

@ -579,7 +585,7 @@ void Base::reweight(vector<Matrix> &A, Vector &error) const {
    BOOST_FOREACH(Matrix& Aj, A) {
      vector_scale_inplace(W,Aj);
    }
-    error = emul(W, error);
+    error = W.cwiseProduct(error);
  }
 }

@ -593,7 +599,7 @@ void Base::reweight(Matrix &A, Vector &error) const {
  else {
    const Vector W = sqrtWeight(error);
    vector_scale_inplace(W,A);
-    error = emul(W, error);
+    error = W.cwiseProduct(error);
  }
 }

@ -609,7 +615,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const {
    const Vector W = sqrtWeight(error);
    vector_scale_inplace(W,A1);
    vector_scale_inplace(W,A2);
-    error = emul(W, error);
+    error = W.cwiseProduct(error);
  }
 }

@ -627,7 +633,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
    vector_scale_inplace(W,A1);
    vector_scale_inplace(W,A2);
    vector_scale_inplace(W,A3);
-    error = emul(W, error);
+    error = W.cwiseProduct(error);
  }
 }

@ -641,7 +647,7 @@ void Null::print(const std::string &s="") const
 Null::shared_ptr Null::Create()
 { return shared_ptr(new Null()); }

-Fair::Fair(const double c, const ReweightScheme reweight)
+Fair::Fair(double c, const ReweightScheme reweight)
  : Base(reweight), c_(c) {
  if ( c_ <= 0 ) {
    cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl;
@ -653,26 +659,26 @@ Fair::Fair(const double c, const ReweightScheme reweight)
 // Fair
 /* ************************************************************************* */

-double Fair::weight(const double &error) const
+double Fair::weight(double error) const
 { return 1.0 / (1.0 + fabs(error)/c_); }

 void Fair::print(const std::string &s="") const
 { cout << s << "fair (" << c_ << ")" << endl; }

-bool Fair::equals(const Base &expected, const double tol) const {
+bool Fair::equals(const Base &expected, double tol) const {
  const Fair* p = dynamic_cast<const Fair*> (&expected);
  if (p == NULL) return false;
  return fabs(c_ - p->c_ ) < tol;
 }

-Fair::shared_ptr Fair::Create(const double c, const ReweightScheme reweight)
+Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight)
 { return shared_ptr(new Fair(c, reweight)); }

 /* ************************************************************************* */
 // Huber
 /* ************************************************************************* */

-Huber::Huber(const double k, const ReweightScheme reweight)
+Huber::Huber(double k, const ReweightScheme reweight)
  : Base(reweight), k_(k) {
  if ( k_ <= 0 ) {
    cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl;
@ -680,7 +686,7 @@ Huber::Huber(const double k, const ReweightScheme reweight)
  }
 }

-double Huber::weight(const double &error) const {
+double Huber::weight(double error) const {
  return (error < k_) ? (1.0) : (k_ / fabs(error));
 }

@ -688,13 +694,13 @@ void Huber::print(const std::string &s="") const {
  cout << s << "huber (" << k_ << ")" << endl;
 }

-bool Huber::equals(const Base &expected, const double tol) const {
+bool Huber::equals(const Base &expected, double tol) const {
  const Huber* p = dynamic_cast<const Huber*>(&expected);
  if (p == NULL) return false;
  return fabs(k_ - p->k_) < tol;
 }

-Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) {
+Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) {
  return shared_ptr(new Huber(c, reweight));
 }

@ -702,7 +708,7 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) {
 // Cauchy
 /* ************************************************************************* */

-Cauchy::Cauchy(const double k, const ReweightScheme reweight)
+Cauchy::Cauchy(double k, const ReweightScheme reweight)
  : Base(reweight), k_(k) {
  if ( k_ <= 0 ) {
    cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl;
@ -710,7 +716,7 @@ Cauchy::Cauchy(const double k, const ReweightScheme reweight)
  }
 }

-double Cauchy::weight(const double &error) const {
+double Cauchy::weight(double error) const {
  return k_*k_ / (k_*k_ + error*error);
 }

@ -718,24 +724,24 @@ void Cauchy::print(const std::string &s="") const {
  cout << s << "cauchy (" << k_ << ")" << endl;
 }

-bool Cauchy::equals(const Base &expected, const double tol) const {
+bool Cauchy::equals(const Base &expected, double tol) const {
  const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
  if (p == NULL) return false;
  return fabs(k_ - p->k_) < tol;
 }

-Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) {
+Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) {
  return shared_ptr(new Cauchy(c, reweight));
 }

 /* ************************************************************************* */
 // Tukey
 /* ************************************************************************* */
-Tukey::Tukey(const double c, const ReweightScheme reweight)
+Tukey::Tukey(double c, const ReweightScheme reweight)
  : Base(reweight), c_(c) {
 }

-double Tukey::weight(const double &error) const {
+double Tukey::weight(double error) const {
  if (fabs(error) <= c_) {
    double xc2 = (error/c_)*(error/c_);
    double one_xc22 = (1.0-xc2)*(1.0-xc2);
@ -748,24 +754,24 @@ void Tukey::print(const std::string &s="") const {
  std::cout << s << ": Tukey (" << c_ << ")" << std::endl;
 }

-bool Tukey::equals(const Base &expected, const double tol) const {
+bool Tukey::equals(const Base &expected, double tol) const {
  const Tukey* p = dynamic_cast<const Tukey*>(&expected);
  if (p == NULL) return false;
  return fabs(c_ - p->c_) < tol;
 }

-Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) {
+Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
  return shared_ptr(new Tukey(c, reweight));
 }

 /* ************************************************************************* */
 // Welsh
 /* ************************************************************************* */
-Welsh::Welsh(const double c, const ReweightScheme reweight)
+Welsh::Welsh(double c, const ReweightScheme reweight)
  : Base(reweight), c_(c) {
 }

-double Welsh::weight(const double &error) const {
+double Welsh::weight(double error) const {
  double xc2 = (error/c_)*(error/c_);
  return std::exp(-xc2);
 }
@ -774,13 +780,13 @@ void Welsh::print(const std::string &s="") const {
  std::cout << s << ": Welsh (" << c_ << ")" << std::endl;
 }

-bool Welsh::equals(const Base &expected, const double tol) const {
+bool Welsh::equals(const Base &expected, double tol) const {
  const Welsh* p = dynamic_cast<const Welsh*>(&expected);
  if (p == NULL) return false;
  return fabs(c_ - p->c_) < tol;
 }

-Welsh::shared_ptr Welsh::Create(const double c, const ReweightScheme reweight) {
+Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) {
  return shared_ptr(new Welsh(c, reweight));
 }

--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@ -26,7 +26,6 @@
 #include <boost/serialization/singleton.hpp>
 #include <boost/serialization/shared_ptr.hpp>
 #include <boost/serialization/optional.hpp>
-#include <cmath>

 namespace gtsam {

@ -59,7 +58,7 @@ namespace gtsam {

    public:

-      /** primary constructor @param dim is the dimension of the model */
+      /// primary constructor @param dim is the dimension of the model
      Base(size_t dim = 1):dim_(dim) {}
      virtual ~Base() {}

@ -75,14 +74,13 @@ namespace gtsam {

      virtual bool equals(const Base& expected, double tol=1e-9) const = 0;

-      /**
-       * Whiten an error vector.
-       */
+      /// Calculate standard deviations
+      virtual Vector sigmas() const;
+
+      /// Whiten an error vector.
      virtual Vector whiten(const Vector& v) const = 0;

-      /**
-       * Unwhiten an error vector.
-       */
+      /// Unwhiten an error vector.
      virtual Vector unwhiten(const Vector& v) const = 0;

      virtual double distance(const Vector& v) const = 0;
@ -189,6 +187,7 @@ namespace gtsam {

      virtual void print(const std::string& name) const;
      virtual bool equals(const Base& expected, double tol=1e-9) const;
+      virtual Vector sigmas() const;
      virtual Vector whiten(const Vector& v) const;
      virtual Vector unwhiten(const Vector& v) const;

@ -220,9 +219,9 @@ namespace gtsam {
      /**
       * Whiten a system, in place as well
       */
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const ;
-      virtual void WhitenSystem(Matrix& A, Vector& b) const ;
-      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ;
+      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
+      virtual void WhitenSystem(Matrix& A, Vector& b) const;
+      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;

      /**
@ -234,11 +233,15 @@ namespace gtsam {
       */
      virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const;

-      /**
-       * Return R itself, but note that Whiten(H) is cheaper than R*H
-       */
+      /// Return R itself, but note that Whiten(H) is cheaper than R*H
      virtual Matrix R() const { return thisR();}

+      /// Compute information matrix
+      virtual Matrix information() const { return thisR().transpose() * thisR(); }
+
+      /// Compute covariance matrix
+      virtual Matrix covariance() const { return information().inverse(); }
+
    private:
      /** Serialization function */
      friend class boost::serialization::access;
@ -303,6 +306,7 @@ namespace gtsam {
      }

      virtual void print(const std::string& name) const;
+      virtual Vector sigmas() const { return sigmas_; }
      virtual Vector whiten(const Vector& v) const;
      virtual Vector unwhiten(const Vector& v) const;
      virtual Matrix Whiten(const Matrix& H) const;
@ -312,7 +316,6 @@ namespace gtsam {
      /**
       * Return standard deviations (sqrt of diagonal)
       */
-      inline const Vector& sigmas() const { return sigmas_; }
      inline double sigma(size_t i) const { return sigmas_(i); }

      /**
@ -629,20 +632,23 @@ namespace gtsam {
        virtual ~Base() {}

        /// robust error function to implement
-        virtual double weight(const double &error) const = 0;
+        virtual double weight(double error) const = 0;

        virtual void print(const std::string &s) const = 0;
-        virtual bool equals(const Base& expected, const double tol=1e-8) const = 0;
+        virtual bool equals(const Base& expected, double tol=1e-8) const = 0;

-        inline double sqrtWeight(const double &error) const
-        { return std::sqrt(weight(error)); }
+        double sqrtWeight(double error) const {
+          return std::sqrt(weight(error));
+        }

        /** produce a weight vector according to an error vector and the implemented
        * robust function */
        Vector weight(const Vector &error) const;

        /** square root version of the weight function */
-        Vector sqrtWeight(const Vector &error) const;
+        Vector sqrtWeight(const Vector &error) const {
+          return weight(error).cwiseSqrt();
+        }

        /** reweight block matrices and a vector according to their weight implementation */
        void reweight(Vector &error) const;
@ -667,9 +673,9 @@ namespace gtsam {

        Null(const ReweightScheme reweight = Block) : Base(reweight) {}
        virtual ~Null() {}
-        virtual double weight(const double& /*error*/) const { return 1.0; }
+        virtual double weight(double /*error*/) const { return 1.0; }
        virtual void print(const std::string &s) const;
-        virtual bool equals(const Base& /*expected*/, const double /*tol*/) const { return true; }
+        virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
        static shared_ptr Create() ;

      private:
@ -686,12 +692,12 @@ namespace gtsam {
      public:
        typedef boost::shared_ptr<Fair> shared_ptr;

-        Fair(const double c = 1.3998, const ReweightScheme reweight = Block);
+        Fair(double c = 1.3998, const ReweightScheme reweight = Block);
        virtual ~Fair() {}
-        virtual double weight(const double &error) const ;
-        virtual void print(const std::string &s) const ;
-        virtual bool equals(const Base& expected, const double tol=1e-8) const ;
-        static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ;
+        virtual double weight(double error) const;
+        virtual void print(const std::string &s) const;
+        virtual bool equals(const Base& expected, double tol=1e-8) const;
+        static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;

      protected:
        double c_;
@ -712,11 +718,11 @@ namespace gtsam {
        typedef boost::shared_ptr<Huber> shared_ptr;

        virtual ~Huber() {}
-        Huber(const double k = 1.345, const ReweightScheme reweight = Block);
-        virtual double weight(const double &error) const ;
-        virtual void print(const std::string &s) const ;
-        virtual bool equals(const Base& expected, const double tol=1e-8) const ;
-        static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ;
+        Huber(double k = 1.345, const ReweightScheme reweight = Block);
+        virtual double weight(double error) const;
+        virtual void print(const std::string &s) const;
+        virtual bool equals(const Base& expected, double tol=1e-8) const;
+        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      protected:
        double k_;
@ -741,11 +747,11 @@ namespace gtsam {
        typedef boost::shared_ptr<Cauchy> shared_ptr;

        virtual ~Cauchy() {}
-        Cauchy(const double k = 0.1, const ReweightScheme reweight = Block);
-        virtual double weight(const double &error) const ;
-        virtual void print(const std::string &s) const ;
-        virtual bool equals(const Base& expected, const double tol=1e-8) const ;
-        static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ;
+        Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
+        virtual double weight(double error) const;
+        virtual void print(const std::string &s) const;
+        virtual bool equals(const Base& expected, double tol=1e-8) const;
+        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      protected:
        double k_;
@ -765,12 +771,12 @@ namespace gtsam {
      public:
        typedef boost::shared_ptr<Tukey> shared_ptr;

-        Tukey(const double c = 4.6851, const ReweightScheme reweight = Block);
+        Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
        virtual ~Tukey() {}
-        virtual double weight(const double &error) const ;
-        virtual void print(const std::string &s) const ;
-        virtual bool equals(const Base& expected, const double tol=1e-8) const ;
-        static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ;
+        virtual double weight(double error) const;
+        virtual void print(const std::string &s) const;
+        virtual bool equals(const Base& expected, double tol=1e-8) const;
+        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      protected:
        double c_;
@ -790,12 +796,12 @@ namespace gtsam {
      public:
        typedef boost::shared_ptr<Welsh> shared_ptr;

-        Welsh(const double c = 2.9846, const ReweightScheme reweight = Block);
+        Welsh(double c = 2.9846, const ReweightScheme reweight = Block);
        virtual ~Welsh() {}
-        virtual double weight(const double &error) const ;
-        virtual void print(const std::string &s) const ;
-        virtual bool equals(const Base& expected, const double tol=1e-8) const ;
-        static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ;
+        virtual double weight(double error) const;
+        virtual void print(const std::string &s) const;
+        virtual bool equals(const Base& expected, double tol=1e-8) const;
+        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      protected:
        double c_;
--- a/gtsam/linear/SubgraphPreconditioner.h
+++ b/gtsam/linear/SubgraphPreconditioner.h
@ -70,7 +70,7 @@ namespace gtsam {
    Subgraph(const std::vector<size_t> &indices) ;

    inline const Edges& edges() const { return edges_; }
-    inline const size_t size() const { return edges_.size(); }
+    inline size_t size() const { return edges_.size(); }
    EdgeIndices edgeIndices() const;

    iterator begin() { return edges_.begin(); }
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@ -33,15 +33,16 @@ using namespace gtsam;
 using namespace noiseModel;
 using namespace boost::assign;

-static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var;
-static Matrix R = (Matrix(3, 3) <<
+static const double kSigma = 2, s_1=1.0/kSigma, kVariance = kSigma*kSigma, prc = 1.0/kVariance;
+static const Matrix R = (Matrix(3, 3) <<
    s_1, 0.0, 0.0,
    0.0, s_1, 0.0,
    0.0, 0.0, s_1).finished();
-static Matrix Sigma = (Matrix(3, 3) <<
-    var, 0.0, 0.0,
-    0.0, var, 0.0,
-    0.0, 0.0, var).finished();
+static const Matrix kCovariance = (Matrix(3, 3) <<
+    kVariance, 0.0, 0.0,
+    0.0, kVariance, 0.0,
+    0.0, 0.0, kVariance).finished();
+static const Vector3 kSigmas(kSigma, kSigma, kSigma);

 //static double inf = numeric_limits<double>::infinity();

@ -53,15 +54,19 @@ TEST(NoiseModel, constructors)

  // Construct noise models
  vector<Gaussian::shared_ptr> m;
-  m.push_back(Gaussian::SqrtInformation(R));
-  m.push_back(Gaussian::Covariance(Sigma));
-  //m.push_back(Gaussian::Information(Q));
-  m.push_back(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()));
-  m.push_back(Diagonal::Variances((Vector(3) << var, var, var).finished()));
-  m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished()));
-  m.push_back(Isotropic::Sigma(3, sigma));
-  m.push_back(Isotropic::Variance(3, var));
-  m.push_back(Isotropic::Precision(3, prc));
+  m.push_back(Gaussian::SqrtInformation(R,false));
+  m.push_back(Gaussian::Covariance(kCovariance,false));
+  m.push_back(Gaussian::Information(kCovariance.inverse(),false));
+  m.push_back(Diagonal::Sigmas(kSigmas,false));
+  m.push_back(Diagonal::Variances((Vector(3) << kVariance, kVariance, kVariance).finished(),false));
+  m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished(),false));
+  m.push_back(Isotropic::Sigma(3, kSigma,false));
+  m.push_back(Isotropic::Variance(3, kVariance,false));
+  m.push_back(Isotropic::Precision(3, prc,false));
+
+  // test kSigmas
+  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+    EXPECT(assert_equal(kSigmas,mi->sigmas()));

  // test whiten
  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
@ -117,9 +122,9 @@ TEST(NoiseModel, equals)
 {
  Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
                       g2 = Gaussian::SqrtInformation(eye(3,3));
-  Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()),
+  Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << kSigma, kSigma, kSigma).finished()),
                       d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
-  Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma),
+  Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma),
                        i2 = Isotropic::Sigma(3, 0.7);

  EXPECT(assert_equal(*g1,*g1));
@ -155,7 +160,7 @@ TEST(NoiseModel, ConstrainedConstructors )
  Constrained::shared_ptr actual;
  size_t d = 3;
  double m = 100.0;
-  Vector sigmas = (Vector(3) << sigma, 0.0, 0.0).finished();
+  Vector sigmas = (Vector(3) << kSigma, 0.0, 0.0).finished();
  Vector mu = Vector3(200.0, 300.0, 400.0);
  actual = Constrained::All(d);
  // TODO: why should this be a thousand ??? Dummy variable?
@ -182,7 +187,7 @@ TEST(NoiseModel, ConstrainedMixed )
 {
  Vector feasible = Vector3(1.0, 0.0, 1.0),
      infeasible = Vector3(1.0, 1.0, 1.0);
-  Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma).finished());
+  Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << kSigma, 0.0, kSigma).finished());
  // NOTE: we catch constrained variables elsewhere, so whitening does nothing
  EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible)));
  EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible)));
@ -357,6 +362,44 @@ TEST(NoiseModel, robustNoise)
  DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8);
 }

+/* ************************************************************************* */
+#define TEST_GAUSSIAN(gaussian)\
+  EQUALITY(info, gaussian->information());\
+  EQUALITY(cov, gaussian->covariance());\
+  EXPECT(assert_equal(white, gaussian->whiten(e)));\
+  EXPECT(assert_equal(e, gaussian->unwhiten(white)));\
+  EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\
+  Matrix A = R.inverse(); Vector b = e;\
+  gaussian->WhitenSystem(A, b);\
+  EXPECT(assert_equal(I, A));\
+  EXPECT(assert_equal(white, b));
+
+TEST(NoiseModel, NonDiagonalGaussian)
+{
+  Matrix3 R;
+  R << 6, 5, 4, 0, 3, 2, 0, 0, 1;
+  const Matrix3 info = R.transpose() * R;
+  const Matrix3 cov = info.inverse();
+  const Vector3 e(1, 1, 1), white = R * e;
+  Matrix I = Matrix3::Identity();
+
+
+  {
+  SharedGaussian gaussian = Gaussian::SqrtInformation(R);
+  TEST_GAUSSIAN(gaussian);
+  }
+
+  {
+  SharedGaussian gaussian = Gaussian::Information(info);
+  TEST_GAUSSIAN(gaussian);
+  }
+
+  {
+  SharedGaussian gaussian = Gaussian::Covariance(cov);
+  TEST_GAUSSIAN(gaussian);
+  }
+}
+
 /* ************************************************************************* */
 int main() {  TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@ -17,8 +17,10 @@

 #pragma once

+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/VectorSpace.h>
 #include <boost/serialization/nvp.hpp>
-#include <gtsam/geometry/Pose3.h>

 /*
 * NOTES:
@ -131,8 +133,8 @@ public:
 /// print with optional string
  void print(const std::string& s = "") const {
    // explicit printing for now.
-    std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl;
-    std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl;
+    std::cout << s + ".Acc  [" << biasAcc_.transpose() << "]" << std::endl;
+    std::cout << s + ".Gyro [" << biasGyro_.transpose() << "]" << std::endl;
  }

  /** equality up to tolerance */
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@ -44,7 +44,6 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
 //------------------------------------------------------------------------------
 void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
  PreintegrationBase::print(s);
-  cout << "  preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl;
 }

 //------------------------------------------------------------------------------
@ -75,8 +74,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
  const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement

  // Update Jacobians
-  updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
-      deltaT);
+  updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);

  // Update preintegrated measurements (also get Jacobian)
  const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
@ -89,8 +87,8 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
  // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
  // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
  // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
-  // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise,
-  // as G and measurementCovariance are blockdiagonal matrices
+  // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise,
+  // as G and measurementCovariance are block-diagonal matrices
  preintMeasCov_ = F * preintMeasCov_ * F.transpose();
  preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT;
  preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance()
@ -156,7 +154,8 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
      << ")\n";
  ImuFactorBase::print("");
  _PIM_.print("  preintegrated measurements:");
-  this->noiseModel_->print("  noise model: ");
+  // Print standard deviations on covariance only
+  cout << "  noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl;
 }

 //------------------------------------------------------------------------------
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@ -110,7 +110,7 @@ public:
     * Add a single IMU measurement to the preintegration.
     * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
     * @param measuredOmega Measured angular velocity (as given by the sensor)
-     * @param deltaT Time interval between two consecutive IMU measurements
+     * @param deltaT Time interval between this and the last IMU measurement
     * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
     * @param Fout, Gout Jacobians used internally (only needed for testing)
     */
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@ -69,11 +69,8 @@ public:
  /// Needed for testable
  void print(const std::string& s) const {
    std::cout << s << std::endl;
-    std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl;
-    deltaRij_.print("  deltaRij ");
-    std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl;
-    std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]"
-        << std::endl;
+    std::cout << "    deltaTij [" << deltaTij_ << "]" << std::endl;
+    std::cout << "    deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl;
  }

  /// Needed for testable
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@ -0,0 +1,357 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  PreintegrationBase.h
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#include "PreintegrationBase.h"
+
+namespace gtsam {
+
+PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias,
+                                       const Matrix3& measuredAccCovariance,
+                                       const Matrix3& measuredOmegaCovariance,
+                                       const Matrix3&integrationErrorCovariance,
+                                       const bool use2ndOrderIntegration)
+    : PreintegratedRotation(measuredOmegaCovariance),
+      biasHat_(bias),
+      use2ndOrderIntegration_(use2ndOrderIntegration),
+      deltaPij_(Vector3::Zero()),
+      deltaVij_(Vector3::Zero()),
+      delPdelBiasAcc_(Z_3x3),
+      delPdelBiasOmega_(Z_3x3),
+      delVdelBiasAcc_(Z_3x3),
+      delVdelBiasOmega_(Z_3x3),
+      accelerometerCovariance_(measuredAccCovariance),
+      integrationCovariance_(integrationErrorCovariance) {
+}
+
+/// Needed for testable
+void PreintegrationBase::print(const std::string& s) const {
+  PreintegratedRotation::print(s);
+  std::cout << "    deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
+  std::cout << "    deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
+  biasHat_.print("    biasHat");
+}
+
+/// Needed for testable
+bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const {
+  return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol)
+      && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
+      && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
+      && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
+      && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
+      && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
+      && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol)
+      && equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol)
+      && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol);
+}
+
+/// Re-initialize PreintegratedMeasurements
+void PreintegrationBase::resetIntegration() {
+  PreintegratedRotation::resetIntegration();
+  deltaPij_ = Vector3::Zero();
+  deltaVij_ = Vector3::Zero();
+  delPdelBiasAcc_ = Z_3x3;
+  delPdelBiasOmega_ = Z_3x3;
+  delVdelBiasAcc_ = Z_3x3;
+  delVdelBiasOmega_ = Z_3x3;
+}
+
+/// Update preintegrated measurements
+void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc,
+                                                         const Rot3& incrR, const double deltaT,
+                                                         OptionalJacobian<9, 9> F) {
+
+  const Matrix3 dRij = deltaRij();  // expensive
+  const Vector3 temp = dRij * correctedAcc * deltaT;
+  if (!use2ndOrderIntegration_) {
+    deltaPij_ += deltaVij_ * deltaT;
+  } else {
+    deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
+  }
+  deltaVij_ += temp;
+
+  Matrix3 R_i, F_angles_angles;
+  if (F)
+    R_i = deltaRij();  // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
+  updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
+
+  if (F) {
+    const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
+    Matrix3 F_pos_angles;
+    if (use2ndOrderIntegration_)
+      F_pos_angles = 0.5 * F_vel_angles * deltaT;
+    else
+      F_pos_angles = Z_3x3;
+
+    //    pos  vel             angle
+    *F <<  //
+        I_3x3, I_3x3 * deltaT, F_pos_angles,    // pos
+        Z_3x3, I_3x3,          F_vel_angles,    // vel
+        Z_3x3, Z_3x3,          F_angles_angles; // angle
+  }
+}
+
+/// Update Jacobians to be used during preintegration
+void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc,
+                                                      const Matrix3& D_Rincr_integratedOmega,
+                                                      const Rot3& incrR, double deltaT) {
+  const Matrix3 dRij = deltaRij();  // expensive
+  const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
+  if (!use2ndOrderIntegration_) {
+    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
+    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
+  } else {
+    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
+    delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
+  }
+  delVdelBiasAcc_ += -dRij * deltaT;
+  delVdelBiasOmega_ += temp;
+  update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
+}
+
+void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
+    const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc,
+    Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
+  correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+
+  // Then compensate for sensor-body displacement: we express the quantities
+  // (originally in the IMU frame) into the body frame
+  if (body_P_sensor) {
+    Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
+    correctedOmega = body_R_sensor * correctedOmega;  // rotation rate vector in the body frame
+    Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
+    correctedAcc = body_R_sensor * correctedAcc
+        - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
+    // linear acceleration vector in the body frame
+  }
+}
+
+/// Predict state at time j
+//------------------------------------------------------------------------------
+PoseVelocityBias PreintegrationBase::predict(
+    const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
+    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
+    boost::optional<Vector3&> deltaPij_biascorrected_out,
+    boost::optional<Vector3&> deltaVij_biascorrected_out) const {
+
+  const imuBias::ConstantBias biasIncr = bias_i - biasHat();
+  const Vector3& biasAccIncr = biasIncr.accelerometer();
+  const Vector3& biasOmegaIncr = biasIncr.gyroscope();
+
+  const Rot3& Rot_i = pose_i.rotation();
+  const Matrix3 Rot_i_matrix = Rot_i.matrix();
+  const Vector3 pos_i = pose_i.translation().vector();
+
+  // Predict state at time j
+  /* ---------------------------------------------------------------------------------------------------- */
+  const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
+      + delPdelBiasOmega() * biasOmegaIncr;
+  if (deltaPij_biascorrected_out)  // if desired, store this
+    *deltaPij_biascorrected_out = deltaPij_biascorrected;
+
+  const double dt = deltaTij(), dt2 = dt * dt;
+  Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt
+      - omegaCoriolis.cross(vel_i) * dt2  // Coriolis term - we got rid of the 2 wrt ins paper
+  + 0.5 * gravity * dt2;
+
+  const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr
+      + delVdelBiasOmega() * biasOmegaIncr;
+  if (deltaVij_biascorrected_out)  // if desired, store this
+    *deltaVij_biascorrected_out = deltaVij_biascorrected;
+
+  Vector3 vel_j = Vector3(
+      vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt  // Coriolis term
+      + gravity * dt);
+
+  if (use2ndOrderCoriolis) {
+    pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2;  // 2nd order coriolis term for position
+    vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt;  // 2nd order term for velocity
+  }
+
+  const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
+  // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
+
+  const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
+  const Vector3 correctedOmega = biascorrectedOmega
+      - Rot_i_matrix.transpose() * omegaCoriolis * dt;  // Coriolis term
+  const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
+  const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij);
+
+  const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j));
+  return PoseVelocityBias(pose_j, vel_j, bias_i);  // bias is predicted as a constant
+}
+
+/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j
+//------------------------------------------------------------------------------
+Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
+                                                     const Pose3& pose_j, const Vector3& vel_j,
+                                                     const imuBias::ConstantBias& bias_i,
+                                                     const Vector3& gravity,
+                                                     const Vector3& omegaCoriolis,
+                                                     const bool use2ndOrderCoriolis,
+                                                     OptionalJacobian<9, 6> H1,
+                                                     OptionalJacobian<9, 3> H2,
+                                                     OptionalJacobian<9, 6> H3,
+                                                     OptionalJacobian<9, 3> H4,
+                                                     OptionalJacobian<9, 6> H5) const {
+
+  // We need the mismatch w.r.t. the biases used for preintegration
+  const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
+
+  // we give some shorter name to rotations and translations
+  const Rot3& Ri = pose_i.rotation();
+  const Matrix3 Ri_transpose_matrix = Ri.transpose();
+
+  const Rot3& Rj = pose_j.rotation();
+  const Vector3 pos_j = pose_j.translation().vector();
+
+  // Evaluate residual error, according to [3]
+  /* ---------------------------------------------------------------------------------------------------- */
+  Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
+  PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
+                                              use2ndOrderCoriolis, deltaPij_biascorrected,
+                                              deltaVij_biascorrected);
+
+  // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
+  const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector());
+
+  // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
+  const Vector3 fv = Ri_transpose_matrix * (vel_j - predictedState_j.velocity);
+
+  // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
+
+  /* ---------------------------------------------------------------------------------------------------- */
+  // Get Get so<3> version of bias corrected rotation
+  // If H5 is asked for, we will need the Jacobian, which we store in H5
+  // H5 will then be corrected below to take into account the Coriolis effect
+  Matrix3 D_cThetaRij_biasOmegaIncr;
+  const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr,
+                                                           H5 ? &D_cThetaRij_biasOmegaIncr : 0);
+
+  // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
+  const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
+  const Vector3 correctedOmega = biascorrectedOmega - coriolis;
+
+  // Calculate Jacobians, matrices below needed only for some Jacobians...
+  Vector3 fR;
+  Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat;
+
+  if (H1 || H2)
+    Ritranspose_omegaCoriolisHat = Ri_transpose_matrix * skewSymmetric(omegaCoriolis);
+
+  // This is done to save computation: we ask for the jacobians only when they are needed
+  const double dt = deltaTij(), dt2 = dt*dt;
+  Rot3 fRrot;
+  const Rot3 RiBetweenRj = Rot3(Ri_transpose_matrix) * Rj;
+  if (H1 || H2 || H3 || H4 || H5) {
+    const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega);
+    // Residual rotation error
+    fRrot = correctedDeltaRij.between(RiBetweenRj);
+    fR = Rot3::Logmap(fRrot, D_fR_fRrot);
+    D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
+  } else {
+    const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
+    // Residual rotation error
+    fRrot = correctedDeltaRij.between(RiBetweenRj);
+    fR = Rot3::Logmap(fRrot);
+  }
+  if (H1) {
+    Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3;
+    if (use2ndOrderCoriolis) {
+      // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
+      const Matrix3 temp = Ritranspose_omegaCoriolisHat
+          * (-Ritranspose_omegaCoriolisHat.transpose());
+      dfPdPi += 0.5 * temp * dt2;
+      dfVdPi += temp * dt;
+    }
+    (*H1) <<
+    // dfP/dRi
+    skewSymmetric(fp + deltaPij_biascorrected),
+    // dfP/dPi
+    dfPdPi,
+    // dfV/dRi
+    skewSymmetric(fv + deltaVij_biascorrected),
+    // dfV/dPi
+    dfVdPi,
+    // dfR/dRi
+    D_fR_fRrot * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
+    // dfR/dPi
+    Z_3x3;
+  }
+  if (H2) {
+    (*H2) <<
+    // dfP/dVi
+    -Ri_transpose_matrix * dt + Ritranspose_omegaCoriolisHat * dt2,  // Coriolis term - we got rid of the 2 wrt ins paper
+    // dfV/dVi
+    -Ri_transpose_matrix + 2 * Ritranspose_omegaCoriolisHat * dt,  // Coriolis term
+    // dfR/dVi
+    Z_3x3;
+  }
+  if (H3) {
+    (*H3) <<
+    // dfP/dPosej
+    Z_3x3, Ri_transpose_matrix * Rj.matrix(),
+    // dfV/dPosej
+    Matrix::Zero(3, 6),
+    // dfR/dPosej
+    D_fR_fRrot, Z_3x3;
+  }
+  if (H4) {
+    (*H4) <<
+    // dfP/dVj
+    Z_3x3,
+    // dfV/dVj
+    Ri_transpose_matrix,
+    // dfR/dVj
+    Z_3x3;
+  }
+  if (H5) {
+    // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
+    const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
+    (*H5) <<
+    // dfP/dBias
+    -delPdelBiasAcc(), -delPdelBiasOmega(),
+    // dfV/dBias
+    -delVdelBiasAcc(), -delVdelBiasOmega(),
+    // dfR/dBias
+    Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega);
+  }
+  Vector9 r;
+  r << fp, fv, fR;
+  return r;
+}
+
+ImuBase::ImuBase()
+    : gravity_(Vector3(0.0, 0.0, 9.81)),
+      omegaCoriolis_(Vector3(0.0, 0.0, 0.0)),
+      body_P_sensor_(boost::none),
+      use2ndOrderCoriolis_(false) {
+}
+
+ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
+                 boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis)
+    : gravity_(gravity),
+      omegaCoriolis_(omegaCoriolis),
+      body_P_sensor_(body_P_sensor),
+      use2ndOrderCoriolis_(use2ndOrderCoriolis) {
+}
+
+}  /// namespace gtsam
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@ -23,6 +23,9 @@

 #include <gtsam/navigation/PreintegratedRotation.h>
 #include <gtsam/navigation/ImuBias.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>

 namespace gtsam {

@ -34,9 +37,10 @@ struct PoseVelocityBias {
  Vector3 velocity;
  imuBias::ConstantBias bias;

-  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
-      const imuBias::ConstantBias _bias) :
-      pose(_pose), velocity(_velocity), bias(_bias) {
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias)
+      : pose(_pose),
+        velocity(_velocity),
+        bias(_bias) {
  }
 };

@ -46,24 +50,24 @@ struct PoseVelocityBias {
 * It includes the definitions of the preintegrated variables and the methods
 * to access, print, and compare them.
 */
-class PreintegrationBase: public PreintegratedRotation {
+class PreintegrationBase : public PreintegratedRotation {

-  imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
-  bool use2ndOrderIntegration_; ///< Controls the order of integration
+  imuBias::ConstantBias biasHat_;  ///< Acceleration and angular rate bias values used during preintegration
+  bool use2ndOrderIntegration_;  ///< Controls the order of integration

-  Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
-  Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
+  Vector3 deltaPij_;  ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+  Vector3 deltaVij_;  ///< Preintegrated relative velocity (in global frame)

-  Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
-  Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
-  Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
-  Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
+  Matrix3 delPdelBiasAcc_;  ///< Jacobian of preintegrated position w.r.t. acceleration bias
+  Matrix3 delPdelBiasOmega_;  ///< Jacobian of preintegrated position w.r.t. angular rate bias
+  Matrix3 delVdelBiasAcc_;  ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
+  Matrix3 delVdelBiasOmega_;  ///< Jacobian of preintegrated velocity w.r.t. angular rate bias

-  Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements
-  Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty
+  const Matrix3 accelerometerCovariance_;  ///< continuous-time "Covariance" of accelerometer measurements
+  const Matrix3 integrationCovariance_;  ///< continuous-time "Covariance" describing integration uncertainty
  /// (to compensate errors in Euler integration)

-public:
+ public:

  /**
   *  Default constructor, initializes the variables in the base class
@ -71,18 +75,9 @@ public:
   *  @param use2ndOrderIntegration     Controls the order of integration
   *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
   */
-  PreintegrationBase(const imuBias::ConstantBias& bias,
-      const Matrix3& measuredAccCovariance,
-      const Matrix3& measuredOmegaCovariance,
-      const Matrix3&integrationErrorCovariance,
-      const bool use2ndOrderIntegration) :
-      PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias), use2ndOrderIntegration_(
-          use2ndOrderIntegration), deltaPij_(Vector3::Zero()), deltaVij_(
-          Vector3::Zero()), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), delVdelBiasAcc_(
-          Z_3x3), delVdelBiasOmega_(Z_3x3), accelerometerCovariance_(
-          measuredAccCovariance), integrationCovariance_(
-          integrationErrorCovariance) {
-  }
+  PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
+                     const Matrix3& measuredOmegaCovariance,
+                     const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration);

  /// methods to access class variables
  bool use2ndOrderIntegration() const {
@ -99,7 +94,7 @@ public:
  }
  Vector6 biasHatVector() const {
    return biasHat_.vector();
-  } // expensive
+  }  // expensive
  const Matrix3& delPdelBiasAcc() const {
    return delPdelBiasAcc_;
  }
@ -120,319 +115,48 @@ public:
    return integrationCovariance_;
  }

-  /// Needed for testable
-  void print(const std::string& s) const {
-    PreintegratedRotation::print(s);
-    std::cout << "  accelerometerCovariance [ " << accelerometerCovariance_
-        << " ]" << std::endl;
-    std::cout << "  integrationCovariance [ " << integrationCovariance_ << " ]"
-        << std::endl;
-    std::cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
-    std::cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
-    biasHat_.print("  biasHat");
-  }
+  /// print
+  void print(const std::string& s) const;

-  /// Needed for testable
-  bool equals(const PreintegrationBase& other, double tol) const {
-    return PreintegratedRotation::equals(other, tol)
-        && biasHat_.equals(other.biasHat_, tol)
-        && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
-        && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
-        && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
-        && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
-        && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
-        && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol)
-        && equal_with_abs_tol(accelerometerCovariance_,
-            other.accelerometerCovariance_, tol)
-        && equal_with_abs_tol(integrationCovariance_,
-            other.integrationCovariance_, tol);
-  }
+  /// check equality
+  bool equals(const PreintegrationBase& other, double tol) const;

  /// Re-initialize PreintegratedMeasurements
-  void resetIntegration() {
-    PreintegratedRotation::resetIntegration();
-    deltaPij_ = Vector3::Zero();
-    deltaVij_ = Vector3::Zero();
-    delPdelBiasAcc_ = Z_3x3;
-    delPdelBiasOmega_ = Z_3x3;
-    delVdelBiasAcc_ = Z_3x3;
-    delVdelBiasOmega_ = Z_3x3;
-  }
+  void resetIntegration();

  /// Update preintegrated measurements
-  void updatePreintegratedMeasurements(const Vector3& correctedAcc,
-      const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) {
-
-    Matrix3 dRij = deltaRij(); // expensive
-    Vector3 temp = dRij * correctedAcc * deltaT;
-    if (!use2ndOrderIntegration_) {
-      deltaPij_ += deltaVij_ * deltaT;
-    } else {
-      deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
-    }
-    deltaVij_ += temp;
-
-    Matrix3 R_i, F_angles_angles;
-    if (F)
-      R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
-    updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
-
-    if (F) {
-      Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
-      Matrix3 F_pos_angles;
-      if (use2ndOrderIntegration_)
-        F_pos_angles = 0.5 * F_vel_angles * deltaT;
-      else
-        F_pos_angles = Z_3x3;
-
-      //    pos          vel              angle
-      *F << //
-          I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
-      Z_3x3, I_3x3, F_vel_angles, // vel
-      Z_3x3, Z_3x3, F_angles_angles; // angle
-    }
-  }
+  void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR,
+                                       const double deltaT, OptionalJacobian<9, 9> F);

  /// Update Jacobians to be used during preintegration
  void updatePreintegratedJacobians(const Vector3& correctedAcc,
-      const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
-      double deltaT) {
-    Matrix3 dRij = deltaRij(); // expensive
-    Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
-        * delRdelBiasOmega();
-    if (!use2ndOrderIntegration_) {
-      delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
-      delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
-    } else {
-      delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT
-          - 0.5 * dRij * deltaT * deltaT;
-      delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
-    }
-    delVdelBiasAcc_ += -dRij * deltaT;
-    delVdelBiasOmega_ += temp;
-    update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
-  }
+                                    const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
+                                    double deltaT);

  void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
-      const Vector3& measuredOmega, Vector3& correctedAcc,
-      Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
-    correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
-    correctedOmega = biasHat_.correctGyroscope(measuredOmega);
-
-    // Then compensate for sensor-body displacement: we express the quantities
-    // (originally in the IMU frame) into the body frame
-    if (body_P_sensor) {
-      Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
-      correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
-      Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
-      correctedAcc = body_R_sensor * correctedAcc
-          - body_omega_body__cross * body_omega_body__cross
-              * body_P_sensor->translation().vector();
-      // linear acceleration vector in the body frame
-    }
-  }
+                                              const Vector3& measuredOmega, Vector3& correctedAcc,
+                                              Vector3& correctedOmega,
+                                              boost::optional<const Pose3&> body_P_sensor);

  /// Predict state at time j
-  //------------------------------------------------------------------------------
-  PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias_i, const Vector3& gravity,
-      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+  PoseVelocityBias predict(
+      const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
+      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
-      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
-
-    const imuBias::ConstantBias biasIncr = bias_i - biasHat();
-    const Vector3& biasAccIncr = biasIncr.accelerometer();
-    const Vector3& biasOmegaIncr = biasIncr.gyroscope();
-
-    const Rot3& Rot_i = pose_i.rotation();
-    const Vector3& pos_i = pose_i.translation().vector();
-
-    // Predict state at time j
-    /* ---------------------------------------------------------------------------------------------------- */
-    Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
-        + delPdelBiasOmega() * biasOmegaIncr;
-    if (deltaPij_biascorrected_out) // if desired, store this
-      *deltaPij_biascorrected_out = deltaPij_biascorrected;
-
-    Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected
-        + vel_i * deltaTij()
-        - omegaCoriolis.cross(vel_i) * deltaTij() * deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
-    + 0.5 * gravity * deltaTij() * deltaTij();
-
-    Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr
-        + delVdelBiasOmega() * biasOmegaIncr;
-    if (deltaVij_biascorrected_out) // if desired, store this
-      *deltaVij_biascorrected_out = deltaVij_biascorrected;
-
-    Vector3 vel_j = Vector3(
-        vel_i + Rot_i.matrix() * deltaVij_biascorrected
-            - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
-        + gravity * deltaTij());
-
-    if (use2ndOrderCoriolis) {
-      pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i))
-          * deltaTij() * deltaTij(); // 2nd order coriolis term for position
-      vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
-    }
-
-    const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
-    // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-    Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
-    Vector3 correctedOmega = biascorrectedOmega
-        - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
-    const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
-    const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij);
-
-    Pose3 pose_j = Pose3(Rot_j, Point3(pos_j));
-    return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
-  }
+      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const;

  /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
-  //------------------------------------------------------------------------------
-  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
-      const Pose3& pose_j, const Vector3& vel_j,
-      const imuBias::ConstantBias& bias_i, const Vector3& gravity,
-      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
-      OptionalJacobian<9, 6> H1 = boost::none, OptionalJacobian<9, 3> H2 =
-          boost::none, OptionalJacobian<9, 6> H3 = boost::none,
-      OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 =
-          boost::none) const {
+  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,
+                                   const Vector3& vel_j, const imuBias::ConstantBias& bias_i,
+                                   const Vector3& gravity, const Vector3& omegaCoriolis,
+                                   const bool use2ndOrderCoriolis, OptionalJacobian<9, 6> H1 =
+                                       boost::none,
+                                   OptionalJacobian<9, 3> H2 = boost::none,
+                                   OptionalJacobian<9, 6> H3 = boost::none,
+                                   OptionalJacobian<9, 3> H4 = boost::none,
+                                   OptionalJacobian<9, 6> H5 = boost::none) const;

-    // We need the mismatch w.r.t. the biases used for preintegration
-    const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
-
-    // we give some shorter name to rotations and translations
-    const Rot3& Ri = pose_i.rotation();
-    const Rot3& Rj = pose_j.rotation();
-    const Vector3& pos_j = pose_j.translation().vector();
-
-    // Evaluate residual error, according to [3]
-    /* ---------------------------------------------------------------------------------------------------- */
-    Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
-    PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
-        omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected,
-        deltaVij_biascorrected);
-
-    // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
-    const Vector3 fp = Ri.transpose()
-        * (pos_j - predictedState_j.pose.translation().vector());
-
-    // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
-    const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity);
-
-    // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
-
-    /* ---------------------------------------------------------------------------------------------------- */
-    // Get Get so<3> version of bias corrected rotation
-    // If H5 is asked for, we will need the Jacobian, which we store in H5
-    // H5 will then be corrected below to take into account the Coriolis effect
-    Matrix3 D_cThetaRij_biasOmegaIncr;
-    Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr,
-        H5 ? &D_cThetaRij_biasOmegaIncr : 0);
-
-    // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
-    const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
-    Vector3 correctedOmega = biascorrectedOmega - coriolis;
-
-    // Calculate Jacobians, matrices below needed only for some Jacobians...
-    Vector3 fR;
-    Rot3 correctedDeltaRij, fRrot;
-    Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot,
-        Ritranspose_omegaCoriolisHat;
-
-    if (H1 || H2)
-      Ritranspose_omegaCoriolisHat = Ri.transpose()
-          * skewSymmetric(omegaCoriolis);
-
-    // This is done to save computation: we ask for the jacobians only when they are needed
-    if (H1 || H2 || H3 || H4 || H5) {
-      correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega);
-      // Residual rotation error
-      fRrot = correctedDeltaRij.between(Ri.between(Rj));
-      fR = Rot3::Logmap(fRrot, D_fR_fRrot);
-      D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
-    } else {
-      correctedDeltaRij = Rot3::Expmap(correctedOmega);
-      // Residual rotation error
-      fRrot = correctedDeltaRij.between(Ri.between(Rj));
-      fR = Rot3::Logmap(fRrot);
-    }
-    if (H1) {
-      H1->resize(9, 6);
-      Matrix3 dfPdPi = -I_3x3;
-      Matrix3 dfVdPi = Z_3x3;
-      if (use2ndOrderCoriolis) {
-        // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
-        Matrix3 temp = Ritranspose_omegaCoriolisHat
-            * (-Ritranspose_omegaCoriolisHat.transpose());
-        dfPdPi += 0.5 * temp * deltaTij() * deltaTij();
-        dfVdPi += temp * deltaTij();
-      }
-      (*H1) <<
-      // dfP/dRi
-          skewSymmetric(fp + deltaPij_biascorrected),
-      // dfP/dPi
-      dfPdPi,
-      // dfV/dRi
-      skewSymmetric(fv + deltaVij_biascorrected),
-      // dfV/dPi
-      dfVdPi,
-      // dfR/dRi
-      D_fR_fRrot
-          * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
-      // dfR/dPi
-      Z_3x3;
-    }
-    if (H2) {
-      H2->resize(9, 3);
-      (*H2) <<
-      // dfP/dVi
-          -Ri.transpose() * deltaTij()
-              + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
-                  // dfV/dVi
-      -Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
-          // dfR/dVi
-      Z_3x3;
-    }
-    if (H3) {
-      H3->resize(9, 6);
-      (*H3) <<
-      // dfP/dPosej
-          Z_3x3, Ri.transpose() * Rj.matrix(),
-      // dfV/dPosej
-      Matrix::Zero(3, 6),
-      // dfR/dPosej
-      D_fR_fRrot, Z_3x3;
-    }
-    if (H4) {
-      H4->resize(9, 3);
-      (*H4) <<
-      // dfP/dVj
-          Z_3x3,
-      // dfV/dVj
-      Ri.transpose(),
-      // dfR/dVj
-      Z_3x3;
-    }
-    if (H5) {
-      // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
-      const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
-      H5->resize(9, 6);
-      (*H5) <<
-      // dfP/dBias
-          -delPdelBiasAcc(), -delPdelBiasOmega(),
-      // dfV/dBias
-      -delVdelBiasAcc(), -delVdelBiasOmega(),
-      // dfR/dBias
-      Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega);
-    }
-    Vector9 r;
-    r << fp, fv, fR;
-    return r;
-  }
-
-private:
+ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
@ -450,26 +174,22 @@ private:

 class ImuBase {

-protected:
+ protected:

  Vector3 gravity_;
  Vector3 omegaCoriolis_;
-  boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
-  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
+  boost::optional<Pose3> body_P_sensor_;  ///< The pose of the sensor in the body frame
+  bool use2ndOrderCoriolis_;  ///< Controls whether higher order terms are included when calculating the Coriolis Effect

-public:
+ public:

-  ImuBase() :
-      gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_(
-          boost::none), use2ndOrderCoriolis_(false) {
-  }
+  /// Default constructor, with decent gravity and zero coriolis
+  ImuBase();

+  /// Fully fledge constructor
  ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
-      boost::optional<const Pose3&> body_P_sensor = boost::none,
-      const bool use2ndOrderCoriolis = false) :
-      gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
-          body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {
-  }
+          boost::optional<const Pose3&> body_P_sensor = boost::none,
+          const bool use2ndOrderCoriolis = false);

  const Vector3& gravity() const {
    return gravity_;
@ -480,4 +200,4 @@ public:

 };

-} /// namespace gtsam
+}  /// namespace gtsam
--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/gtsam/navigation/tests/testImuFactor.cpp
@ -17,6 +17,7 @@

 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Pose3.h>
@ -81,19 +82,27 @@ Rot3 updatePreintegratedRot(const Rot3& deltaRij_old,
  return deltaRij_new;
 }

-// Auxiliary functions to test preintegrated Jacobians
-// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
+// Define covariance matrices
 /* ************************************************************************* */
 double accNoiseVar = 0.01;
 double omegaNoiseVar = 0.03;
 double intNoiseVar = 0.0001;
+const Matrix3 kMeasuredAccCovariance = accNoiseVar * Matrix3::Identity();
+const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * Matrix3::Identity();
+const Matrix3 kIntegrationErrorCovariance = intNoiseVar * Matrix3::Identity();
+
+// Auxiliary functions to test preintegrated Jacobians
+// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
+/* ************************************************************************* */
 ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
    const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
    const bool use2ndOrderIntegration = false) {
  ImuFactor::PreintegratedMeasurements result(bias,
-      accNoiseVar * Matrix3::Identity(), omegaNoiseVar * Matrix3::Identity(),
-      intNoiseVar * Matrix3::Identity(), use2ndOrderIntegration);
+                                              kMeasuredAccCovariance,
+                                              kMeasuredOmegaCovariance,
+                                              kIntegrationErrorCovariance,
+                                              use2ndOrderIntegration);

  list<Vector3>::const_iterator itAcc = measuredAccs.begin();
  list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -156,8 +165,11 @@ TEST( ImuFactor, PreintegratedMeasurements ) {

  bool use2ndOrderIntegration = true;
  // Actual preintegrated values
-  ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(),
-      Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
+  ImuFactor::PreintegratedMeasurements actual1(bias,
+                                               kMeasuredAccCovariance,
+                                               kMeasuredOmegaCovariance,
+                                               kIntegrationErrorCovariance,
+                                               use2ndOrderIntegration);
  actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  EXPECT(
@ -208,8 +220,11 @@ TEST( ImuFactor, ErrorAndJacobians ) {
  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector();
  double deltaT = 1.0;
  bool use2ndOrderIntegration = true;
-  ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(),
-      Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
+  ImuFactor::PreintegratedMeasurements pre_int_data(bias,
+                                                    kMeasuredAccCovariance,
+                                                    kMeasuredOmegaCovariance,
+                                                    kIntegrationErrorCovariance,
+                                                    use2ndOrderIntegration);
  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
@ -260,6 +275,40 @@ TEST( ImuFactor, ErrorAndJacobians ) {
  Matrix H5e = numericalDerivative11<Vector, imuBias::ConstantBias>(
      boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
  EXPECT(assert_equal(H5e, H5a));
+
+  ////////////////////////////////////////////////////////////////////////////
+  // Evaluate error with wrong values
+  Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1);
+  errorActual = factor.evaluateError(x1, v1, x2, v2_wrong, bias);
+  errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0;
+  EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
+
+  Values values;
+  values.insert(X(1), x1);
+  values.insert(V(1), v1);
+  values.insert(X(2), x2);
+  values.insert(V(2), v2_wrong);
+  values.insert(B(1), bias);
+  errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0;
+  EXPECT(assert_equal(factor.unwhitenedError(values), errorActual, 1e-6));
+
+  // Make sure the whitening is done correctly
+  Matrix cov = pre_int_data.preintMeasCov();
+  Matrix R = RtR(cov.inverse());
+  Vector whitened = R * errorActual;
+  EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6));
+
+  ///////////////////////////////////////////////////////////////////////////////
+  // Make sure linearization is correct
+  // Create expected value by numerical differentiation
+  JacobianFactor expected = linearizeNumerically(factor, values, 1e-8);
+
+  // Create actual value by linearize
+  GaussianFactor::shared_ptr linearized = factor.linearize(values);
+  JacobianFactor* actual = dynamic_cast<JacobianFactor*>(linearized.get());
+
+  // Check cast result and then equality
+  EXPECT(assert_equal(expected, *actual, 1e-4));
 }

 /* ************************************************************************* */
@ -284,8 +333,8 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) {
  double deltaT = 1.0;

  ImuFactor::PreintegratedMeasurements pre_int_data(
-      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
-      Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
+      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
@ -354,8 +403,8 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) {
  double deltaT = 1.0;

  ImuFactor::PreintegratedMeasurements pre_int_data(
-      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
-      Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
+      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
@ -874,8 +923,8 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
      Point3(1, 0, 0));

  ImuFactor::PreintegratedMeasurements pre_int_data(
-      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
-      Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
+      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance);

  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

@ -933,8 +982,8 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
  I6x6 = Matrix::Identity(6, 6);

  ImuFactor::PreintegratedMeasurements pre_int_data(
-      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
-      Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
+      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true);

  for (int i = 0; i < 1000; ++i)
    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -974,8 +1023,8 @@ TEST(ImuFactor, PredictRotation) {
  I6x6 = Matrix::Identity(6, 6);

  ImuFactor::PreintegratedMeasurements pre_int_data(
-      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
-      Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
+      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true);

  for (int i = 0; i < 1000; ++i)
    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
--- a/gtsam/nonlinear/Expression-inl.h
+++ b/gtsam/nonlinear/Expression-inl.h
@ -19,773 +19,244 @@

 #pragma once

-#include <gtsam/nonlinear/CallRecord.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/base/Lie.h>
+#include <gtsam/nonlinear/internal/ExpressionNode.h>

-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <boost/bind.hpp>
-#include <boost/type_traits/aligned_storage.hpp>
-
-// template meta-programming headers
-#include <boost/mpl/fold.hpp>
-namespace MPL = boost::mpl::placeholders;
-
-#include <typeinfo>       // operator typeid
-#include <map>
-
-class ExpressionFactorBinaryTest;
-// Forward declare for testing
+#include <boost/range/adaptor/map.hpp>
+#include <boost/range/algorithm.hpp>

 namespace gtsam {

-const unsigned TraceAlignment = 16;
-
 template<typename T>
-T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
-  // right now only word sized types are supported.
-  // Easy to extend if needed,
-  //   by somehow inferring the unsigned integer of same size
-  BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t));
-  size_t & uiValue = reinterpret_cast<size_t &>(value);
-  size_t misAlignment = uiValue % requiredAlignment;
-  if (misAlignment) {
-    uiValue += requiredAlignment - misAlignment;
-  }
-  return value;
-}
-template<typename T>
-T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
-  return upAlign(value, requiredAlignment);
+void Expression<T>::print(const std::string& s) const {
+  std::cout << s << *root_ << std::endl;
 }

 template<typename T>
-class Expression;
-
-/**
- * Expressions are designed to write their derivatives into an already allocated
- * Jacobian of the correct size, of type VerticalBlockMatrix.
- * The JacobianMap provides a mapping from keys to the underlying blocks.
- */
-class JacobianMap {
-  const FastVector<Key>& keys_;
-  VerticalBlockMatrix& Ab_;
-public:
-  JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
-      keys_(keys), Ab_(Ab) {
-  }
-  /// Access via key
-  VerticalBlockMatrix::Block operator()(Key key) {
-    FastVector<Key>::const_iterator it = std::find(keys_.begin(), keys_.end(),
-        key);
-    DenseIndex block = it - keys_.begin();
-    return Ab_(block);
-  }
-};
-
-//-----------------------------------------------------------------------------
-
-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;
-  }
-};
+Expression<T>::Expression(const T& value) :
+    root_(new internal::ConstantExpression<T>(value)) {
 }

-/// Handle Leaf Case: reverse AD 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);
+template<typename T>
+Expression<T>::Expression(const Key& key) :
+    root_(new internal::LeafExpression<T>(key)) {
 }

-//-----------------------------------------------------------------------------
-/**
- * The ExecutionTrace class records a tree-structured expression's execution.
- *
- * The class looks a bit complicated but it is so for performance.
- * It is a tagged union that obviates the need to create
- * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead
- * the key for the leaf is stored in the space normally used to store a
- * CallRecord*. Nothing is stored for a Constant.
- *
- * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be:
- * Trace(Function) ->
- *   BinaryRecord with two traces in it
- *     trace1(Function) ->
- *       UnaryRecord with one trace in it
- *         trace1(Function) ->
- *           BinaryRecord with two traces in it
- *             trace1(Leaf)
- *             trace2(Constant)
- *     trace2(Leaf)
- * Hence, there are three Record structs, written to memory by traceExecution
- */
-template<class T>
-class ExecutionTrace {
+template<typename T>
+Expression<T>::Expression(const Symbol& symbol) :
+    root_(new internal::LeafExpression<T>(symbol)) {
+}
+
+template<typename T>
+Expression<T>::Expression(unsigned char c, size_t j) :
+    root_(new internal::LeafExpression<T>(Symbol(c, j))) {
+}
+
+/// Construct a unary function expression
+template<typename T>
+template<typename A>
+Expression<T>::Expression(typename UnaryFunction<A>::type function,
+    const Expression<A>& expression) :
+    root_(new internal::UnaryExpression<T, A>(function, expression)) {
+}
+
+/// Construct a binary function expression
+template<typename T>
+template<typename A1, typename A2>
+Expression<T>::Expression(typename BinaryFunction<A1, A2>::type function,
+    const Expression<A1>& expression1, const Expression<A2>& expression2) :
+    root_(
+        new internal::BinaryExpression<T, A1, A2>(function, expression1,
+            expression2)) {
+}
+
+/// Construct a ternary function expression
+template<typename T>
+template<typename A1, typename A2, typename A3>
+Expression<T>::Expression(typename TernaryFunction<A1, A2, A3>::type function,
+    const Expression<A1>& expression1, const Expression<A2>& expression2,
+    const Expression<A3>& expression3) :
+    root_(
+        new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
+            expression2, expression3)) {
+}
+
+/// Construct a nullary method expression
+template<typename T>
+template<typename A>
+Expression<T>::Expression(const Expression<A>& expression,
+    T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
+    root_(
+        new internal::UnaryExpression<T, A>(boost::bind(method, _1, _2),
+            expression)) {
+}
+
+/// Construct a unary method expression
+template<typename T>
+template<typename A1, typename A2>
+Expression<T>::Expression(const Expression<A1>& expression1,
+    T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
+        typename MakeOptionalJacobian<T, A2>::type) const,
+    const Expression<A2>& expression2) :
+    root_(
+        new internal::BinaryExpression<T, A1, A2>(
+            boost::bind(method, _1, _2, _3, _4), expression1, expression2)) {
+}
+
+/// Construct a binary method expression
+template<typename T>
+template<typename A1, typename A2, typename A3>
+Expression<T>::Expression(const Expression<A1>& expression1,
+    T (A1::*method)(const A2&, const A3&,
+        typename MakeOptionalJacobian<T, A1>::type,
+        typename MakeOptionalJacobian<T, A2>::type,
+        typename MakeOptionalJacobian<T, A3>::type) const,
+    const Expression<A2>& expression2, const Expression<A3>& expression3) :
+    root_(
+        new internal::TernaryExpression<T, A1, A2, A3>(
+            boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
+            expression2, expression3)) {
+}
+
+template<typename T>
+std::set<Key> Expression<T>::keys() const {
+  return root_->keys();
+}
+
+template<typename T>
+void Expression<T>::dims(std::map<Key, int>& map) const {
+  root_->dims(map);
+}
+
+template<typename T>
+T Expression<T>::value(const Values& values,
+    boost::optional<std::vector<Matrix>&> H) const {
+
+  if (H) {
+    // Call private version that returns derivatives in H
+    KeysAndDims pair = keysAndDims();
+    return valueAndDerivatives(values, pair.first, pair.second, *H);
+  } else
+    // no derivatives needed, just return value
+    return root_->value(values);
+}
+
+template<typename T>
+const boost::shared_ptr<internal::ExpressionNode<T> >& Expression<T>::root() const {
+  return root_;
+}
+
+template<typename T>
+size_t Expression<T>::traceSize() const {
+  return root_->traceSize();
+}
+
+// Private methods:
+
+template<typename T>
+T Expression<T>::valueAndDerivatives(const Values& values,
+    const FastVector<Key>& keys, const FastVector<int>& dims,
+    std::vector<Matrix>& H) const {
+
+  // H should be pre-allocated
+  assert(H.size()==keys.size());
+
+  // Pre-allocate and zero VerticalBlockMatrix
  static const int Dim = traits<T>::dimension;
-  enum {
-    Constant, Leaf, Function
-  } kind;
-  union {
-    Key key;
-    CallRecord<Dim>* ptr;
-  } content;
-public:
-  /// Pointer always starts out as a Constant
-  ExecutionTrace() :
-      kind(Constant) {
-  }
-  /// Change pointer to a Leaf Record
-  void setLeaf(Key key) {
-    kind = Leaf;
-    content.key = key;
-  }
-  /// Take ownership of pointer to a Function Record
-  void setFunction(CallRecord<Dim>* record) {
-    kind = Function;
-    content.ptr = record;
-  }
-  /// Print
-  void print(const std::string& indent = "") const {
-    if (kind == Constant)
-      std::cout << indent << "Constant" << std::endl;
-    else if (kind == Leaf)
-      std::cout << indent << "Leaf, key = " << content.key << std::endl;
-    else if (kind == Function) {
-      std::cout << indent << "Function" << std::endl;
-      content.ptr->print(indent + "  ");
-    }
-  }
-  /// Return record pointer, quite unsafe, used only for testing
-  template<class Record>
-  boost::optional<Record*> record() {
-    if (kind != Function)
-      return boost::none;
-    else {
-      Record* p = dynamic_cast<Record*>(content.ptr);
-      return p ? boost::optional<Record*>(p) : boost::none;
-    }
-  }
-  /**
-   *  *** This is the main entry point for reverse AD, called from Expression ***
-   * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
-   */
-  typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
-  void startReverseAD1(JacobianMap& jacobians) const {
-    if (kind == Leaf) {
-      // This branch will only be called on trivial Leaf expressions, i.e. Priors
-      static const JacobianTT I = JacobianTT::Identity();
-      handleLeafCase(I, jacobians, content.key);
-    } else if (kind == Function)
-      // This is the more typical entry point, starting the AD pipeline
-      // Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
-      content.ptr->startReverseAD2(jacobians);
-  }
-  // Either add to Jacobians (Leaf) or propagate (Function)
-  template<typename DerivedMatrix>
-  void reverseAD1(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
-      JacobianMap& jacobians) const {
-    if (kind == Leaf)
-      handleLeafCase(dTdA, jacobians, content.key);
-    else if (kind == Function)
-      content.ptr->reverseAD2(dTdA, jacobians);
-  }
+  VerticalBlockMatrix Ab(dims, Dim);
+  Ab.matrix().setZero();
+  internal::JacobianMap jacobianMap(keys, Ab);

-  /// Define type so we can apply it as a meta-function
-  typedef ExecutionTrace<T> type;
-};
+  // Call unsafe version
+  T result = valueAndJacobianMap(values, jacobianMap);

-/// Storage type for the execution trace.
-/// It enforces the proper alignment in a portable way.
-/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
-typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
+  // Copy blocks into the vector of jacobians passed in
+  for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
+    H[i] = Ab(i);

-//-----------------------------------------------------------------------------
-/**
- * Expression node. The superclass for objects that do the heavy lifting
- * An Expression<T> has a pointer to an ExpressionNode<T> underneath
- * allowing Expressions to have polymorphic behaviour even though they
- * are passed by value. This is the same way boost::function works.
- * http://loki-lib.sourceforge.net/html/a00652.html
- */
-template<class T>
-class ExpressionNode {
+  return result;
+}

-protected:
-
-  size_t traceSize_;
-
-  /// Constructor, traceSize is size of the execution trace of expression rooted here
-  ExpressionNode(size_t traceSize = 0) :
-      traceSize_(traceSize) {
-  }
-
-public:
-
-  /// Destructor
-  virtual ~ExpressionNode() {
-  }
-
-  /// Streaming
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os,
-      const ExpressionNode& node) {
-    os << "Expression of type " << typeid(T).name();
-    if (node.traceSize_>0) os << ", trace size = " << node.traceSize_;
-    os << "\n";
-    return os;
-  }
-
-  /// Return keys that play in this expression as a set
-  virtual std::set<Key> keys() const {
-    std::set<Key> keys;
-    return keys;
-  }
-
-  /// Return dimensions for each argument, as a map
-  virtual void dims(std::map<Key, int>& map) const {
-  }
-
-  // Return size needed for memory buffer in traceExecution
-  size_t traceSize() const {
-    return traceSize_;
-  }
-
-  /// Return value
-  virtual T value(const Values& values) const = 0;
-
-  /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const = 0;
-};
-
-//-----------------------------------------------------------------------------
-/// Constant Expression
-template<class T>
-class ConstantExpression: public ExpressionNode<T> {
-
-  /// The constant value
-  T constant_;
-
-  /// Constructor with a value, yielding a constant
-  ConstantExpression(const T& value) :
-      constant_(value) {
-  }
-
-  friend class Expression<T> ;
-
-public:
-
-  /// Return value
-  virtual T value(const Values& values) const {
-    return constant_;
-  }
-
-  /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
-    return constant_;
-  }
-};
-
-
-//-----------------------------------------------------------------------------
-/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
 template<typename T>
-class LeafExpression : public ExpressionNode<T> {
-  typedef T value_type;
+T Expression<T>::traceExecution(const Values& values,
+    internal::ExecutionTrace<T>& trace, void* traceStorage) const {
+  return root_->traceExecution(values, trace,
+      static_cast<internal::ExecutionTraceStorage*>(traceStorage));
+}

-  /// The key into values
-  Key key_;
+template<typename T>
+T Expression<T>::valueAndJacobianMap(const Values& values,
+    internal::JacobianMap& jacobians) const {
+  // The following piece of code is absolutely crucial for performance.
+  // We allocate a block of memory on the stack, which can be done at runtime
+  // with modern C++ compilers. The traceExecution then fills this memory
+  // with an execution trace, made up entirely of "Record" structs, see
+  // the FunctionalNode class in expression-inl.h
+  size_t size = traceSize();

-  /// Constructor with a single key
-  LeafExpression(Key key) :
-      key_(key) {
-  }
-  // todo: do we need a virtual destructor here too?
-
-  friend class Expression<T> ;
-
-public:
-
-  /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
-    std::set<Key> keys;
-    keys.insert(key_);
-    return keys;
-  }
-
-  /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
-    map[key_] = traits<T>::dimension;
-  }
-
-  /// Return value
-  virtual T value(const Values& values) const {
-    return values.at<T>(key_);
-  }
-
-  /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
-    trace.setLeaf(key_);
-    return values.at<T>(key_);
-  }
-
-};
-
-//-----------------------------------------------------------------------------
-// Below we use the "Class Composition" technique described in the book
-//   C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
-//   and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education.
-// to recursively generate a class, that will be the base for function nodes.
-//
-// The class generated, for three arguments A1, A2, and A3 will be
-//
-// struct Base1 : Argument<T,A1,1>, FunctionalBase<T> {
-//   ... storage related to A1 ...
-//   ... methods that work on A1 ...
-// };
-//
-// struct Base2 : Argument<T,A2,2>, Base1 {
-//   ... storage related to A2 ...
-//   ... methods that work on A2 and (recursively) on A1 ...
-// };
-//
-// struct Base3 : Argument<T,A3,3>, Base2 {
-//   ... storage related to A3 ...
-//   ... methods that work on A3 and (recursively) on A2 and A1 ...
-// };
-//
-// struct FunctionalNode : Base3 {
-//   Provides convenience access to storage in hierarchy by using
-//   static_cast<Argument<T, A, N> &>(*this)
-// }
-//
-// All this magic happens when  we generate the Base3 base class of FunctionalNode
-// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode
-//
-// Similarly, the inner Record struct will be
-//
-// struct Record1 : JacobianTrace<T,A1,1>, CallRecord<traits::dimension<T>::value> {
-//   ... storage related to A1 ...
-//   ... methods that work on A1 ...
-// };
-//
-// struct Record2 : JacobianTrace<T,A2,2>, Record1 {
-//   ... storage related to A2 ...
-//   ... methods that work on A2 and (recursively) on A1 ...
-// };
-//
-// struct Record3 : JacobianTrace<T,A3,3>, Record2 {
-//   ... storage related to A3 ...
-//   ... methods that work on A3 and (recursively) on A2 and A1 ...
-// };
-//
-// struct Record : Record3 {
-//   Provides convenience access to storage in hierarchy by using
-//   static_cast<JacobianTrace<T, A, N> &>(*this)
-// }
-//
-
-//-----------------------------------------------------------------------------
-
-/// meta-function to generate fixed-size JacobianTA type
-template<class T, class A>
-struct Jacobian {
-  typedef Eigen::Matrix<double, traits<T>::dimension,
-      traits<A>::dimension> type;
-};
-
-/// meta-function to generate JacobianTA optional reference
-template<class T, class A>
-struct MakeOptionalJacobian {
-  typedef OptionalJacobian<traits<T>::dimension,
-      traits<A>::dimension> type;
-};
-
-/**
- * Base case for recursive FunctionalNode class
- */
-template<class T>
-struct FunctionalBase: ExpressionNode<T> {
-  static size_t const N = 0; // number of arguments
-
-  struct Record {
-    void print(const std::string& indent) const {
-    }
-    void startReverseAD4(JacobianMap& jacobians) const {
-    }
-    template<typename SomeMatrix>
-    void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
-    }
-  };
-  /// Construct an execution trace for reverse AD
-  void trace(const Values& values, Record* record,
-      ExecutionTraceStorage*& traceStorage) const {
-    // base case: does not do anything
-  }
-};
-
-/**
- * Building block for recursive FunctionalNode class
- * The integer argument N is to guarantee a unique type signature,
- * so we are guaranteed to be able to extract their values by static cast.
- */
-template<class T, class A, size_t N>
-struct Argument {
-  /// Expression that will generate value/derivatives for argument
-  boost::shared_ptr<ExpressionNode<A> > expression;
-};
-
-/**
- * Building block for Recursive Record Class
- * Records the evaluation of a single argument in a functional expression
- */
-template<class T, class A, size_t N>
-struct JacobianTrace {
-  A value;
-  ExecutionTrace<A> trace;
-  typename Jacobian<T, A>::type dTdA;
-};
-
-// Recursive Definition of Functional ExpressionNode
-// The reason we inherit from Argument<T, A, N> is because we can then
-// case to this unique signature to retrieve the expression at any level
-template<class T, class A, class Base>
-struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
-
-  static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy
-  typedef Argument<T, A, N> This; ///< The storage we have direct access to
-
-  /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
-    std::set<Key> keys = Base::keys();
-    std::set<Key> myKeys = This::expression->keys();
-    keys.insert(myKeys.begin(), myKeys.end());
-    return keys;
-  }
-
-  /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
-    Base::dims(map);
-    This::expression->dims(map);
-  }
-
-  // Recursive Record Class for Functional Expressions
-  // The reason we inherit from JacobianTrace<T, A, N> is because we can then
-  // case to this unique signature to retrieve the value/trace at any level
-  struct Record: JacobianTrace<T, A, N>, Base::Record {
-
-    typedef T return_type;
-    typedef JacobianTrace<T, A, N> This;
-
-    /// Print to std::cout
-    void print(const std::string& indent) const {
-      Base::Record::print(indent);
-      static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
-      std::cout << This::dTdA.format(matlab) << std::endl;
-      This::trace.print(indent);
-    }
-
-    /// Start the reverse AD process
-    void startReverseAD4(JacobianMap& jacobians) const {
-      Base::Record::startReverseAD4(jacobians);
-      // This is the crucial point where the size of the AD pipeline is selected.
-      // One pipeline is started for each argument, but the number of rows in each
-      // pipeline is the same, namely the dimension of the output argument T.
-      // For example, if the entire expression is rooted by a binary function
-      // yielding a 2D result, then the matrix dTdA will have 2 rows.
-      // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2
-      // which calls the correctly sized CallRecord::reverseAD3, which in turn
-      // calls reverseAD4 below.
-      This::trace.reverseAD1(This::dTdA, jacobians);
-    }
-
-    /// Given df/dT, multiply in dT/dA and continue reverse AD process
-    // Cols is always known at compile time
-    template<typename SomeMatrix>
-    void reverseAD4(const SomeMatrix & dFdT,
-        JacobianMap& jacobians) const {
-      Base::Record::reverseAD4(dFdT, jacobians);
-      This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
-    }
-  };
-
-  /// Construct an execution trace for reverse AD
-  void trace(const Values& values, Record* record,
-      ExecutionTraceStorage*& traceStorage) const {
-    Base::trace(values, record, traceStorage); // recurse
-    // Write an Expression<A> execution trace in record->trace
-    // Iff Constant or Leaf, this will not write to traceStorage, only to trace.
-    // Iff the expression is functional, write all Records in traceStorage buffer
-    // Return value of type T is recorded in record->value
-    record->Record::This::value = This::expression->traceExecution(values,
-        record->Record::This::trace, traceStorage);
-    // traceStorage is never modified by traceExecution, but if traceExecution has
-    // written in the buffer, the next caller expects we advance the pointer
-    traceStorage += This::expression->traceSize();
-  }
-};
-
-/**
- *  Recursive GenerateFunctionalNode class Generator
- */
-template<class T, class TYPES>
-struct FunctionalNode {
-
-  /// The following typedef generates the recursively defined Base class
-  typedef typename boost::mpl::fold<TYPES, FunctionalBase<T>,
-      GenerateFunctionalNode<T, MPL::_2, MPL::_1> >::type Base;
-
-  /**
-   *  The type generated by this meta-function derives from Base
-   *  and adds access functions as well as the crucial [trace] function
-   */
-  struct type: public Base {
-
-    // Argument types and derived, note these are base 0 !
-    // These are currently not used - useful for Phoenix in future
-#ifdef EXPRESSIONS_PHOENIX
-    typedef TYPES Arguments;
-    typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
-    typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
+  // Windows does not support variable length arrays, so memory must be dynamically
+  // allocated on Visual Studio. For more information see the issue below
+  // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
+#ifdef _MSC_VER
+  internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size];
+#else
+  internal::ExecutionTraceStorage traceStorage[size];
 #endif

-    /// Reset expression shared pointer
-    template<class A, size_t N>
-    void reset(const boost::shared_ptr<ExpressionNode<A> >& ptr) {
-      static_cast<Argument<T, A, N> &>(*this).expression = ptr;
-    }
+  internal::ExecutionTrace<T> trace;
+  T value(this->traceExecution(values, trace, traceStorage));
+  trace.startReverseAD1(jacobians);

-    /// Access Expression shared pointer
-    template<class A, size_t N>
-    boost::shared_ptr<ExpressionNode<A> > expression() const {
-      return static_cast<Argument<T, A, N> const &>(*this).expression;
-    }
+#ifdef _MSC_VER
+  delete[] traceStorage;
+#endif

-    /// Provide convenience access to Record storage and implement
-    /// the virtual function based interface of CallRecord using the CallRecordImplementor
-    struct Record: public internal::CallRecordImplementor<Record,
-        traits<T>::dimension>, public Base::Record {
-      using Base::Record::print;
-      using Base::Record::startReverseAD4;
-      using Base::Record::reverseAD4;
-
-      virtual ~Record() {
-      }
-
-      /// Access Value
-      template<class A, size_t N>
-      const A& value() const {
-        return static_cast<JacobianTrace<T, A, N> const &>(*this).value;
-      }
-
-      /// Access Jacobian
-      template<class A, size_t N>
-      typename Jacobian<T, A>::type& jacobian() {
-        return static_cast<JacobianTrace<T, A, N>&>(*this).dTdA;
-      }
-    };
-
-    /// Construct an execution trace for reverse AD
-    Record* trace(const Values& values,
-        ExecutionTraceStorage* traceStorage) const {
-      assert(reinterpret_cast<size_t>(traceStorage) % TraceAlignment == 0);
-
-      // Create the record and advance the pointer
-      Record* record = new (traceStorage) Record();
-      traceStorage += upAligned(sizeof(Record));
-
-      // Record the traces for all arguments
-      // After this, the traceStorage pointer is set to after what was written
-      Base::trace(values, record, traceStorage);
-
-      // Return the record for this function evaluation
-      return record;
-    }
-  };
-};
-//-----------------------------------------------------------------------------
-
-/// Unary Function Expression
-template<class T, class A1>
-class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
-
-  typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
-
-public:
-
-  typedef boost::function<T(const A1&, OJ1)> Function;
-  typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
-  typedef typename Base::Record Record;
-
-private:
-
-  Function function_;
-
-  /// Constructor with a unary function f, and input argument e
-  UnaryExpression(Function f, const Expression<A1>& e1) :
-      function_(f) {
-    this->template reset<A1, 1>(e1.root());
-    ExpressionNode<T>::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize();
-  }
-
-  friend class Expression<T> ;
-
-public:
-
-  /// Return value
-  virtual T value(const Values& values) const {
-    return function_(this->template expression<A1, 1>()->value(values), boost::none);
-  }
-
-  /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
-
-    Record* record = Base::trace(values, traceStorage);
-    trace.setFunction(record);
-
-    return function_(record->template value<A1, 1>(),
-        record->template jacobian<A1, 1>());
-  }
-};
-
-//-----------------------------------------------------------------------------
-/// Binary Expression
-
-template<class T, class A1, class A2>
-class BinaryExpression:
-    public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
-
-  typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
-  typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
-
-public:
-
-  typedef boost::function<T(const A1&, const A2&, OJ1, OJ2)> Function;
-  typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
-  typedef typename Base::Record Record;
-
-private:
-
-  Function function_;
-
-  /// Constructor with a ternary function f, and three input arguments
-  BinaryExpression(Function f, const Expression<A1>& e1,
-      const Expression<A2>& e2) :
-      function_(f) {
-    this->template reset<A1, 1>(e1.root());
-    this->template reset<A2, 2>(e2.root());
-    ExpressionNode<T>::traceSize_ = //
-        upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize();
-  }
-
-  friend class Expression<T> ;
-  friend class ::ExpressionFactorBinaryTest;
-
-public:
-
-  /// Return value
-  virtual T value(const Values& values) const {
-    using boost::none;
-    return function_(this->template expression<A1, 1>()->value(values),
-    this->template expression<A2, 2>()->value(values),
-    none, none);
-  }
-
-  /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
-
-    Record* record = Base::trace(values, traceStorage);
-    trace.setFunction(record);
-
-    return function_(record->template value<A1, 1>(),
-        record->template value<A2,2>(), record->template jacobian<A1, 1>(),
-        record->template jacobian<A2, 2>());
-  }
-};
-
-//-----------------------------------------------------------------------------
-/// Ternary Expression
-
-template<class T, class A1, class A2, class A3>
-class TernaryExpression:
-    public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
-
-  typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
-  typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
-  typedef typename MakeOptionalJacobian<T, A3>::type OJ3;
-
-public:
-
-  typedef boost::function<T(const A1&, const A2&, const A3&, OJ1, OJ2, OJ3)> Function;
-  typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
-  typedef typename Base::Record Record;
-
-private:
-
-  Function function_;
-
-  /// Constructor with a ternary function f, and three input arguments
-  TernaryExpression(Function f, const Expression<A1>& e1,
-      const Expression<A2>& e2, const Expression<A3>& e3) :
-      function_(f) {
-    this->template reset<A1, 1>(e1.root());
-    this->template reset<A2, 2>(e2.root());
-    this->template reset<A3, 3>(e3.root());
-    ExpressionNode<T>::traceSize_ = //
-        upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize()
-            + e3.traceSize();
-  }
-
-  friend class Expression<T> ;
-
-public:
-
-  /// Return value
-  virtual T value(const Values& values) const {
-    using boost::none;
-    return function_(this->template expression<A1, 1>()->value(values),
-    this->template expression<A2, 2>()->value(values),
-    this->template expression<A3, 3>()->value(values),
-    none, none, none);
-  }
-
-  /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
-
-    Record* record = Base::trace(values, traceStorage);
-    trace.setFunction(record);
-
-    return function_(
-        record->template value<A1, 1>(), record->template value<A2, 2>(),
-        record->template value<A3, 3>(), record->template jacobian<A1, 1>(),
-        record->template jacobian<A2, 2>(), record->template jacobian<A3, 3>());
-  }
-
-};
-//-----------------------------------------------------------------------------
+  return value;
 }
+
+template<typename T>
+typename Expression<T>::KeysAndDims Expression<T>::keysAndDims() const {
+  std::map<Key, int> map;
+  dims(map);
+  size_t n = map.size();
+  KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
+  boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
+  boost::copy(map | boost::adaptors::map_values, pair.second.begin());
+  return pair;
+}
+
+namespace internal {
+// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
+template<class T>
+struct apply_compose {
+  typedef T result_type;
+  static const int Dim = traits<T>::dimension;
+  T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
+      boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
+    return x.compose(y, H1, H2);
+  }
+};
+}
+
+// Global methods:
+
+/// Construct a product expression, assumes T::compose(T) -> T
+template<typename T>
+Expression<T> operator*(const Expression<T>& expression1,
+    const Expression<T>& expression2) {
+  return Expression<T>(
+      boost::bind(internal::apply_compose<T>(), _1, _2, _3, _4), expression1,
+      expression2);
+}
+
+/// Construct an array of leaves
+template<typename T>
+std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
+  std::vector<Expression<T> > unknowns;
+  unknowns.reserve(n);
+  for (size_t i = start; i < start + n; i++)
+    unknowns.push_back(Expression<T>(c, i));
+  return unknowns;
+}
+
+} // namespace gtsam
--- a/gtsam/nonlinear/Expression.h
+++ b/gtsam/nonlinear/Expression.h
@ -19,21 +19,28 @@

 #pragma once

-#include <gtsam/nonlinear/Expression-inl.h>
+#include <gtsam/nonlinear/internal/JacobianMap.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/base/FastVector.h>
+#include <gtsam/base/OptionalJacobian.h>

 #include <boost/bind.hpp>
-#include <boost/range/adaptor/map.hpp>
-#include <boost/range/algorithm.hpp>
+#include <boost/make_shared.hpp>
+#include <map>

+// Forward declare tests
 class ExpressionFactorShallowTest;

 namespace gtsam {

-// Forward declare
+// Forward declares
+class Values;
 template<typename T> class ExpressionFactor;

+namespace internal {
+template<typename T> class ExecutionTrace;
+template<typename T> class ExpressionNode;
+}
+
 /**
 * Expression class that supports automatic differentiation
 */
@ -48,110 +55,97 @@ public:
 private:

  // Paul's trick shared pointer, polymorphic root of entire expression tree
-  boost::shared_ptr<ExpressionNode<T> > root_;
+  boost::shared_ptr<internal::ExpressionNode<T> > root_;

 public:

+  // Expressions wrap trees of functions that can evaluate their own derivatives.
+  // The meta-functions below are useful to specify the type of those functions.
+  // Example, a function taking a camera and a 3D point and yielding a 2D point:
+  //   Expression<Point2>::BinaryFunction<SimpleCamera,Point3>::type
+  template<class A1>
+  struct UnaryFunction {
+    typedef boost::function<
+        T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
+  };
+
+  template<class A1, class A2>
+  struct BinaryFunction {
+    typedef boost::function<
+        T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
+            typename MakeOptionalJacobian<T, A2>::type)> type;
+  };
+
+  template<class A1, class A2, class A3>
+  struct TernaryFunction {
+    typedef boost::function<
+        T(const A1&, const A2&, const A3&,
+            typename MakeOptionalJacobian<T, A1>::type,
+            typename MakeOptionalJacobian<T, A2>::type,
+            typename MakeOptionalJacobian<T, A3>::type)> type;
+  };
+
  /// Print
-  void print(const std::string& s) const {
-    std::cout << s << *root_ << std::endl;
-  }
+  void print(const std::string& s) const;

-  // Construct a constant expression
-  Expression(const T& value) :
-      root_(new ConstantExpression<T>(value)) {
-  }
+  /// Construct a constant expression
+  Expression(const T& value);

-  // Construct a leaf expression, with Key
-  Expression(const Key& key) :
-      root_(new LeafExpression<T>(key)) {
-  }
+  /// Construct a leaf expression, with Key
+  Expression(const Key& key);

-  // Construct a leaf expression, with Symbol
-  Expression(const Symbol& symbol) :
-      root_(new LeafExpression<T>(symbol)) {
-  }
+  /// Construct a leaf expression, with Symbol
+  Expression(const Symbol& symbol);

-  // Construct a leaf expression, creating Symbol
-  Expression(unsigned char c, size_t j) :
-      root_(new LeafExpression<T>(Symbol(c, j))) {
-  }
+  /// Construct a leaf expression, creating Symbol
+  Expression(unsigned char c, size_t j);
+
+  /// Construct a unary function expression
+  template<typename A>
+  Expression(typename UnaryFunction<A>::type function,
+      const Expression<A>& expression);
+
+  /// Construct a binary function expression
+  template<typename A1, typename A2>
+  Expression(typename BinaryFunction<A1, A2>::type function,
+      const Expression<A1>& expression1, const Expression<A2>& expression2);
+
+  /// Construct a ternary function expression
+  template<typename A1, typename A2, typename A3>
+  Expression(typename TernaryFunction<A1, A2, A3>::type function,
+      const Expression<A1>& expression1, const Expression<A2>& expression2,
+      const Expression<A3>& expression3);

  /// Construct a nullary method expression
  template<typename A>
  Expression(const Expression<A>& expression,
-      T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
-      root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
-  }
-
-  /// Construct a unary function expression
-  template<typename A>
-  Expression(typename UnaryExpression<T, A>::Function function,
-      const Expression<A>& expression) :
-      root_(new UnaryExpression<T, A>(function, expression)) {
-  }
+      T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const);

  /// Construct a unary method expression
  template<typename A1, typename A2>
  Expression(const Expression<A1>& expression1,
      T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
          typename MakeOptionalJacobian<T, A2>::type) const,
-      const Expression<A2>& expression2) :
-      root_(
-          new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
-              expression1, expression2)) {
-  }
-
-  /// Construct a binary function expression
-  template<typename A1, typename A2>
-  Expression(typename BinaryExpression<T, A1, A2>::Function function,
-      const Expression<A1>& expression1, const Expression<A2>& expression2) :
-      root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
-  }
+      const Expression<A2>& expression2);

  /// Construct a binary method expression
  template<typename A1, typename A2, typename A3>
  Expression(const Expression<A1>& expression1,
      T (A1::*method)(const A2&, const A3&,
-          typename TernaryExpression<T, A1, A2, A3>::OJ1,
-          typename TernaryExpression<T, A1, A2, A3>::OJ2,
-          typename TernaryExpression<T, A1, A2, A3>::OJ3) const,
-      const Expression<A2>& expression2, const Expression<A3>& expression3) :
-      root_(
-          new TernaryExpression<T, A1, A2, A3>(
-              boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
-              expression2, expression3)) {
-  }
+          typename MakeOptionalJacobian<T, A1>::type,
+          typename MakeOptionalJacobian<T, A2>::type,
+          typename MakeOptionalJacobian<T, A3>::type) const,
+      const Expression<A2>& expression2, const Expression<A3>& expression3);

-  /// Construct a ternary function expression
-  template<typename A1, typename A2, typename A3>
-  Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
-      const Expression<A1>& expression1, const Expression<A2>& expression2,
-      const Expression<A3>& expression3) :
-      root_(
-          new TernaryExpression<T, A1, A2, A3>(function, expression1,
-              expression2, expression3)) {
-  }
-
-  /// Return root
-  const boost::shared_ptr<ExpressionNode<T> >& root() const {
-    return root_;
-  }
-
-  // Return size needed for memory buffer in traceExecution
-  size_t traceSize() const {
-    return root_->traceSize();
+  /// Destructor
+  virtual ~Expression() {
  }

  /// Return keys that play in this expression
-  std::set<Key> keys() const {
-    return root_->keys();
-  }
+  std::set<Key> keys() const;

  /// Return dimensions for each argument, as a map
-  void dims(std::map<Key, int>& map) const {
-    root_->dims(map);
-  }
+  void dims(std::map<Key, int>& map) const;

  /**
   * @brief Return value and optional derivatives, reverse AD version
@ -159,16 +153,7 @@ public:
   * The order of the Jacobians is same as keys in either keys() or dims()
   */
  T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
-      boost::none) const {
-
-    if (H) {
-      // Call private version that returns derivatives in H
-      KeysAndDims pair = keysAndDims();
-      return value(values, pair.first, pair.second, *H);
-    } else
-      // no derivatives needed, just return value
-      return root_->value(values);
-  }
+      boost::none) const;

  /**
   *  @return a "deep" copy of this Expression
@ -179,116 +164,55 @@ public:
    return boost::make_shared<Expression>(*this);
  }

+  /// Return root
+  const boost::shared_ptr<internal::ExpressionNode<T> >& root() const;
+
+  /// Return size needed for memory buffer in traceExecution
+  size_t traceSize() const;
+
 private:

-  /// Vaguely unsafe keys and dimensions in same order
+  /// Keys and dimensions in same order
  typedef std::pair<FastVector<Key>, FastVector<int> > KeysAndDims;
-  KeysAndDims keysAndDims() const {
-    std::map<Key, int> map;
-    dims(map);
-    size_t n = map.size();
-    KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
-    boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
-    boost::copy(map | boost::adaptors::map_values, pair.second.begin());
-    return pair;
-  }
+  KeysAndDims keysAndDims() const;

  /// private version that takes keys and dimensions, returns derivatives
-  T value(const Values& values, const FastVector<Key>& keys,
-      const FastVector<int>& dims, std::vector<Matrix>& H) const {
-
-    // H should be pre-allocated
-    assert(H.size()==keys.size());
-
-    // Pre-allocate and zero VerticalBlockMatrix
-    static const int Dim = traits<T>::dimension;
-    VerticalBlockMatrix Ab(dims, Dim);
-    Ab.matrix().setZero();
-    JacobianMap jacobianMap(keys, Ab);
-
-    // Call unsafe version
-    T result = value(values, jacobianMap);
-
-    // Copy blocks into the vector of jacobians passed in
-    for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
-      H[i] = Ab(i);
-
-    return result;
-  }
+  T valueAndDerivatives(const Values& values, const FastVector<Key>& keys,
+      const FastVector<int>& dims, std::vector<Matrix>& H) const;

  /// trace execution, very unsafe
-  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
-    return root_->traceExecution(values, trace, traceStorage);
-  }
+  T traceExecution(const Values& values, internal::ExecutionTrace<T>& trace,
+      void* traceStorage) const;

-  /**
-   * @brief Return value and derivatives, reverse AD version
-   * This very unsafe method needs a JacobianMap with correctly allocated
-   * and initialized VerticalBlockMatrix, hence is declared private.
-   */
-  T value(const Values& values, JacobianMap& jacobians) const {
-    // The following piece of code is absolutely crucial for performance.
-    // We allocate a block of memory on the stack, which can be done at runtime
-    // with modern C++ compilers. The traceExecution then fills this memory
-    // with an execution trace, made up entirely of "Record" structs, see
-    // the FunctionalNode class in expression-inl.h
-    size_t size = traceSize();
-
-    // Windows does not support variable length arrays, so memory must be dynamically
-    // allocated on Visual Studio. For more information see the issue below
-    // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
-#ifdef _MSC_VER
-    ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size];
-#else
-    ExecutionTraceStorage traceStorage[size];
-#endif
-
-    ExecutionTrace<T> trace;
-    T value(traceExecution(values, trace, traceStorage));
-    trace.startReverseAD1(jacobians);
-
-#ifdef _MSC_VER
-    delete[] traceStorage;
-#endif
-
-    return value;
-  }
+  /// brief Return value and derivatives, reverse AD version
+  T valueAndJacobianMap(const Values& values,
+      internal::JacobianMap& jacobians) const;

  // be very selective on who can access these private methods:
  friend class ExpressionFactor<T> ;
+  friend class internal::ExpressionNode<T>;
+
+  // and add tests
  friend class ::ExpressionFactorShallowTest;
-
 };

-// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
-template<class T>
-struct apply_compose {
-  typedef T result_type;
-  static const int Dim = traits<T>::dimension;
-  T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
-      boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
-    return x.compose(y, H1, H2);
-  }
-};
-
-/// Construct a product expression, assumes T::compose(T) -> T
+/**
+ *  Construct a product expression, assumes T::compose(T) -> T
+ *  Example:
+ *    Expression<Point2> a(0), b(1), c = a*b;
+ */
 template<typename T>
-Expression<T> operator*(const Expression<T>& expression1,
-    const Expression<T>& expression2) {
-  return Expression<T>(boost::bind(apply_compose<T>(), _1, _2, _3, _4),
-      expression1, expression2);
-}
+Expression<T> operator*(const Expression<T>& e1, const Expression<T>& e2);

-/// Construct an array of leaves
+/**
+ *  Construct an array of unknown expressions with successive symbol keys
+ *  Example:
+ *    createUnknowns<Pose2>(3,'x') creates unknown expressions for x0,x1,x2
+ */
 template<typename T>
-std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0) {
-  std::vector<Expression<T> > unknowns;
-  unknowns.reserve(n);
-  for (size_t i = start; i < start + n; i++)
-    unknowns.push_back(Expression<T>(c, i));
-  return unknowns;
-}
+std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0);

-}
+} // namespace gtsam
+
+#include <gtsam/nonlinear/Expression-inl.h>

--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@ -32,7 +32,9 @@ namespace gtsam {
 template<class T>
 class ExpressionFactor: public NoiseModelFactor {

-protected:
+ protected:
+
+  typedef ExpressionFactor<T> This;

  T measurement_; ///< the measurement to be compared with the expression
  Expression<T> expression_; ///< the expression that is AD enabled
@ -40,7 +42,9 @@ protected:

  static const int Dim = traits<T>::dimension;

-public:
+ public:
+
+  typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;

  /// Constructor
  ExpressionFactor(const SharedNoiseModel& noiseModel, //
@ -65,8 +69,8 @@ public:
   */
  virtual Vector unwhitenedError(const Values& x,
      boost::optional<std::vector<Matrix>&> H = boost::none) const {
-     if (H) {
-      const T value = expression_.value(x, keys_, dims_, *H);
+    if (H) {
+      const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
      return traits<T>::Local(measurement_, value);
    } else {
      const T value = expression_.value(x);
@ -89,13 +93,13 @@ public:

    // Wrap keys and VerticalBlockMatrix into structure passed to expression_
    VerticalBlockMatrix& Ab = factor->matrixObject();
-    JacobianMap jacobianMap(keys_, Ab);
+    internal::JacobianMap jacobianMap(keys_, Ab);

    // Zero out Jacobian so we can simply add to it
    Ab.matrix().setZero();

    // Get value and Jacobians, writing directly into JacobianFactor
-    T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here !
+    T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !

    // Evaluate error and set RHS vector b
    Ab(size()).col(0) = -traits<T>::Local(measurement_, value);
@ -106,8 +110,13 @@ public:

    return factor;
  }
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 };
 // ExpressionFactor

-} // \ namespace gtsam
+}// \ namespace gtsam

--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@ -98,7 +98,7 @@ DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationMode
 }

 /* ************************************************************************* */
-ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) const {
+ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) {
  std::string s = str;  boost::algorithm::to_upper(s);
  if (s == "QR") return ISAM2Params::QR;
  if (s == "CHOLESKY") return ISAM2Params::CHOLESKY;
@ -108,7 +108,7 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::strin
 }

 /* ************************************************************************* */
-std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) const {
+std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) {
  std::string s;
  switch (value) {
  case ISAM2Params::QR:         s = "QR"; break;
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@ -186,6 +186,7 @@ struct GTSAM_EXPORT ISAM2Params {
      enableDetailedResults(false), enablePartialRelinearizationCheck(false),
      findUnusedFactorSlots(false) {}

+  /// print iSAM2 parameters
  void print(const std::string& str = "") const {
    std::cout << str << "\n";
    if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
@ -214,7 +215,9 @@ struct GTSAM_EXPORT ISAM2Params {
    std::cout.flush();
  }

-   /** Getters and Setters for all properties */
+  /// @name Getters and Setters for all properties
+  /// @{
+
  OptimizationParams getOptimizationParams() const { return this->optimizationParams; }
  RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; }
  int getRelinearizeSkip() const { return relinearizeSkip; }
@ -237,14 +240,21 @@ struct GTSAM_EXPORT ISAM2Params {
  void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; }
  void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; }

-  Factorization factorizationTranslator(const std::string& str) const;
-  std::string factorizationTranslator(const Factorization& value) const;
-
  GaussianFactorGraph::Eliminate getEliminationFunction() const {
    return factorization == CHOLESKY
      ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky
      : (GaussianFactorGraph::Eliminate)EliminateQR;
  }
+
+  /// @}
+
+  /// @name Some utilities
+  /// @{
+
+  static Factorization factorizationTranslator(const std::string& str);
+  static std::string factorizationTranslator(const Factorization& value);
+
+  /// @}
 };


@ -544,8 +554,15 @@ public:
    boost::optional<std::vector<size_t>&> marginalFactorsIndices = boost::none,
    boost::optional<std::vector<size_t>&> deletedFactorsIndices = boost::none);

-  /** Access the current linearization point */
-  const Values& getLinearizationPoint() const { return theta_; }
+  /// Access the current linearization point
+  const Values& getLinearizationPoint() const {
+    return theta_;
+  }
+
+  /// Check whether variable with given key exists in linearization point
+  bool valueExists(Key key) const {
+    return theta_.exists(key);
+  }

  /** Compute an estimate from the incomplete linear delta computed during the last update.
   * This delta is incomplete because it was not updated below wildfire_threshold.  If only
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@ -25,9 +25,10 @@

 #include <boost/algorithm/string.hpp>
 #include <boost/range/adaptor/map.hpp>
+#include <fstream>
+#include <limits>
 #include <string>
 #include <cmath>
-#include <fstream>

 using namespace std;

@ -178,7 +179,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
        SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
        damped += boost::make_shared<JacobianFactor>(key_vector.first, A, b,
            model);
-      } catch (std::exception e) {
+      } catch (const std::exception& e) {
        // Don't attempt any damping if no key found in diagonal
        continue;
      }
@ -247,7 +248,7 @@ void LevenbergMarquardtOptimizer::iterate() {
    double modelFidelity = 0.0;
    bool step_is_successful = false;
    bool stopSearchingLambda = false;
-    double newError;
+    double newError = numeric_limits<double>::infinity();
    Values newValues;
    VectorValues delta;

@ -255,7 +256,7 @@ void LevenbergMarquardtOptimizer::iterate() {
    try {
      delta = solve(dampedSystem, state_.values, params_);
      systemSolvedSuccessfully = true;
-    } catch (IndeterminantLinearSystemException) {
+    } catch (const IndeterminantLinearSystemException& e) {
      systemSolvedSuccessfully = false;
    }

--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@ -21,21 +21,14 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>

+#include <boost/bind.hpp>
+
 #include <limits>
 #include <iostream>
 #include <cmath>

 namespace gtsam {

-/**
- * Template default compare function that assumes a testable T
- */
-template<class T>
-bool compare(const T& a, const T& b) {
-  GTSAM_CONCEPT_TESTABLE_TYPE(T);
-  return a.equals(b);
-}
-
 /**
 * An equality factor that forces either one variable to a constant,
 * or a set of variables to be equal to each other.
@ -76,7 +69,9 @@ public:
  /**
   * Function that compares two values
   */
-  bool (*compare_)(const T& a, const T& b);
+  typedef boost::function<bool(const T&, const T&)> CompareFunction;
+  CompareFunction compare_;
+//  bool (*compare_)(const T& a, const T& b);

  /** default constructor - only for serialization */
  NonlinearEquality() {
@ -92,7 +87,7 @@ public:
   * Constructor - forces exact evaluation
   */
  NonlinearEquality(Key j, const T& feasible,
-      bool (*_compare)(const T&, const T&) = compare<T>) :
+      const CompareFunction &_compare = boost::bind(traits<T>::Equals,_1,_2,1e-9)) :
      Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
          j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
      compare_(_compare) {
@ -102,7 +97,7 @@ public:
   * Constructor - allows inexact evaluation
   */
  NonlinearEquality(Key j, const T& feasible, double error_gain,
-      bool (*_compare)(const T&, const T&) = compare<T>) :
+      const CompareFunction &_compare = boost::bind(traits<T>::Equals,_1,_2,1e-9)) :
      Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
          j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
      compare_(_compare) {
@ -122,7 +117,7 @@ public:
  /** Check if two factors are equal */
  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
    const This* e = dynamic_cast<const This*>(&f);
-    return e && Base::equals(f) && feasible_.equals(e->feasible_, tol)
+    return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
        && std::abs(error_gain_ - e->error_gain_) < tol;
  }

--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@ -47,11 +47,12 @@ double NonlinearFactorGraph::probPrime(const Values& c) const {

 /* ************************************************************************* */
 void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
-  cout << str << "size: " << size() << endl;
+  cout << str << "size: " << size() << endl << endl;
  for (size_t i = 0; i < factors_.size(); i++) {
    stringstream ss;
-    ss << "factor " << i << ": ";
+    ss << "Factor " << i << ": ";
    if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
+    cout << endl;
  }
 }

@ -62,12 +63,12 @@ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol)

 /* ************************************************************************* */
 void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
-    const GraphvizFormatting& graphvizFormatting,
+    const GraphvizFormatting& formatting,
    const KeyFormatter& keyFormatter) const
 {
  stm << "graph {\n";
-  stm << "  size=\"" << graphvizFormatting.figureWidthInches << "," <<
-    graphvizFormatting.figureHeightInches << "\";\n\n";
+  stm << "  size=\"" << formatting.figureWidthInches << "," <<
+    formatting.figureHeightInches << "\";\n\n";

  FastSet<Key> keys = this->keys();

@ -75,72 +76,38 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
  struct { boost::optional<Point2> operator()(
    const Value& value, const GraphvizFormatting& graphvizFormatting)
  {
-    if(const Pose2* p = dynamic_cast<const Pose2*>(&value)) {
-      double x, y;
-      switch (graphvizFormatting.paperHorizontalAxis) {
-      case GraphvizFormatting::X: x = p->x(); break;
-      case GraphvizFormatting::Y: x = p->y(); break;
-      case GraphvizFormatting::Z: x = 0.0; break;
-      case GraphvizFormatting::NEGX: x = -p->x(); break;
-      case GraphvizFormatting::NEGY: x = -p->y(); break;
-      case GraphvizFormatting::NEGZ: x = 0.0; break;
-      default: throw std::runtime_error("Invalid enum value");
-      }
-      switch (graphvizFormatting.paperVerticalAxis) {
-      case GraphvizFormatting::X: y = p->x(); break;
-      case GraphvizFormatting::Y: y = p->y(); break;
-      case GraphvizFormatting::Z: y = 0.0; break;
-      case GraphvizFormatting::NEGX: y = -p->x(); break;
-      case GraphvizFormatting::NEGY: y = -p->y(); break;
-      case GraphvizFormatting::NEGZ: y = 0.0; break;
-      default: throw std::runtime_error("Invalid enum value");
-      }
-      return Point2(x,y);
-    } else if(const Pose3* p = dynamic_cast<const Pose3*>(&value)) {
-      double x, y;
-      switch (graphvizFormatting.paperHorizontalAxis) {
-      case GraphvizFormatting::X: x = p->x(); break;
-      case GraphvizFormatting::Y: x = p->y(); break;
-      case GraphvizFormatting::Z: x = p->z(); break;
-      case GraphvizFormatting::NEGX: x = -p->x(); break;
-      case GraphvizFormatting::NEGY: x = -p->y(); break;
-      case GraphvizFormatting::NEGZ: x = -p->z(); break;
-      default: throw std::runtime_error("Invalid enum value");
-      }
-      switch (graphvizFormatting.paperVerticalAxis) {
-      case GraphvizFormatting::X: y = p->x(); break;
-      case GraphvizFormatting::Y: y = p->y(); break;
-      case GraphvizFormatting::Z: y = p->z(); break;
-      case GraphvizFormatting::NEGX: y = -p->x(); break;
-      case GraphvizFormatting::NEGY: y = -p->y(); break;
-      case GraphvizFormatting::NEGZ: y = -p->z(); break;
-      default: throw std::runtime_error("Invalid enum value");
-      }
-     return Point2(x,y);
-    } else if(const Point3* p = dynamic_cast<const Point3*>(&value)) {
-      double x, y;
-      switch (graphvizFormatting.paperHorizontalAxis) {
-      case GraphvizFormatting::X: x = p->x(); break;
-      case GraphvizFormatting::Y: x = p->y(); break;
-      case GraphvizFormatting::Z: x = p->z(); break;
-      case GraphvizFormatting::NEGX: x = -p->x(); break;
-      case GraphvizFormatting::NEGY: x = -p->y(); break;
-      case GraphvizFormatting::NEGZ: x = -p->z(); break;
-      default: throw std::runtime_error("Invalid enum value");
-      }
-      switch (graphvizFormatting.paperVerticalAxis) {
-      case GraphvizFormatting::X: y = p->x(); break;
-      case GraphvizFormatting::Y: y = p->y(); break;
-      case GraphvizFormatting::Z: y = p->z(); break;
-      case GraphvizFormatting::NEGX: y = -p->x(); break;
-      case GraphvizFormatting::NEGY: y = -p->y(); break;
-      case GraphvizFormatting::NEGZ: y = -p->z(); break;
-      default: throw std::runtime_error("Invalid enum value");
-      }
-      return Point2(x,y);
+    Vector3 t;
+    if (const GenericValue<Pose2>* p = dynamic_cast<const GenericValue<Pose2>*>(&value)) {
+      t << p->value().x(), p->value().y(), 0;
+    } else if (const GenericValue<Point2>* p = dynamic_cast<const GenericValue<Point2>*>(&value)) {
+      t << p->value().x(), p->value().y(), 0;
+    } else if (const GenericValue<Pose3>* p = dynamic_cast<const GenericValue<Pose3>*>(&value)) {
+      t = p->value().translation().vector();
+    } else if (const GenericValue<Point3>* p = dynamic_cast<const GenericValue<Point3>*>(&value)) {
+      t = p->value().vector();
    } else {
      return boost::none;
    }
+    double x, y;
+    switch (graphvizFormatting.paperHorizontalAxis) {
+      case GraphvizFormatting::X: x = t.x(); break;
+      case GraphvizFormatting::Y: x = t.y(); break;
+      case GraphvizFormatting::Z: x = t.z(); break;
+      case GraphvizFormatting::NEGX: x = -t.x(); break;
+      case GraphvizFormatting::NEGY: x = -t.y(); break;
+      case GraphvizFormatting::NEGZ: x = -t.z(); break;
+      default: throw std::runtime_error("Invalid enum value");
+    }
+    switch (graphvizFormatting.paperVerticalAxis) {
+      case GraphvizFormatting::X: y = t.x(); break;
+      case GraphvizFormatting::Y: y = t.y(); break;
+      case GraphvizFormatting::Z: y = t.z(); break;
+      case GraphvizFormatting::NEGX: y = -t.x(); break;
+      case GraphvizFormatting::NEGY: y = -t.y(); break;
+      case GraphvizFormatting::NEGZ: y = -t.z(); break;
+      default: throw std::runtime_error("Invalid enum value");
+    }
+    return Point2(x,y);
  }} getXY;

  // Find bounds
@ -148,7 +115,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
  double minY = numeric_limits<double>::infinity(), maxY = -numeric_limits<double>::infinity();
  BOOST_FOREACH(Key key, keys) {
    if(values.exists(key)) {
-      boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
+      boost::optional<Point2> xy = getXY(values.at(key), formatting);
      if(xy) {
        if(xy->x() < minX)
          minX = xy->x();
@ -163,33 +130,22 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
  }

  // Create nodes for each variable in the graph
-  bool firstTimePoses = true;
-  Key lastKey;
-  BOOST_FOREACH(Key key, keys) {
+  BOOST_FOREACH(Key key, keys){
    // Label the node with the label from the KeyFormatter
    stm << "  var" << key << "[label=\"" << keyFormatter(key) << "\"";
    if(values.exists(key)) {
-      boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
+      boost::optional<Point2> xy = getXY(values.at(key), formatting);
      if(xy)
-        stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\"";
+      stm << ", pos=\"" << formatting.scale*(xy->x() - minX) << "," << formatting.scale*(xy->y() - minY) << "!\"";
    }
    stm << "];\n";
-
-    if (firstTimePoses) {
-      lastKey = key;
-      firstTimePoses = false;
-    } else {
-      stm << "  var" << key << "--" << "var" << lastKey << ";\n";
-      lastKey = key;
-    }
  }
  stm << "\n";

-
-  if(graphvizFormatting.mergeSimilarFactors) {
+  if (formatting.mergeSimilarFactors) {
    // Remove duplicate factors
    FastSet<vector<Key> > structure;
-    BOOST_FOREACH(const sharedFactor& factor, *this) {
+    BOOST_FOREACH(const sharedFactor& factor, *this){
      if(factor) {
        vector<Key> factorKeys = factor->keys();
        std::sort(factorKeys.begin(), factorKeys.end());
@ -199,57 +155,65 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,

    // Create factors and variable connections
    size_t i = 0;
-    BOOST_FOREACH(const vector<Key>& factorKeys, structure) {
+    BOOST_FOREACH(const vector<Key>& factorKeys, structure){
      // Make each factor a dot
      stm << "  factor" << i << "[label=\"\", shape=point";
      {
-        map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
-        if(pos != graphvizFormatting.factorPositions.end())
-          stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
+        map<size_t, Point2>::const_iterator pos = formatting.factorPositions.find(i);
+        if(pos != formatting.factorPositions.end())
+        stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << ","
+                          << formatting.scale*(pos->second.y() - minY) << "!\"";
      }
      stm << "];\n";

      // Make factor-variable connections
      BOOST_FOREACH(Key key, factorKeys) {
-        stm << "  var" << key << "--" << "factor" << i << ";\n"; }
+        stm << "  var" << key << "--" << "factor" << i << ";\n";
+      }

      ++ i;
    }
  } else {
    // Create factors and variable connections
    for(size_t i = 0; i < this->size(); ++i) {
-      if(graphvizFormatting.plotFactorPoints){
-    // Make each factor a dot
-    stm << "  factor" << i << "[label=\"\", shape=point";
-    {
-    map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
-    if(pos != graphvizFormatting.factorPositions.end())
-      stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
-    }
-    stm << "];\n";
+      const NonlinearFactor::shared_ptr& factor = this->at(i);
+      if(formatting.plotFactorPoints) {
+        const FastVector<Key>& keys = factor->keys();
+        if (formatting.binaryEdges && keys.size()==2) {
+          stm << "  var" << keys[0] << "--" << "var" << keys[1] << ";\n";
+        } else {
+          // Make each factor a dot
+          stm << "  factor" << i << "[label=\"\", shape=point";
+          {
+            map<size_t, Point2>::const_iterator pos = formatting.factorPositions.find(i);
+            if(pos != formatting.factorPositions.end())
+              stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << ","
+                  << formatting.scale*(pos->second.y() - minY) << "!\"";
+          }
+          stm << "];\n";

-    // Make factor-variable connections
-    if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
-      BOOST_FOREACH(Key key, *this->at(i)) {
-      stm << "  var" << key << "--" << "factor" << i << ";\n";
+          // Make factor-variable connections
+          if(formatting.connectKeysToFactor && factor) {
+            BOOST_FOREACH(Key key, *factor) {
+              stm << "  var" << key << "--" << "factor" << i << ";\n";
+            }
+          }
+        }
      }
-    }
-
-    }
      else {
-      if(this->at(i)) {
-        Key k;
-        bool firstTime = true;
-        BOOST_FOREACH(Key key, *this->at(i)) {
-        if(firstTime){
-          k = key;
-          firstTime = false;
-          continue;
+        if(factor) {
+          Key k;
+          bool firstTime = true;
+          BOOST_FOREACH(Key key, *this->at(i)) {
+            if(firstTime) {
+              k = key;
+              firstTime = false;
+              continue;
+            }
+            stm << "  var" << key << "--" << "var" << k << ";\n";
+            k = key;
+          }
        }
-        stm << "  var" << key << "--" << "var" << k << ";\n";
-        k = key;
-        }
-      }
      }
    }
  }
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@ -32,6 +32,10 @@ namespace gtsam {
  class Ordering;
  class GaussianFactorGraph;
  class SymbolicFactorGraph;
+  template<typename T>
+  class Expression;
+  template<typename T>
+  class ExpressionFactor;

  /**
   * Formatting options when saving in GraphViz format using
@ -47,6 +51,7 @@ namespace gtsam {
    bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity
    bool plotFactorPoints; ///< Plots each factor as a dot between the variables
    bool connectKeysToFactor; ///< Draw a line from each key within a factor to the dot of the factor
+    bool binaryEdges; ///< just use non-dotted edges for binary factors
    std::map<size_t, Point2> factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions.
    /// Default constructor sets up robot coordinates.  Paper horizontal is robot Y,
    /// paper vertical is robot X.  Default figure size of 5x5 in.
@ -54,7 +59,7 @@ namespace gtsam {
      paperHorizontalAxis(Y), paperVerticalAxis(X),
      figureWidthInches(5), figureHeightInches(5), scale(1),
      mergeSimilarFactors(false), plotFactorPoints(true),
-      connectKeysToFactor(true) {}
+      connectKeysToFactor(true), binaryEdges(true) {}
  };


@ -150,6 +155,18 @@ namespace gtsam {
     */
    NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;

+    /**
+     * Directly add ExpressionFactor that implements |h(x)-z|^2_R
+     * @param h expression that implements measurement function
+     * @param z measurement
+     * @param R model
+     */
+    template<typename T>
+    void addExpressionFactor(const SharedNoiseModel& R, const T& z,
+                             const Expression<T>& h) {
+      push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
+    }
+
  private:

    /** Serialization function */
--- a/gtsam/nonlinear/expressionTesting.h
+++ b/gtsam/nonlinear/expressionTesting.h
@ -21,9 +21,9 @@

 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/Expression.h>
+#include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/linear/VectorValues.h>

 #include <CppUnitLite/TestResult.h>
@ -32,47 +32,6 @@

 namespace gtsam {

-/**
- * Linearize a nonlinear factor using numerical differentiation
- * The benefit of this method is that it does not need to know what types are
- * involved to evaluate the factor. If all the machinery of gtsam is working
- * correctly, we should get the correct numerical derivatives out the other side.
- */
-JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
-    const Values& values, double delta = 1e-5) {
-
-  // We will fill a map of Jacobians
-  std::map<Key, Matrix> jacobians;
-
-  // Get size
-  Eigen::VectorXd e = factor.whitenedError(values);
-  const size_t rows = e.size();
-
-  // Loop over all variables
-  VectorValues dX = values.zeroVectors();
-  BOOST_FOREACH(Key key, factor) {
-    // Compute central differences using the values struct.
-    const size_t cols = dX.dim(key);
-    Matrix J = Matrix::Zero(rows, cols);
-    for (size_t col = 0; col < cols; ++col) {
-      Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
-      dx[col] = delta;
-      dX[key] = dx;
-      Values eval_values = values.retract(dX);
-      Eigen::VectorXd left = factor.whitenedError(eval_values);
-      dx[col] = -delta;
-      dX[key] = dx;
-      eval_values = values.retract(dX);
-      Eigen::VectorXd right = factor.whitenedError(eval_values);
-      J.col(col) = (left - right) * (1.0 / (2.0 * delta));
-    }
-    jacobians[key] = J;
-  }
-
-  // Next step...return JacobianFactor
-  return JacobianFactor(jacobians, -e);
-}
-
 namespace internal {
 // CPPUnitLite-style test for linearization of a factor
 void testFactorJacobians(TestResult& result_, const std::string& name_,
@ -88,7 +47,7 @@ void testFactorJacobians(TestResult& result_, const std::string& name_,

  // Check cast result and then equality
  CHECK(actual);
-  EXPECT( assert_equal(expected, *actual, tolerance));
+  EXPECT(assert_equal(expected, *actual, tolerance));
 }
 }

@ -112,7 +71,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_,
      expression.value(values), expression);
  testFactorJacobians(result_, name_, f, values, nd_step, tolerance);
 }
-}
+} // namespace internal
 } // namespace gtsam

 /// \brief Check the Jacobians produced by an expression against finite differences.
--- a/gtsam/nonlinear/expressions.h
+++ b/gtsam/nonlinear/expressions.h
@ -18,6 +18,12 @@ Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
  return Expression<T>(traits<T>::Between, t1, t2);
 }

+// Generics
+template<typename T>
+Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
+  return Expression<T>(traits<T>::Compose, t1, t2);
+}
+
 typedef Expression<double> double_;
 typedef Expression<Vector3> Vector3_;

--- a/gtsam/nonlinear/factorTesting.h
+++ b/gtsam/nonlinear/factorTesting.h
@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 factorTesting.h
+ * @date September 18, 2014
+ * @author Frank Dellaert
+ * @author Paul Furgale
+ * @brief Evaluate derivatives of a nonlinear factor numerically
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+
+namespace gtsam {
+
+/**
+ * Linearize a nonlinear factor using numerical differentiation
+ * The benefit of this method is that it does not need to know what types are
+ * involved to evaluate the factor. If all the machinery of gtsam is working
+ * correctly, we should get the correct numerical derivatives out the other side.
+ */
+JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
+    const Values& values, double delta = 1e-5) {
+
+  // We will fill a vector of key/Jacobians pairs (a map would sort)
+  std::vector<std::pair<Key, Matrix> > jacobians;
+
+  // Get size
+  Eigen::VectorXd e = factor.whitenedError(values);
+  const size_t rows = e.size();
+
+  // Loop over all variables
+  VectorValues dX = values.zeroVectors();
+  BOOST_FOREACH(Key key, factor) {
+    // Compute central differences using the values struct.
+    const size_t cols = dX.dim(key);
+    Matrix J = Matrix::Zero(rows, cols);
+    for (size_t col = 0; col < cols; ++col) {
+      Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
+      dx[col] = delta;
+      dX[key] = dx;
+      Values eval_values = values.retract(dX);
+      Eigen::VectorXd left = factor.whitenedError(eval_values);
+      dx[col] = -delta;
+      dX[key] = dx;
+      eval_values = values.retract(dX);
+      Eigen::VectorXd right = factor.whitenedError(eval_values);
+      J.col(col) = (left - right) * (1.0 / (2.0 * delta));
+    }
+    jacobians.push_back(std::make_pair(key,J));
+  }
+
+  // Next step...return JacobianFactor
+  return JacobianFactor(jacobians, -e);
+}
+
+} // namespace gtsam
--- a/gtsam/nonlinear/internal/CallRecord.h
+++ b/gtsam/nonlinear/internal/CallRecord.h
@ -20,18 +20,9 @@

 #pragma once

-#include <gtsam/base/VerticalBlockMatrix.h>
-#include <gtsam/base/FastVector.h>
-#include <gtsam/base/Matrix.h>
-
-#include <boost/mpl/transform.hpp>
+#include <gtsam/nonlinear/internal/JacobianMap.h>

 namespace gtsam {
-
-class JacobianMap;
-// forward declaration
-
-//-----------------------------------------------------------------------------
 namespace internal {

 /**
@ -64,8 +55,6 @@ struct ConvertToVirtualFunctionSupportedMatrixType<false> {
  }
 };

-} // namespace internal
-
 /**
 * The CallRecord is an abstract base class for the any class that stores
 * the Jacobians of applying a function with respect to each of its arguments,
@ -94,9 +83,8 @@ struct CallRecord {
  inline void reverseAD2(const Eigen::MatrixBase<Derived> & dFdT,
      JacobianMap& jacobians) const {
    _reverseAD3(
-        internal::ConvertToVirtualFunctionSupportedMatrixType<
-            (Derived::RowsAtCompileTime > 5)>::convert(dFdT),
-        jacobians);
+        ConvertToVirtualFunctionSupportedMatrixType<
+            (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians);
  }

  // This overload supports matrices with both rows and columns dynamically sized.
@ -140,7 +128,6 @@ private:
 */
 const int CallRecordMaxVirtualStaticRows = 5;

-namespace internal {
 /**
 * The CallRecordImplementor implements the CallRecord interface for a Derived class by
 * delegating to its corresponding (templated) non-virtual methods.
@ -193,5 +180,4 @@ private:
 };

 } // namespace internal
-
 } // gtsam
--- a/gtsam/nonlinear/internal/ExecutionTrace.h
+++ b/gtsam/nonlinear/internal/ExecutionTrace.h
@ -0,0 +1,166 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 ExecutionTrace.h
+ * @date May 11, 2015
+ * @author Frank Dellaert
+ * @brief Execution trace for expressions
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/internal/JacobianMap.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/base/Manifold.h>
+
+#include <boost/type_traits/aligned_storage.hpp>
+
+#include <Eigen/Core>
+
+namespace gtsam {
+namespace internal {
+
+template<int T> struct CallRecord;
+
+/// Storage type for the execution trace.
+/// It enforces the proper alignment in a portable way.
+/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
+static const unsigned TraceAlignment = 16;
+typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
+
+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: reverse AD ends here, by writing a matrix into Jacobians
+template<typename Derived>
+void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
+    JacobianMap& jacobians, Key key) {
+  UseBlockIf<
+      Derived::RowsAtCompileTime != Eigen::Dynamic
+          && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian(
+      dTdA, jacobians, key);
+}
+
+/**
+ * The ExecutionTrace class records a tree-structured expression's execution.
+ *
+ * The class looks a bit complicated but it is so for performance.
+ * It is a tagged union that obviates the need to create
+ * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead
+ * the key for the leaf is stored in the space normally used to store a
+ * CallRecord*. Nothing is stored for a Constant.
+ *
+ * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be:
+ * Trace(Function) ->
+ *   BinaryRecord with two traces in it
+ *     trace1(Function) ->
+ *       UnaryRecord with one trace in it
+ *         trace1(Function) ->
+ *           BinaryRecord with two traces in it
+ *             trace1(Leaf)
+ *             trace2(Constant)
+ *     trace2(Leaf)
+ * Hence, there are three Record structs, written to memory by traceExecution
+ */
+template<class T>
+class ExecutionTrace {
+  static const int Dim = traits<T>::dimension;
+  enum {
+    Constant, Leaf, Function
+  } kind;
+  union {
+    Key key;
+    CallRecord<Dim>* ptr;
+  } content;
+public:
+  /// Pointer always starts out as a Constant
+  ExecutionTrace() :
+      kind(Constant) {
+  }
+  /// Change pointer to a Leaf Record
+  void setLeaf(Key key) {
+    kind = Leaf;
+    content.key = key;
+  }
+  /// Take ownership of pointer to a Function Record
+  void setFunction(CallRecord<Dim>* record) {
+    kind = Function;
+    content.ptr = record;
+  }
+  /// Print
+  void print(const std::string& indent = "") const {
+    if (kind == Constant)
+      std::cout << indent << "Constant" << std::endl;
+    else if (kind == Leaf)
+      std::cout << indent << "Leaf, key = " << content.key << std::endl;
+    else if (kind == Function) {
+      std::cout << indent << "Function" << std::endl;
+      content.ptr->print(indent + "  ");
+    }
+  }
+  /// Return record pointer, quite unsafe, used only for testing
+  template<class Record>
+  boost::optional<Record*> record() {
+    if (kind != Function)
+      return boost::none;
+    else {
+      Record* p = dynamic_cast<Record*>(content.ptr);
+      return p ? boost::optional<Record*>(p) : boost::none;
+    }
+  }
+  /**
+   *  *** This is the main entry point for reverse AD, called from Expression ***
+   * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
+   */
+  typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
+  void startReverseAD1(JacobianMap& jacobians) const {
+    if (kind == Leaf) {
+      // This branch will only be called on trivial Leaf expressions, i.e. Priors
+      static const JacobianTT I = JacobianTT::Identity();
+      handleLeafCase(I, jacobians, content.key);
+    } else if (kind == Function)
+      // This is the more typical entry point, starting the AD pipeline
+      // Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
+      content.ptr->startReverseAD2(jacobians);
+  }
+  // Either add to Jacobians (Leaf) or propagate (Function)
+  template<typename DerivedMatrix>
+  void reverseAD1(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
+      JacobianMap& jacobians) const {
+    if (kind == Leaf)
+      handleLeafCase(dTdA, jacobians, content.key);
+    else if (kind == Function)
+      content.ptr->reverseAD2(dTdA, jacobians);
+  }
+
+  /// Define type so we can apply it as a meta-function
+  typedef ExecutionTrace<T> type;
+};
+
+} // namespace internal
+} // namespace gtsam
--- a/gtsam/nonlinear/internal/ExpressionNode.h
+++ b/gtsam/nonlinear/internal/ExpressionNode.h
@ -0,0 +1,516 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 ExpressionNode.h
+ * @date May 10, 2015
+ * @author Frank Dellaert
+ * @author Paul Furgale
+ * @brief ExpressionNode class
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/internal/ExecutionTrace.h>
+#include <gtsam/nonlinear/internal/CallRecord.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <typeinfo>       // operator typeid
+#include <ostream>
+#include <map>
+
+class ExpressionFactorBinaryTest;
+// Forward declare for testing
+
+namespace gtsam {
+namespace internal {
+
+template<typename T>
+T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
+  // right now only word sized types are supported.
+  // Easy to extend if needed,
+  //   by somehow inferring the unsigned integer of same size
+  BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t));
+  size_t & uiValue = reinterpret_cast<size_t &>(value);
+  size_t misAlignment = uiValue % requiredAlignment;
+  if (misAlignment) {
+    uiValue += requiredAlignment - misAlignment;
+  }
+  return value;
+}
+template<typename T>
+T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
+  return upAlign(value, requiredAlignment);
+}
+
+//-----------------------------------------------------------------------------
+
+/**
+ * Expression node. The superclass for objects that do the heavy lifting
+ * An Expression<T> has a pointer to an ExpressionNode<T> underneath
+ * allowing Expressions to have polymorphic behaviour even though they
+ * are passed by value. This is the same way boost::function works.
+ * http://loki-lib.sourceforge.net/html/a00652.html
+ */
+template<class T>
+class ExpressionNode {
+
+protected:
+
+  size_t traceSize_;
+
+  /// Constructor, traceSize is size of the execution trace of expression rooted here
+  ExpressionNode(size_t traceSize = 0) :
+      traceSize_(traceSize) {
+  }
+
+public:
+
+  /// Destructor
+  virtual ~ExpressionNode() {
+  }
+
+  /// Streaming
+  GTSAM_EXPORT
+  friend std::ostream &operator<<(std::ostream &os,
+      const ExpressionNode& node) {
+    os << "Expression of type " << typeid(T).name();
+    if (node.traceSize_ > 0)
+      os << ", trace size = " << node.traceSize_;
+    os << "\n";
+    return os;
+  }
+
+  /// Return keys that play in this expression as a set
+  virtual std::set<Key> keys() const {
+    std::set<Key> keys;
+    return keys;
+  }
+
+  /// Return dimensions for each argument, as a map
+  virtual void dims(std::map<Key, int>& map) const {
+  }
+
+  // Return size needed for memory buffer in traceExecution
+  size_t traceSize() const {
+    return traceSize_;
+  }
+
+  /// Return value
+  virtual T value(const Values& values) const = 0;
+
+  /// Construct an execution trace for reverse AD
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* traceStorage) const = 0;
+};
+
+//-----------------------------------------------------------------------------
+/// Constant Expression
+template<class T>
+class ConstantExpression: public ExpressionNode<T> {
+
+  /// The constant value
+  T constant_;
+
+  /// Constructor with a value, yielding a constant
+  ConstantExpression(const T& value) :
+      constant_(value) {
+  }
+
+  friend class Expression<T> ;
+
+public:
+
+  /// Destructor
+  virtual ~ConstantExpression() {
+  }
+
+  /// Return value
+  virtual T value(const Values& values) const {
+    return constant_;
+  }
+
+  /// Construct an execution trace for reverse AD
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* traceStorage) const {
+    return constant_;
+  }
+};
+
+//-----------------------------------------------------------------------------
+/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
+template<typename T>
+class LeafExpression: public ExpressionNode<T> {
+  typedef T value_type;
+
+  /// The key into values
+  Key key_;
+
+  /// Constructor with a single key
+  LeafExpression(Key key) :
+      key_(key) {
+  }
+
+  friend class Expression<T> ;
+
+public:
+
+  /// Destructor
+  virtual ~LeafExpression() {
+  }
+
+  /// Return keys that play in this expression
+  virtual std::set<Key> keys() const {
+    std::set<Key> keys;
+    keys.insert(key_);
+    return keys;
+  }
+
+  /// Return dimensions for each argument
+  virtual void dims(std::map<Key, int>& map) const {
+    map[key_] = traits<T>::dimension;
+  }
+
+  /// Return value
+  virtual T value(const Values& values) const {
+    return values.at<T>(key_);
+  }
+
+  /// Construct an execution trace for reverse AD
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* traceStorage) const {
+    trace.setLeaf(key_);
+    return values.at<T>(key_);
+  }
+
+};
+
+//-----------------------------------------------------------------------------
+/// meta-function to generate fixed-size JacobianTA type
+template<class T, class A>
+struct Jacobian {
+  typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
+};
+
+// Eigen format for printing Jacobians
+static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]");
+
+//-----------------------------------------------------------------------------
+/// Unary Function Expression
+template<class T, class A1>
+class UnaryExpression: public ExpressionNode<T> {
+
+  typedef typename Expression<T>::template UnaryFunction<A1>::type Function;
+  boost::shared_ptr<ExpressionNode<A1> > expression1_;
+  Function function_;
+
+  /// Constructor with a unary function f, and input argument e1
+  UnaryExpression(Function f, const Expression<A1>& e1) :
+      expression1_(e1.root()), function_(f) {
+    ExpressionNode<T>::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize();
+  }
+
+  friend class Expression<T> ;
+
+public:
+
+  /// Destructor
+  virtual ~UnaryExpression() {
+  }
+
+  /// Return value
+  virtual T value(const Values& values) const {
+    return function_(expression1_->value(values), boost::none);
+  }
+
+  /// Return keys that play in this expression
+  virtual std::set<Key> keys() const {
+    return expression1_->keys();
+  }
+
+  /// Return dimensions for each argument
+  virtual void dims(std::map<Key, int>& map) const {
+    expression1_->dims(map);
+  }
+
+  // Inner Record Class
+  struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
+
+    A1 value1;
+    ExecutionTrace<A1> trace1;
+    typename Jacobian<T, A1>::type dTdA1;
+
+    /// Print to std::cout
+    void print(const std::string& indent) const {
+      std::cout << indent << "UnaryExpression::Record {" << std::endl;
+      std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl;
+      trace1.print(indent);
+      std::cout << indent << "}" << std::endl;
+    }
+
+    /// Start the reverse AD process
+    void startReverseAD4(JacobianMap& jacobians) const {
+      // This is the crucial point where the size of the AD pipeline is selected.
+      // One pipeline is started for each argument, but the number of rows in each
+      // pipeline is the same, namely the dimension of the output argument T.
+      // For example, if the entire expression is rooted by a binary function
+      // yielding a 2D result, then the matrix dTdA will have 2 rows.
+      // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2
+      // which calls the correctly sized CallRecord::reverseAD3, which in turn
+      // calls reverseAD4 below.
+      trace1.reverseAD1(dTdA1, jacobians);
+    }
+
+    /// Given df/dT, multiply in dT/dA and continue reverse AD process
+    template<typename SomeMatrix>
+    void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
+      trace1.reverseAD1(dFdT * dTdA1, jacobians);
+    }
+  };
+
+  /// Construct an execution trace for reverse AD
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* ptr) const {
+    assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
+
+    // Create the record at the start of the traceStorage and advance the pointer
+    Record* record = new (ptr) Record();
+    ptr += upAligned(sizeof(Record));
+
+    // Record the traces for all arguments
+    // After this, the traceStorage pointer is set to after what was written
+    // Write an Expression<A> execution trace in record->trace
+    // Iff Constant or Leaf, this will not write to traceStorage, only to trace.
+    // Iff the expression is functional, write all Records in traceStorage buffer
+    // Return value of type T is recorded in record->value
+    record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
+
+    // ptr is never modified by traceExecution, but if traceExecution has
+    // written in the buffer, the next caller expects we advance the pointer
+    ptr += expression1_->traceSize();
+    trace.setFunction(record);
+
+    return function_(record->value1, record->dTdA1);
+  }
+};
+
+//-----------------------------------------------------------------------------
+/// Binary Expression
+template<class T, class A1, class A2>
+class BinaryExpression: public ExpressionNode<T> {
+
+  typedef typename Expression<T>::template BinaryFunction<A1, A2>::type Function;
+  boost::shared_ptr<ExpressionNode<A1> > expression1_;
+  boost::shared_ptr<ExpressionNode<A2> > expression2_;
+  Function function_;
+
+  /// Constructor with a binary function f, and two input arguments
+  BinaryExpression(Function f, const Expression<A1>& e1,
+      const Expression<A2>& e2) :
+      expression1_(e1.root()), expression2_(e2.root()), function_(f) {
+    ExpressionNode<T>::traceSize_ = //
+        upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize();
+  }
+
+  friend class Expression<T> ;
+  friend class ::ExpressionFactorBinaryTest;
+
+public:
+
+  /// Destructor
+  virtual ~BinaryExpression() {
+  }
+
+  /// Return value
+  virtual T value(const Values& values) const {
+    using boost::none;
+    return function_(expression1_->value(values), expression2_->value(values),
+        none, none);
+  }
+
+  /// Return keys that play in this expression
+  virtual std::set<Key> keys() const {
+    std::set<Key> keys = expression1_->keys();
+    std::set<Key> myKeys = expression2_->keys();
+    keys.insert(myKeys.begin(), myKeys.end());
+    return keys;
+  }
+
+  /// Return dimensions for each argument
+  virtual void dims(std::map<Key, int>& map) const {
+    expression1_->dims(map);
+    expression2_->dims(map);
+  }
+
+  // Inner Record Class
+  struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
+
+    A1 value1;
+    ExecutionTrace<A1> trace1;
+    typename Jacobian<T, A1>::type dTdA1;
+
+    A2 value2;
+    ExecutionTrace<A2> trace2;
+    typename Jacobian<T, A2>::type dTdA2;
+
+    /// Print to std::cout
+    void print(const std::string& indent) const {
+      std::cout << indent << "BinaryExpression::Record {" << std::endl;
+      std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl;
+      trace1.print(indent);
+      std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl;
+      trace2.print(indent);
+      std::cout << indent << "}" << std::endl;
+    }
+
+    /// Start the reverse AD process, see comments in Base
+    void startReverseAD4(JacobianMap& jacobians) const {
+      trace1.reverseAD1(dTdA1, jacobians);
+      trace2.reverseAD1(dTdA2, jacobians);
+    }
+
+    /// Given df/dT, multiply in dT/dA and continue reverse AD process
+    template<typename SomeMatrix>
+    void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
+      trace1.reverseAD1(dFdT * dTdA1, jacobians);
+      trace2.reverseAD1(dFdT * dTdA2, jacobians);
+    }
+  };
+
+  /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* ptr) const {
+    assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
+    Record* record = new (ptr) Record();
+    ptr += upAligned(sizeof(Record));
+    record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
+    record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
+    ptr += expression1_->traceSize() + expression2_->traceSize();
+    trace.setFunction(record);
+    return function_(record->value1, record->value2, record->dTdA1,
+        record->dTdA2);
+  }
+};
+
+//-----------------------------------------------------------------------------
+/// Ternary Expression
+template<class T, class A1, class A2, class A3>
+class TernaryExpression: public ExpressionNode<T> {
+
+  typedef typename Expression<T>::template TernaryFunction<A1, A2, A3>::type Function;
+  boost::shared_ptr<ExpressionNode<A1> > expression1_;
+  boost::shared_ptr<ExpressionNode<A2> > expression2_;
+  boost::shared_ptr<ExpressionNode<A3> > expression3_;
+  Function function_;
+
+  /// Constructor with a ternary function f, and two input arguments
+  TernaryExpression(Function f, const Expression<A1>& e1,
+      const Expression<A2>& e2, const Expression<A3>& e3) :
+      expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), //
+      function_(f) {
+    ExpressionNode<T>::traceSize_ = upAligned(sizeof(Record)) + //
+        e1.traceSize() + e2.traceSize() + e3.traceSize();
+  }
+
+  friend class Expression<T> ;
+
+public:
+
+  /// Destructor
+  virtual ~TernaryExpression() {
+  }
+
+  /// Return value
+  virtual T value(const Values& values) const {
+    using boost::none;
+    return function_(expression1_->value(values), expression2_->value(values),
+        expression3_->value(values), none, none, none);
+  }
+
+  /// Return keys that play in this expression
+  virtual std::set<Key> keys() const {
+    std::set<Key> keys = expression1_->keys();
+    std::set<Key> myKeys = expression2_->keys();
+    keys.insert(myKeys.begin(), myKeys.end());
+    myKeys = expression3_->keys();
+    keys.insert(myKeys.begin(), myKeys.end());
+    return keys;
+  }
+
+  /// Return dimensions for each argument
+  virtual void dims(std::map<Key, int>& map) const {
+    expression1_->dims(map);
+    expression2_->dims(map);
+    expression3_->dims(map);
+  }
+
+  // Inner Record Class
+  struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
+
+    A1 value1;
+    ExecutionTrace<A1> trace1;
+    typename Jacobian<T, A1>::type dTdA1;
+
+    A2 value2;
+    ExecutionTrace<A2> trace2;
+    typename Jacobian<T, A2>::type dTdA2;
+
+    A3 value3;
+    ExecutionTrace<A3> trace3;
+    typename Jacobian<T, A3>::type dTdA3;
+
+    /// Print to std::cout
+    void print(const std::string& indent) const {
+      std::cout << indent << "TernaryExpression::Record {" << std::endl;
+      std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl;
+      trace1.print(indent);
+      std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl;
+      trace2.print(indent);
+      std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl;
+      trace3.print(indent);
+      std::cout << indent << "}" << std::endl;
+    }
+
+    /// Start the reverse AD process, see comments in Base
+    void startReverseAD4(JacobianMap& jacobians) const {
+      trace1.reverseAD1(dTdA1, jacobians);
+      trace2.reverseAD1(dTdA2, jacobians);
+      trace3.reverseAD1(dTdA3, jacobians);
+    }
+
+    /// Given df/dT, multiply in dT/dA and continue reverse AD process
+    template<typename SomeMatrix>
+    void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
+      trace1.reverseAD1(dFdT * dTdA1, jacobians);
+      trace2.reverseAD1(dFdT * dTdA2, jacobians);
+      trace3.reverseAD1(dFdT * dTdA3, jacobians);
+    }
+  };
+
+  /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* ptr) const {
+    assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
+    Record* record = new (ptr) Record();
+    ptr += upAligned(sizeof(Record));
+    record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
+    record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
+    record->value3 = expression3_->traceExecution(values, record->trace3, ptr);
+    ptr += expression1_->traceSize() + expression2_->traceSize()
+        + expression3_->traceSize();
+    trace.setFunction(record);
+    return function_(record->value1, record->value2, record->value3,
+        record->dTdA1, record->dTdA2, record->dTdA3);
+  }
+};
+
+} // namespace internal
+} // namespace gtsam
--- a/gtsam/nonlinear/internal/JacobianMap.h
+++ b/gtsam/nonlinear/internal/JacobianMap.h
@ -0,0 +1,54 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 JacobianMap.h
+ * @date May 11, 2015
+ * @author Frank Dellaert
+ * @brief JacobianMap for returning derivatives from expressions
+ */
+
+#pragma once
+
+#include <gtsam/inference/Key.h>
+#include <gtsam/base/FastVector.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
+
+namespace gtsam {
+namespace internal {
+
+// A JacobianMap is the primary mechanism by which derivatives are returned.
+// Expressions are designed to write their derivatives into an already allocated
+// Jacobian of the correct size, of type VerticalBlockMatrix.
+// The JacobianMap provides a mapping from keys to the underlying blocks.
+class JacobianMap {
+private:
+  typedef FastVector<Key> Keys;
+  const FastVector<Key>& keys_;
+  VerticalBlockMatrix& Ab_;
+
+public:
+  /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab
+  JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) :
+      keys_(keys), Ab_(Ab) {
+  }
+
+  /// Access blocks of via key
+  VerticalBlockMatrix::Block operator()(Key key) {
+    Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key);
+    DenseIndex block = it - keys_.begin();
+    return Ab_(block);
+  }
+};
+
+} // namespace internal
+} // namespace gtsam
+
--- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
+++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
@ -265,9 +265,10 @@ TEST(AdaptAutoDiff, Snavely) {
  typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
  Expression<Point2> expression(Adaptor(), P, X);
 #ifdef GTSAM_USE_QUATERNIONS
-  EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero
+  EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero
 #else
-  EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero
+  EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression<Point2, Camera, Point3>::Record),
+                     expression.traceSize());  // Todo, should be zero
 #endif
  set<Key> expected = list_of(1)(2);
  EXPECT(expected == expression.keys());
--- a/gtsam/nonlinear/tests/testCallRecord.cpp
+++ b/gtsam/nonlinear/tests/testCallRecord.cpp
@ -18,7 +18,7 @@
 * @brief unit tests for CallRecord class
 */

-#include <gtsam/nonlinear/CallRecord.h>
+#include <gtsam/nonlinear/internal/CallRecord.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>

@ -32,7 +32,7 @@ static const int Cols = 3;


 int dynamicIfAboveMax(int i){
-  if(i > CallRecordMaxVirtualStaticRows){
+  if(i > internal::CallRecordMaxVirtualStaticRows){
    return Eigen::Dynamic;
  }
  else return i;
@ -80,13 +80,13 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
  }
  void print(const std::string& indent) const {
  }
-  void startReverseAD4(JacobianMap& jacobians) const {
+  void startReverseAD4(internal::JacobianMap& jacobians) const {
  }

  mutable CallConfig cc;
 private:
  template<typename SomeMatrix>
-  void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
+  void reverseAD4(const SomeMatrix & dFdT, internal::JacobianMap& jacobians) const {
    cc.compTimeRows = SomeMatrix::RowsAtCompileTime;
    cc.compTimeCols = SomeMatrix::ColsAtCompileTime;
    cc.runTimeRows = dFdT.rows();
@ -97,7 +97,7 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
  friend struct internal::CallRecordImplementor;
 };

-JacobianMap & NJM= *static_cast<JacobianMap *>(NULL);
+internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(NULL);

 /* ************************************************************************* */
 typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat;
--- a/gtsam/nonlinear/tests/testExecutionTrace.cpp
+++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp
@ -0,0 +1,39 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------1------------------------------------------- */
+
+/**
+ * @file testExecutionTrace.cpp
+ * @date May 11, 2015
+ * @author Frank Dellaert
+ * @brief unit tests for Expression internals
+ */
+
+#include <gtsam/nonlinear/internal/ExecutionTrace.h>
+#include <gtsam/geometry/Point2.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// Constant
+TEST(ExecutionTrace, construct) {
+  internal::ExecutionTrace<Point2> trace;
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@ -42,7 +42,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);

 /* ************************************************************************* */
 // Constant
-TEST(Expression, constant) {
+TEST(Expression, Constant) {
  Expression<Rot3> R(someR);
  Values values;
  Rot3 actual = R.value(values);
@ -68,24 +68,27 @@ TEST(Expression, Leaves) {
  Point3 somePoint(1, 2, 3);
  values.insert(Symbol('p', 10), somePoint);
  std::vector<Expression<Point3> > points = createUnknowns<Point3>(10, 'p', 1);
-  EXPECT(assert_equal(somePoint,points.back().value(values)));
+  EXPECT(assert_equal(somePoint, points.back().value(values)));
 }

 /* ************************************************************************* */
 // Unary(Leaf)
 namespace unary {
-Point2 f0(const Point3& p, OptionalJacobian<2,3> H) {
+Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) {
  return Point2();
 }
 double f2(const Point3& p, OptionalJacobian<1, 3> H) {
  return 0.0;
 }
+Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
+  return p.vector();
+}
 Expression<Point3> p(1);
 set<Key> expected = list_of(1);
 }
-TEST(Expression, Unary0) {
+TEST(Expression, Unary1) {
  using namespace unary;
-  Expression<Point2> e(f0, p);
+  Expression<Point2> e(f1, p);
  EXPECT(expected == e.keys());
 }
 TEST(Expression, Unary2) {
@ -94,6 +97,12 @@ TEST(Expression, Unary2) {
  EXPECT(expected == e.keys());
 }
 /* ************************************************************************* */
+// Unary(Leaf), dynamic
+TEST(Expression, Unary3) {
+  using namespace unary;
+//  Expression<Vector> e(f3, p);
+}
+/* ************************************************************************* */
 //Nullary Method
 TEST(Expression, NullaryMethod) {

@ -118,7 +127,7 @@ TEST(Expression, NullaryMethod) {
  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,H[0]));
+  EXPECT(assert_equal(expected, H[0]));
 }
 /* ************************************************************************* */
 // Binary(Leaf,Leaf)
@ -149,12 +158,12 @@ TEST(Expression, BinaryKeys) {
 TEST(Expression, BinaryDimensions) {
  map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
  binary::p_cam.dims(actual);
-  EXPECT(actual==expected);
+  EXPECT(actual == expected);
 }
 /* ************************************************************************* */
 // dimensions
 TEST(Expression, BinaryTraceSize) {
-  typedef BinaryExpression<Point3, Pose3, Point3> Binary;
+  typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
  size_t expectedTraceSize = sizeof(Binary::Record);
  EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize());
 }
@ -180,17 +189,26 @@ TEST(Expression, TreeKeys) {
 TEST(Expression, TreeDimensions) {
  map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3)(3, 5);
  tree::uv_hat.dims(actual);
-  EXPECT(actual==expected);
+  EXPECT(actual == expected);
 }
 /* ************************************************************************* */
 // TraceSize
 TEST(Expression, TreeTraceSize) {
-  typedef UnaryExpression<Point2, Point3> Unary;
-  typedef BinaryExpression<Point3, Pose3, Point3> Binary1;
-  typedef BinaryExpression<Point2, Point2, Cal3_S2> Binary2;
-  size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record)
-      + sizeof(Binary2::Record);
-  EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize());
+  typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary1;
+  EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)),
+      tree::p_cam.traceSize());
+
+  typedef internal::UnaryExpression<Point2, Point3> Unary;
+  EXPECT_LONGS_EQUAL(
+      internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(),
+      tree::projection.traceSize());
+
+  EXPECT_LONGS_EQUAL(0, tree::K.traceSize());
+
+  typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary2;
+  EXPECT_LONGS_EQUAL(
+      internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize()
+          + tree::projection.traceSize(), tree::uv_hat.traceSize());
 }
 /* ************************************************************************* */

@ -234,7 +252,8 @@ TEST(Expression, compose3) {
 /* ************************************************************************* */
 // Test with ternary function
 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
-    OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
+    OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2,
+    OptionalJacobian<3, 3> H3) {
  // return dummy derivatives (not correct, but that's ok for testing here)
  if (H1)
    *H1 = eye(3);
--- a/gtsam/nonlinear/tests/testExpressionFactor.cpp
+++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp
@ -22,6 +22,7 @@
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/expressionTesting.h>
 #include <gtsam/base/Testable.h>

@ -169,7 +170,7 @@ static Point2 myUncal(const Cal3_S2& K, const Point2& p,
 // Binary(Leaf,Leaf)
 TEST(ExpressionFactor, Binary) {

-  typedef BinaryExpression<Point2, Cal3_S2, Point2> Binary;
+  typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary;

  Cal3_S2_ K_(1);
  Point2_ p_(2);
@ -184,11 +185,15 @@ TEST(ExpressionFactor, Binary) {
  EXPECT_LONGS_EQUAL(8, sizeof(double));
  EXPECT_LONGS_EQUAL(16, sizeof(Point2));
  EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2));
-  EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Point2>));
-  EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Cal3_S2>));
-  EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian<Point2,Cal3_S2>::type));
-  EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian<Point2,Point2>::type));
-  size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32;
+  EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace<Point2>));
+  EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace<Cal3_S2>));
+  EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian<Point2,Cal3_S2>::type));
+  EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian<Point2,Point2>::type));
+  size_t expectedRecordSize = sizeof(Cal3_S2)
+      + sizeof(internal::ExecutionTrace<Cal3_S2>)
+      + +sizeof(internal::Jacobian<Point2, Cal3_S2>::type) + sizeof(Point2)
+      + sizeof(internal::ExecutionTrace<Point2>)
+      + sizeof(internal::Jacobian<Point2, Point2>::type);
  EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record));

  // Check size
@ -197,8 +202,8 @@ TEST(ExpressionFactor, Binary) {
  EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size);
  // Use Variable Length Array, allocated on stack by gcc
  // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla
-  ExecutionTraceStorage traceStorage[size];
-  ExecutionTrace<Point2> trace;
+  internal::ExecutionTraceStorage traceStorage[size];
+  internal::ExecutionTrace<Point2> trace;
  Point2 value = binary.traceExecution(values, trace, traceStorage);
  EXPECT(assert_equal(Point2(),value, 1e-9));
  // trace.print();
@ -212,9 +217,8 @@ TEST(ExpressionFactor, Binary) {
  // Check matrices
  boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
  CHECK(r);
-  EXPECT(
-      assert_equal(expected25, (Matrix) (*r)-> jacobian<Cal3_S2, 1>(), 1e-9));
-  EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian<Point2, 2>(), 1e-9));
+  EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
+  EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
 }
 /* ************************************************************************* */
 // Unary(Binary(Leaf,Leaf))
@ -250,22 +254,22 @@ TEST(ExpressionFactor, Shallow) {
  LONGS_EQUAL(3,dims[1]);

  // traceExecution of shallow tree
-  typedef UnaryExpression<Point2, Point3> Unary;
-  typedef BinaryExpression<Point3, Pose3, Point3> Binary;
+  typedef internal::UnaryExpression<Point2, Point3> Unary;
+  typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
  size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record);
-  EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record));
+  EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record));
 #ifdef GTSAM_USE_QUATERNIONS
  EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record));
-  LONGS_EQUAL(112+352, expectedTraceSize);
+  LONGS_EQUAL(96+352, expectedTraceSize);
 #else
-  EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record));
-  LONGS_EQUAL(112+400, expectedTraceSize);
+  EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record));
+  LONGS_EQUAL(96+384, expectedTraceSize);
 #endif
  size_t size = expression.traceSize();
  CHECK(size);
  EXPECT_LONGS_EQUAL(expectedTraceSize, size);
-  ExecutionTraceStorage traceStorage[size];
-  ExecutionTrace<Point2> trace;
+  internal::ExecutionTraceStorage traceStorage[size];
+  internal::ExecutionTrace<Point2> trace;
  Point2 value = expression.traceExecution(values, trace, traceStorage);
  EXPECT(assert_equal(Point2(),value, 1e-9));
  // trace.print();
@ -277,7 +281,7 @@ TEST(ExpressionFactor, Shallow) {
  // Check matrices
  boost::optional<Unary::Record*> r = trace.record<Unary::Record>();
  CHECK(r);
-  EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian<Point3, 1>(), 1e-9));
+  EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9));

  // Linearization
  ExpressionFactor<Point2> f2(model, measured, expression);
@ -327,7 +331,7 @@ TEST(ExpressionFactor, tree) {
  boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
  EXPECT( assert_equal(*expected, *gf2, 1e-9));

-  TernaryExpression<Point2, Pose3, Point3, Cal3_S2>::Function fff = project6;
+  Expression<Point2>::TernaryFunction<Pose3, Point3, Cal3_S2>::type fff = project6;

  // Try ternary version
  ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
@ -493,6 +497,11 @@ TEST(ExpressionFactor, tree_finite_differences) {
  EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance);
 }

+TEST(ExpressionFactor, push_back) {
+  NonlinearFactorGraph graph;
+  graph.addExpressionFactor(model, Point2(0, 0), leaf::p);
+}
+
 /* ************************************************************************* */
 int main() {
  TestResult tr;
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@ -57,6 +57,11 @@ namespace gtsam {
      Base(model, key), prior_(prior) {
    }

+    /** Convenience constructor that takes a full covariance argument */
+    PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) :
+      Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) {
+    }
+
    /// @return a deep copy of this factor
    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@ -64,8 +64,8 @@ string findExampleDataFile(const string& name) {
  throw
 invalid_argument(
    "gtsam::findExampleDataFile could not find a matching file in\n"
-    SOURCE_TREE_DATASET_DIR " or\n"
-    INSTALLED_DATASET_DIR " named\n" +
+    GTSAM_SOURCE_TREE_DATASET_DIR " or\n"
+    GTSAM_INSTALLED_DATASET_DIR " named\n" +
    name + ", " + name + ".graph, or " + name + ".txt");
 }

--- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
+++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
@ -88,11 +88,13 @@ int main(int argc, char** argv){
  }

  Values initial_pose_values = initial_estimate.filter<Pose3>();
-  if(output_poses){
-    init_pose3Out.open(init_poseOutput.c_str(),ios::out);
-    for(int i = 1; i<=initial_pose_values.size(); i++){
-      init_pose3Out << i << " " << initial_pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
-        " ", " ")) << endl;
+  if (output_poses) {
+    init_pose3Out.open(init_poseOutput.c_str(), ios::out);
+    for (size_t i = 1; i <= initial_pose_values.size(); i++) {
+      init_pose3Out
+          << i << " "
+          << initial_pose_values.at<Pose3>(Symbol('x', i)).matrix().format(
+              Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl;
    }
  }
  
@ -141,7 +143,7 @@ int main(int argc, char** argv){

  if(output_poses){
    pose3Out.open(poseOutput.c_str(),ios::out);
-    for(int i = 1; i<=pose_values.size(); i++){
+    for(size_t i = 1; i<=pose_values.size(); i++){
      pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
        " ", " ")) << endl;
    }
--- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
@ -28,25 +28,34 @@
 namespace gtsam {

 /* ************************************************************************* */
-void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
+void BatchFixedLagSmoother::print(const std::string& s,
+    const KeyFormatter& keyFormatter) const {
  FixedLagSmoother::print(s, keyFormatter);
  // TODO: What else to print?
 }

 /* ************************************************************************* */
-bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const {
-  const BatchFixedLagSmoother* e =  dynamic_cast<const BatchFixedLagSmoother*> (&rhs);
-  return e != NULL
-      && FixedLagSmoother::equals(*e, tol)
-      && factors_.equals(e->factors_, tol)
-      && theta_.equals(e->theta_, tol);
+bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs,
+    double tol) const {
+  const BatchFixedLagSmoother* e =
+      dynamic_cast<const BatchFixedLagSmoother*>(&rhs);
+  return e != NULL && FixedLagSmoother::equals(*e, tol)
+      && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol);
 }

 /* ************************************************************************* */
-FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) {
+Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const {
+  throw std::runtime_error(
+      "BatchFixedLagSmoother::marginalCovariance not implemented");
+}
+
+/* ************************************************************************* */
+FixedLagSmoother::Result BatchFixedLagSmoother::update(
+    const NonlinearFactorGraph& newFactors, const Values& newTheta,
+    const KeyTimestampMap& timestamps) {

  const bool debug = ISDEBUG("BatchFixedLagSmoother update");
-  if(debug) {
+  if (debug) {
    std::cout << "BatchFixedLagSmoother::update() START" << std::endl;
  }

@ -70,11 +79,13 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap

  // Get current timestamp
  double current_timestamp = getCurrentTimestamp();
-  if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl;
+  if (debug)
+    std::cout << "Current Timestamp: " << current_timestamp << std::endl;

  // Find the set of variables to be marginalized out
-  std::set<Key> marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_);
-  if(debug) {
+  std::set<Key> marginalizableKeys = findKeysBefore(
+      current_timestamp - smootherLag_);
+  if (debug) {
    std::cout << "Marginalizable Keys: ";
    BOOST_FOREACH(Key key, marginalizableKeys) {
      std::cout << DefaultKeyFormatter(key) << " ";
@ -90,19 +101,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
  // Optimize
  gttic(optimize);
  Result result;
-  if(factors_.size() > 0) {
+  if (factors_.size() > 0) {
    result = optimize();
  }
  gttoc(optimize);

  // Marginalize out old variables.
  gttic(marginalize);
-  if(marginalizableKeys.size() > 0) {
+  if (marginalizableKeys.size() > 0) {
    marginalize(marginalizableKeys);
  }
  gttoc(marginalize);

-  if(debug) {
+  if (debug) {
    std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl;
  }

@ -110,11 +121,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
 }

 /* ************************************************************************* */
-void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) {
+void BatchFixedLagSmoother::insertFactors(
+    const NonlinearFactorGraph& newFactors) {
  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) {
    Key index;
    // Insert the factor into an existing hole in the factor graph, if possible
-    if(availableSlots_.size() > 0) {
+    if (availableSlots_.size() > 0) {
      index = availableSlots_.front();
      availableSlots_.pop();
      factors_.replace(index, factor);
@ -130,9 +142,10 @@ void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors
 }

 /* ************************************************************************* */
-void BatchFixedLagSmoother::removeFactors(const std::set<size_t>& deleteFactors) {
+void BatchFixedLagSmoother::removeFactors(
+    const std::set<size_t>& deleteFactors) {
  BOOST_FOREACH(size_t slot, deleteFactors) {
-    if(factors_.at(slot)) {
+    if (factors_.at(slot)) {
      // Remove references to this factor from the FactorIndex
      BOOST_FOREACH(Key key, *(factors_.at(slot))) {
        factorIndex_[key].erase(slot);
@ -143,7 +156,8 @@ void BatchFixedLagSmoother::removeFactors(const std::set<size_t>& deleteFactors)
      availableSlots_.push(slot);
    } else {
      // TODO: Throw an error??
-      std::cout << "Attempting to remove a factor from slot " << slot << ", but it is already NULL." << std::endl;
+      std::cout << "Attempting to remove a factor from slot " << slot
+          << ", but it is already NULL." << std::endl;
    }
  }
 }
@ -159,7 +173,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
    factorIndex_.erase(key);

    // Erase the key from the set of linearized keys
-    if(linearKeys_.exists(key)) {
+    if (linearKeys_.exists(key)) {
      linearKeys_.erase(key);
    }
  }
@ -178,11 +192,11 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {

  const bool debug = ISDEBUG("BatchFixedLagSmoother reorder");

-  if(debug) {
+  if (debug) {
    std::cout << "BatchFixedLagSmoother::reorder() START" << std::endl;
  }

-  if(debug) {
+  if (debug) {
    std::cout << "Marginalizable Keys: ";
    BOOST_FOREACH(Key key, marginalizeKeys) {
      std::cout << DefaultKeyFormatter(key) << " ";
@ -191,13 +205,14 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
  }

  // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
-  ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()));
+  ordering_ = Ordering::colamdConstrainedFirst(factors_,
+      std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()));

-  if(debug) {
+  if (debug) {
    ordering_.print("New Ordering: ");
  }

-  if(debug) {
+  if (debug) {
    std::cout << "BatchFixedLagSmoother::reorder() FINISH" << std::endl;
  }
 }
@ -207,7 +222,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {

  const bool debug = ISDEBUG("BatchFixedLagSmoother optimize");

-  if(debug) {
+  if (debug) {
    std::cout << "BatchFixedLagSmoother::optimize() START" << std::endl;
  }

@ -231,14 +246,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
  result.error = factors_.error(evalpoint);

  // check if we're already close enough
-  if(result.error <= errorTol) {
-    if(debug) { std::cout << "BatchFixedLagSmoother::optimize  Exiting, as error = " << result.error << " < " << errorTol << std::endl; }
+  if (result.error <= errorTol) {
+    if (debug) {
+      std::cout << "BatchFixedLagSmoother::optimize  Exiting, as error = "
+          << result.error << " < " << errorTol << std::endl;
+    }
    return result;
  }

-  if(debug) {
-    std::cout << "BatchFixedLagSmoother::optimize  linearValues: " << linearKeys_.size() << std::endl;
-    std::cout << "BatchFixedLagSmoother::optimize  Initial error: " << result.error << std::endl;
+  if (debug) {
+    std::cout << "BatchFixedLagSmoother::optimize  linearValues: "
+        << linearKeys_.size() << std::endl;
+    std::cout << "BatchFixedLagSmoother::optimize  Initial error: "
+        << result.error << std::endl;
  }

  // Use a custom optimization loop so the linearization points can be controlled
@ -254,9 +274,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
      GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_);

      // Keep increasing lambda until we make make progress
-      while(true) {
+      while (true) {

-        if(debug) { std::cout << "BatchFixedLagSmoother::optimize  trying lambda = " << lambda << std::endl; }
+        if (debug) {
+          std::cout << "BatchFixedLagSmoother::optimize  trying lambda = "
+              << lambda << std::endl;
+        }

        // Add prior factors at the current solution
        gttic(damp);
@ -267,10 +290,11 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
          double sigma = 1.0 / std::sqrt(lambda);
          BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) {
            size_t dim = key_value.second.size();
-            Matrix A = Matrix::Identity(dim,dim);
+            Matrix A = Matrix::Identity(dim, dim);
            Vector b = key_value.second;
            SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
-            GaussianFactor::shared_ptr prior(new JacobianFactor(key_value.first, A, b, model));
+            GaussianFactor::shared_ptr prior(
+                new JacobianFactor(key_value.first, A, b, model));
            dampedFactorGraph.push_back(prior);
          }
        }
@ -279,7 +303,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {

        gttic(solve);
        // Solve Damped Gaussian Factor Graph
-        newDelta = dampedFactorGraph.optimize(ordering_, parameters_.getEliminationFunction());
+        newDelta = dampedFactorGraph.optimize(ordering_,
+            parameters_.getEliminationFunction());
        // update the evalpoint with the new delta
        evalpoint = theta_.retract(newDelta);
        gttoc(solve);
@ -289,12 +314,14 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
        double error = factors_.error(evalpoint);
        gttoc(compute_error);

-        if(debug) {
-          std::cout << "BatchFixedLagSmoother::optimize  linear delta norm = " << newDelta.norm() << std::endl;
-          std::cout << "BatchFixedLagSmoother::optimize  next error = " << error << std::endl;
+        if (debug) {
+          std::cout << "BatchFixedLagSmoother::optimize  linear delta norm = "
+              << newDelta.norm() << std::endl;
+          std::cout << "BatchFixedLagSmoother::optimize  next error = " << error
+              << std::endl;
        }

-        if(error < result.error) {
+        if (error < result.error) {
          // Keep this change
          // Update the error value
          result.error = error;
@ -303,7 +330,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
          // Reset the deltas to zeros
          delta_.setZero();
          // Put the linearization points and deltas back for specific variables
-          if(enforceConsistency_ && (linearKeys_.size() > 0)) {
+          if (enforceConsistency_ && (linearKeys_.size() > 0)) {
            theta_.update(linearKeys_);
            BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) {
              delta_.at(key_value.key) = newDelta.at(key_value.key);
@ -311,16 +338,18 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
          }
          // Decrease lambda for next time
          lambda /= lambdaFactor;
-          if(lambda < lambdaLowerBound) {
+          if (lambda < lambdaLowerBound) {
            lambda = lambdaLowerBound;
          }
          // End this lambda search iteration
          break;
        } else {
          // Reject this change
-          if(lambda >= lambdaUpperBound) {
+          if (lambda >= lambdaUpperBound) {
            // The maximum lambda has been used. Print a warning and end the search.
-            std::cout << "Warning:  Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
+            std::cout
+                << "Warning:  Levenberg-Marquardt giving up because cannot decrease error with maximum lambda"
+                << std::endl;
            break;
          } else {
            // Increase lambda and continue searching
@ -331,15 +360,22 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
    }
    gttoc(optimizer_iteration);

-    if(debug) { std::cout << "BatchFixedLagSmoother::optimize  using lambda = " << lambda << std::endl; }
+    if (debug) {
+      std::cout << "BatchFixedLagSmoother::optimize  using lambda = " << lambda
+          << std::endl;
+    }

    result.iterations++;
-  } while(result.iterations < maxIterations &&
-      !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT));
+  } while (result.iterations < maxIterations
+      && !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol,
+          previousError, result.error, NonlinearOptimizerParams::SILENT));

-  if(debug) { std::cout << "BatchFixedLagSmoother::optimize  newError: " << result.error << std::endl; }
+  if (debug) {
+    std::cout << "BatchFixedLagSmoother::optimize  newError: " << result.error
+        << std::endl;
+  }

-  if(debug) {
+  if (debug) {
    std::cout << "BatchFixedLagSmoother::optimize() FINISH" << std::endl;
  }

@ -356,9 +392,10 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {

  const bool debug = ISDEBUG("BatchFixedLagSmoother marginalize");

-  if(debug) std::cout << "BatchFixedLagSmoother::marginalize  Begin" << std::endl;
+  if (debug)
+    std::cout << "BatchFixedLagSmoother::marginalize  Begin" << std::endl;

-  if(debug) {
+  if (debug) {
    std::cout << "BatchFixedLagSmoother::marginalize  Marginalize Keys: ";
    BOOST_FOREACH(Key key, marginalizeKeys) {
      std::cout << DefaultKeyFormatter(key) << " ";
@ -374,7 +411,7 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
    removedFactorSlots.insert(slots.begin(), slots.end());
  }

-  if(debug) {
+  if (debug) {
    std::cout << "BatchFixedLagSmoother::marginalize  Removed Factor Slots: ";
    BOOST_FOREACH(size_t slot, removedFactorSlots) {
      std::cout << slot << " ";
@ -385,20 +422,24 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
  // Add the removed factors to a factor graph
  NonlinearFactorGraph removedFactors;
  BOOST_FOREACH(size_t slot, removedFactorSlots) {
-    if(factors_.at(slot)) {
+    if (factors_.at(slot)) {
      removedFactors.push_back(factors_.at(slot));
    }
  }

-  if(debug) {
-    PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize  Removed Factors: ");
+  if (debug) {
+    PrintSymbolicGraph(removedFactors,
+        "BatchFixedLagSmoother::marginalize  Removed Factors: ");
  }

  // Calculate marginal factors on the remaining keys
-  NonlinearFactorGraph marginalFactors = calculateMarginalFactors(removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction());
+  NonlinearFactorGraph marginalFactors = calculateMarginalFactors(
+      removedFactors, theta_, marginalizeKeys,
+      parameters_.getEliminationFunction());

-  if(debug) {
-    PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize  Marginal Factors: ");
+  if (debug) {
+    PrintSymbolicGraph(removedFactors,
+        "BatchFixedLagSmoother::marginalize  Marginal Factors: ");
  }

  // Remove marginalized factors from the factor graph
@ -412,7 +453,8 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
 }

 /* ************************************************************************* */
-void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const std::string& label) {
+void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
+    const std::string& label) {
  std::cout << label;
  BOOST_FOREACH(gtsam::Key key, keys) {
    std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -421,7 +463,8 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const std::st
 }

 /* ************************************************************************* */
-void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys, const std::string& label) {
+void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys,
+    const std::string& label) {
  std::cout << label;
  BOOST_FOREACH(gtsam::Key key, keys) {
    std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -430,9 +473,10 @@ void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys, const s
 }

 /* ************************************************************************* */
-void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) {
+void BatchFixedLagSmoother::PrintSymbolicFactor(
+    const NonlinearFactor::shared_ptr& factor) {
  std::cout << "f(";
-  if(factor) {
+  if (factor) {
    BOOST_FOREACH(Key key, factor->keys()) {
      std::cout << " " << gtsam::DefaultKeyFormatter(key);
    }
@ -443,7 +487,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt
 }

 /* ************************************************************************* */
-void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) {
+void BatchFixedLagSmoother::PrintSymbolicFactor(
+    const GaussianFactor::shared_ptr& factor) {
  std::cout << "f(";
  BOOST_FOREACH(Key key, factor->keys()) {
    std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -452,7 +497,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr
 }

 /* ************************************************************************* */
-void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) {
+void BatchFixedLagSmoother::PrintSymbolicGraph(
+    const NonlinearFactorGraph& graph, const std::string& label) {
  std::cout << label << std::endl;
  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) {
    PrintSymbolicFactor(factor);
@ -460,59 +506,79 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph
 }

 /* ************************************************************************* */
-void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) {
+void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph,
+    const std::string& label) {
  std::cout << label << std::endl;
  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
    PrintSymbolicFactor(factor);
  }
 }

-
-
 /* ************************************************************************* */
-NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
-    const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
+NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
+    const NonlinearFactorGraph& graph, const Values& theta,
+    const std::set<Key>& marginalizeKeys,
+    const GaussianFactorGraph::Eliminate& eliminateFunction) {

  const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors");

-  if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" << std::endl;
+  if (debug)
+    std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START"
+        << std::endl;

-  if(debug) PrintKeySet(marginalizeKeys, "BatchFixedLagSmoother::calculateMarginalFactors  Marginalize Keys: ");
+  if (debug)
+    PrintKeySet(marginalizeKeys,
+        "BatchFixedLagSmoother::calculateMarginalFactors  Marginalize Keys: ");

  // Get the set of all keys involved in the factor graph
  FastSet<Key> allKeys(graph.keys());
-  if(debug) PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors  All Keys: ");
+  if (debug)
+    PrintKeySet(allKeys,
+        "BatchFixedLagSmoother::calculateMarginalFactors  All Keys: ");

  // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys
  FastSet<Key> remainingKeys;
-  std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end()));
-  if(debug) PrintKeySet(remainingKeys, "BatchFixedLagSmoother::calculateMarginalFactors  Remaining Keys: ");
+  std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(),
+      marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end()));
+  if (debug)
+    PrintKeySet(remainingKeys,
+        "BatchFixedLagSmoother::calculateMarginalFactors  Remaining Keys: ");

-  if(marginalizeKeys.size() == 0) {
+  if (marginalizeKeys.size() == 0) {
    // There are no keys to marginalize. Simply return the input factors
-    if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl;
+    if (debug)
+      std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH"
+          << std::endl;
    return graph;
  } else {

    // Create the linear factor graph
    GaussianFactorGraph linearFactorGraph = *graph.linearize(theta);
    // .first is the eliminated Bayes tree, while .second is the remaining factor graph
-    GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end())).second;
+    GaussianFactorGraph marginalLinearFactors =
+        *linearFactorGraph.eliminatePartialMultifrontal(
+            std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end())).second;

    // Wrap in nonlinear container factors
    NonlinearFactorGraph marginalFactors;
    marginalFactors.reserve(marginalLinearFactors.size());
    BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) {
-      marginalFactors += boost::make_shared<LinearContainerFactor>(gaussianFactor, theta);
-      if(debug) {
-        std::cout << "BatchFixedLagSmoother::calculateMarginalFactors  Marginal Factor: ";
+      marginalFactors += boost::make_shared<LinearContainerFactor>(
+          gaussianFactor, theta);
+      if (debug) {
+        std::cout
+            << "BatchFixedLagSmoother::calculateMarginalFactors  Marginal Factor: ";
        PrintSymbolicFactor(marginalFactors.back());
      }
    }

-    if(debug) PrintSymbolicGraph(marginalFactors, "BatchFixedLagSmoother::calculateMarginalFactors  All Marginal Factors: ");
+    if (debug)
+      PrintSymbolicGraph(marginalFactors,
+          "BatchFixedLagSmoother::calculateMarginalFactors  All Marginal Factors: ");

-    if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl;
+    if (debug)
+      std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH"
+          << std::endl;

    return marginalFactors;
  }
--- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
@ -100,6 +100,12 @@ public:
    return delta_;
  }

+  /// Calculate marginal covariance on given variable
+  Matrix marginalCovariance(Key key) const;
+
+  static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
+      const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
+
 protected:

  /** A typedef defining an Key-Factor mapping **/
@ -134,8 +140,6 @@ protected:
  /** A cross-reference structure to allow efficient factor lookups by key **/
  FactorIndex factorIndex_;

-
-
  /** Augment the list of factors with a set of new factors */
  void insertFactors(const NonlinearFactorGraph& newFactors);

@ -154,9 +158,6 @@ protected:
  /** Marginalize out selected variables */
  void marginalize(const std::set<Key>& marginalizableKeys);

-  static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
-      const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
-
 private:
  /** Private methods for printing debug information */
  static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
--- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp
@ -37,7 +37,7 @@ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const {
 void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) {
  // Loop through each key and add/update it in the map
  BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) {
-    // Check to see if this key already exists inthe database
+    // Check to see if this key already exists in the database
    KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first);

    // If the key already exists
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@ -25,10 +25,13 @@
 namespace gtsam {

 /* ************************************************************************* */
-void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
+void recursiveMarkAffectedKeys(const Key& key,
+    const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {

  // Check if the separator keys of the current clique contain the specified key
-  if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) {
+  if (std::find(clique->conditional()->beginParents(),
+      clique->conditional()->endParents(), key)
+      != clique->conditional()->endParents()) {

    // Mark the frontal keys of the current clique
    BOOST_FOREACH(Key i, clique->conditional()->frontals()) {
@ -44,32 +47,36 @@ void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& cl
 }

 /* ************************************************************************* */
-void IncrementalFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
+void IncrementalFixedLagSmoother::print(const std::string& s,
+    const KeyFormatter& keyFormatter) const {
  FixedLagSmoother::print(s, keyFormatter);
  // TODO: What else to print?
 }

 /* ************************************************************************* */
-bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const {
-  const IncrementalFixedLagSmoother* e =  dynamic_cast<const IncrementalFixedLagSmoother*> (&rhs);
-  return e != NULL
-      && FixedLagSmoother::equals(*e, tol)
+bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs,
+    double tol) const {
+  const IncrementalFixedLagSmoother* e =
+      dynamic_cast<const IncrementalFixedLagSmoother*>(&rhs);
+  return e != NULL && FixedLagSmoother::equals(*e, tol)
      && isam_.equals(e->isam_, tol);
 }

 /* ************************************************************************* */
-FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) {
+FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
+    const NonlinearFactorGraph& newFactors, const Values& newTheta,
+    const KeyTimestampMap& timestamps) {

  const bool debug = ISDEBUG("IncrementalFixedLagSmoother update");

-  if(debug) {
+  if (debug) {
    std::cout << "IncrementalFixedLagSmoother::update() Start" << std::endl;
    PrintSymbolicTree(isam_, "Bayes Tree Before Update:");
    std::cout << "END" << std::endl;
  }

  FastVector<size_t> removedFactors;
-  boost::optional<FastMap<Key,int> > constrainedKeys = boost::none;
+  boost::optional<FastMap<Key, int> > constrainedKeys = boost::none;

  // Update the Timestamps associated with the factor keys
  updateKeyTimestampMap(timestamps);
@ -77,12 +84,14 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
  // Get current timestamp
  double current_timestamp = getCurrentTimestamp();

-  if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl;
+  if (debug)
+    std::cout << "Current Timestamp: " << current_timestamp << std::endl;

  // Find the set of variables to be marginalized out
-  std::set<Key> marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_);
+  std::set<Key> marginalizableKeys = findKeysBefore(
+      current_timestamp - smootherLag_);

-  if(debug) {
+  if (debug) {
    std::cout << "Marginalizable Keys: ";
    BOOST_FOREACH(Key key, marginalizableKeys) {
      std::cout << DefaultKeyFormatter(key) << " ";
@ -93,11 +102,13 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
  // Force iSAM2 to put the marginalizable variables at the beginning
  createOrderingConstraints(marginalizableKeys, constrainedKeys);

-  if(debug) {
+  if (debug) {
    std::cout << "Constrained Keys: ";
-    if(constrainedKeys) {
-      for(FastMap<Key,int>::const_iterator iter = constrainedKeys->begin(); iter != constrainedKeys->end(); ++iter) {
-        std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second << ")  ";
+    if (constrainedKeys) {
+      for (FastMap<Key, int>::const_iterator iter = constrainedKeys->begin();
+          iter != constrainedKeys->end(); ++iter) {
+        std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second
+            << ")  ";
      }
    }
    std::cout << std::endl;
@ -114,23 +125,26 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
  KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());

  // Update iSAM2
-  ISAM2Result isamResult = isam_.update(newFactors, newTheta, FastVector<size_t>(), constrainedKeys, boost::none, additionalMarkedKeys);
+  ISAM2Result isamResult = isam_.update(newFactors, newTheta,
+      FastVector<size_t>(), constrainedKeys, boost::none, additionalMarkedKeys);

-  if(debug) {
-    PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:");
+  if (debug) {
+    PrintSymbolicTree(isam_,
+        "Bayes Tree After Update, Before Marginalization:");
    std::cout << "END" << std::endl;
  }

  // Marginalize out any needed variables
-  if(marginalizableKeys.size() > 0) {
-    FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
+  if (marginalizableKeys.size() > 0) {
+    FastList<Key> leafKeys(marginalizableKeys.begin(),
+        marginalizableKeys.end());
    isam_.marginalizeLeaves(leafKeys);
  }

  // Remove marginalized keys from the KeyTimestampMap
  eraseKeyTimestampMap(marginalizableKeys);

-  if(debug) {
+  if (debug) {
    PrintSymbolicTree(isam_, "Final Bayes Tree:");
    std::cout << "END" << std::endl;
  }
@ -142,7 +156,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
  result.nonlinearVariables = 0;
  result.error = 0;

-  if(debug) std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl;
+  if (debug)
+    std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl;

  return result;
 }
@ -151,30 +166,33 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
 void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) {
  TimestampKeyMap::iterator end = timestampKeyMap_.lower_bound(timestamp);
  TimestampKeyMap::iterator iter = timestampKeyMap_.begin();
-  while(iter != end) {
+  while (iter != end) {
    keyTimestampMap_.erase(iter->second);
    timestampKeyMap_.erase(iter++);
  }
 }

 /* ************************************************************************* */
-void IncrementalFixedLagSmoother::createOrderingConstraints(const std::set<Key>& marginalizableKeys, boost::optional<FastMap<Key,int> >& constrainedKeys) const {
-  if(marginalizableKeys.size() > 0) {
-    constrainedKeys = FastMap<Key,int>();
+void IncrementalFixedLagSmoother::createOrderingConstraints(
+    const std::set<Key>& marginalizableKeys,
+    boost::optional<FastMap<Key, int> >& constrainedKeys) const {
+  if (marginalizableKeys.size() > 0) {
+    constrainedKeys = FastMap<Key, int>();
    // Generate ordering constraints so that the marginalizable variables will be eliminated first
    // Set all variables to Group1
    BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) {
      constrainedKeys->operator[](timestamp_key.second) = 1;
    }
    // Set marginalizable variables to Group0
-    BOOST_FOREACH(Key key, marginalizableKeys){
+    BOOST_FOREACH(Key key, marginalizableKeys) {
      constrainedKeys->operator[](key) = 0;
    }
  }
 }

 /* ************************************************************************* */
-void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const std::string& label) {
+void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
+    const std::string& label) {
  std::cout << label;
  BOOST_FOREACH(gtsam::Key key, keys) {
    std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -183,7 +201,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const s
 }

 /* ************************************************************************* */
-void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) {
+void IncrementalFixedLagSmoother::PrintSymbolicFactor(
+    const GaussianFactor::shared_ptr& factor) {
  std::cout << "f(";
  BOOST_FOREACH(gtsam::Key key, factor->keys()) {
    std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -192,7 +211,8 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shar
 }

 /* ************************************************************************* */
-void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) {
+void IncrementalFixedLagSmoother::PrintSymbolicGraph(
+    const GaussianFactorGraph& graph, const std::string& label) {
  std::cout << label << std::endl;
  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
    PrintSymbolicFactor(factor);
@ -200,27 +220,28 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph&
 }

 /* ************************************************************************* */
-void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) {
+void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam,
+    const std::string& label) {
  std::cout << label << std::endl;
-  if(!isam.roots().empty())
-  {
-    BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){
-    PrintSymbolicTreeHelper(root);
+  if (!isam.roots().empty()) {
+    BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) {
+      PrintSymbolicTreeHelper(root);
    }
-  }
-  else
+  } else
    std::cout << "{Empty Tree}" << std::endl;
 }

 /* ************************************************************************* */
-void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) {
+void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(
+    const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) {

  // Print the current clique
  std::cout << indent << "P( ";
  BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) {
    std::cout << gtsam::DefaultKeyFormatter(key) << " ";
  }
-  if(clique->conditional()->nrParents() > 0) std::cout << "| ";
+  if (clique->conditional()->nrParents() > 0)
+    std::cout << "| ";
  BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) {
    std::cout << gtsam::DefaultKeyFormatter(key) << " ";
  }
@ -228,7 +249,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Cliq

  // Recursively print all of the children
  BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
-    PrintSymbolicTreeHelper(child, indent+" ");
+    PrintSymbolicTreeHelper(child, indent + " ");
  }
 }

--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
@ -23,7 +23,6 @@
 #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
 #include <gtsam/nonlinear/ISAM2.h>

-
 namespace gtsam {

 /**
@ -31,7 +30,7 @@ namespace gtsam {
 * such that the active states are placed in/near the root. This base class implements a function
 * to calculate the ordering, and an update function to incorporate new factors into the HMF.
 */
-class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoother {
+class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {

 public:

@ -39,20 +38,30 @@ public:
  typedef boost::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;

  /** default constructor */
-  IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = ISAM2Params())
-  : FixedLagSmoother(smootherLag), isam_(parameters) {}
+  IncrementalFixedLagSmoother(double smootherLag = 0.0,
+      const ISAM2Params& parameters = ISAM2Params()) :
+      FixedLagSmoother(smootherLag), isam_(parameters) {
+  }

  /** destructor */
-  virtual ~IncrementalFixedLagSmoother() {}
+  virtual ~IncrementalFixedLagSmoother() {
+  }

  /** Print the factor for debugging and testing (implementing Testable) */
-  virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

  /** Check if two IncrementalFixedLagSmoother Objects are equal */
  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;

-  /** Add new factors, updating the solution and relinearizing as needed. */
-  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
+  /**
+   * Add new factors, updating the solution and re-linearizing as needed.
+   * @param newFactors new factors on old and/or new variables
+   * @param newTheta new values for new variables only
+   * @param timestamps an (optional) map from keys to real time stamps
+   */
+  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
+      const Values& newTheta = Values(), //
      const KeyTimestampMap& timestamps = KeyTimestampMap());

  /** Compute an estimate from the incomplete linear delta computed during the last update.
@ -94,6 +103,11 @@ public:
    return isam_.getDelta();
  }

+  /// Calculate marginal covariance on given variable
+  Matrix marginalCovariance(Key key) const {
+    return isam_.marginalCovariance(key);
+  }
+
 protected:
  /** An iSAM2 object used to perform inference. The smoother lag is controlled
   * by what factors are removed each iteration */
@ -103,16 +117,23 @@ protected:
  void eraseKeysBefore(double timestamp);

  /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */
-  void createOrderingConstraints(const std::set<Key>& marginalizableKeys, boost::optional<FastMap<Key,int> >& constrainedKeys) const;
+  void createOrderingConstraints(const std::set<Key>& marginalizableKeys,
+      boost::optional<FastMap<Key, int> >& constrainedKeys) const;

 private:
  /** Private methods for printing debug information */
-  static void PrintKeySet(const std::set<Key>& keys, const std::string& label = "Keys:");
+  static void PrintKeySet(const std::set<Key>& keys, const std::string& label =
+      "Keys:");
  static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
-  static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = "Factor Graph:");
-  static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:");
-  static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = "");
+  static void PrintSymbolicGraph(const GaussianFactorGraph& graph,
+      const std::string& label = "Factor Graph:");
+  static void PrintSymbolicTree(const gtsam::ISAM2& isam,
+      const std::string& label = "Bayes Tree:");
+  static void PrintSymbolicTreeHelper(
+      const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent =
+          "");

-}; // IncrementalFixedLagSmoother
+};
+// IncrementalFixedLagSmoother

-} /// namespace gtsam
+}/// namespace gtsam
--- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
@ -1,247 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testExpressionMeta.cpp
- * @date October 14, 2014
- * @author Frank Dellaert
- * @brief Test meta-programming constructs for Expressions
- */
-
-#include <gtsam/nonlinear/ExpressionFactor.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/base/Testable.h>
-
-#include <CppUnitLite/TestHarness.h>
-#include <algorithm>
-
-using namespace std;
-using namespace gtsam;
-
-/* ************************************************************************* */
-namespace mpl = boost::mpl;
-
-#include <boost/mpl/assert.hpp>
-#include <boost/mpl/equal.hpp>
-template<class T> struct Incomplete;
-
-// Check generation of FunctionalNode
-typedef mpl::vector<Pose3, Point3> MyTypes;
-typedef FunctionalNode<Point2, MyTypes>::type Generated;
-//Incomplete<Generated> incomplete;
-
-// Try generating vectors of ExecutionTrace
-typedef mpl::vector<ExecutionTrace<Pose3>, ExecutionTrace<Point3> > ExpectedTraces;
-
-typedef mpl::transform<MyTypes, ExecutionTrace<MPL::_1> >::type MyTraces;
-BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >));
-
-template<class T>
-struct MakeTrace {
-  typedef ExecutionTrace<T> type;
-};
-typedef mpl::transform<MyTypes, MakeTrace<MPL::_1> >::type MyTraces1;
-BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >));
-
-// Try generating vectors of Expression types
-typedef mpl::vector<Expression<Pose3>, Expression<Point3> > ExpectedExpressions;
-typedef mpl::transform<MyTypes, Expression<MPL::_1> >::type Expressions;
-BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >));
-
-// Try generating vectors of Jacobian types
-typedef mpl::vector<Matrix26, Matrix23> ExpectedJacobians;
-typedef mpl::transform<MyTypes, Jacobian<Point2, MPL::_1> >::type Jacobians;
-BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >));
-
-// Try accessing a Jacobian
-typedef mpl::at_c<Jacobians, 1>::type Jacobian23; // base zero !
-BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>));
-
-/* ************************************************************************* */
-// Boost Fusion includes allow us to create/access values from MPL vectors
-#include <boost/fusion/include/mpl.hpp>
-#include <boost/fusion/include/at_c.hpp>
-
-// Create a value and access it
-TEST(ExpressionFactor, JacobiansValue) {
-  using boost::fusion::at_c;
-  Matrix23 expected;
-  Jacobians jacobians;
-
-  at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6;
-
-  Matrix23 actual = at_c<1>(jacobians);
-  CHECK(actual.cols() == expected.cols());
-  CHECK(actual.rows() == expected.rows());
-}
-
-/* ************************************************************************* */
-// 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 F>
-  struct result<F(int)> {
-    typedef double type; // result for int argument
-  };
-
-  template<class F>
-  struct result<F(const int&)> {
-    typedef double type; // result for int argument
-  };
-
-  template<class F>
-  struct result<F(const double &)> {
-    typedef double type; // result for double argument
-  };
-
-  template<class F>
-  struct result<F(double)> {
-    typedef double type; // result for double argument
-  };
-
-  // actual function
-  template<typename T>
-  typename result<triple(T)>::type operator()(const T& x) const {
-    return (double) x;
-  }
-};
-
-// Test out polymorphic transform
-TEST(ExpressionFactor, Triple) {
-  typedef boost::fusion::vector<int, double> IntDouble;
-  IntDouble H = boost::fusion::make_vector(1, 2.0);
-
-  // Only works if I use Double2
-  typedef boost::fusion::result_of::transform<IntDouble, triple>::type Result;
-  typedef boost::fusion::vector<double, double> Double2;
-  Double2 D = boost::fusion::transform(H, triple());
-
-  using boost::fusion::at_c;
-  DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9);
-  DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9);
-}
-
-/* ************************************************************************* */
-#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 invoke
-TEST(ExpressionFactor, Invoke) {
-  EXPECT_LONGS_EQUAL(2, invoke(plus<int>(),boost::fusion::make_vector(1,1)));
-
-  // Creating a Pose3 (is there another way?)
-  boost::fusion::vector<Rot3, Point3> pair;
-  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, MakeOptionalJacobian<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,
-      OptionalJacobian<3,6> Dpose,
-      OptionalJacobian<3,3> 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;
-  return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */
-
--- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
@ -17,19 +17,20 @@
 */


-#include <CppUnitLite/TestHarness.h>
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
-#include <gtsam/base/debug.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/inference/Key.h>
-#include <gtsam/inference/Ordering.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/geometry/Point2.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Ordering.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/base/debug.h>
+
+#include <CppUnitLite/TestHarness.h>

 using namespace std;
 using namespace gtsam;
--- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp
+++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp
@ -73,9 +73,8 @@ TEST( InvDepthFactorVariant1, optimize) {
  // Optimize the graph to recover the actual landmark position
  LevenbergMarquardtParams params;
  Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
-  Vector6 actual = result.at<Vector6>(landmarkKey);
-  

+//  Vector6 actual = result.at<Vector6>(landmarkKey);
 //  values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
 //  result.at<Pose3>(poseKey1).print("Pose1 After:\n");
 //  pose1.print("Pose1 Truth:\n");
@ -116,8 +115,11 @@ TEST( InvDepthFactorVariant1, optimize) {
  // However, since this is an over-parameterization, there can be
  // many 6D landmark values that equate to the same 3D world position
  // Instead, directly test the recovered 3D landmark position
-  //EXPECT(assert_equal(expected, actual, 1e-9));
  EXPECT(assert_equal(landmark, world_landmarkAfter, 1e-9));
+
+  // Frank asks: why commented out?
+  //Vector6 actual = result.at<Vector6>(landmarkKey);
+  //EXPECT(assert_equal(expected, actual, 1e-9));
 }


--- a/wrap/tests/testWrap.cpp
+++ b/wrap/tests/testWrap.cpp
@ -41,10 +41,6 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define

 typedef vector<string> strvec;

-// NOTE: as this path is only used to generate makefiles, it is hardcoded here for testing
-// In practice, this path will be an absolute system path, which makes testing it more annoying
-static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap";
-
 /* ************************************************************************* */
 TEST( wrap, ArgumentList ) {
  ArgumentList args;
--- a/wrap/utilities.cpp
+++ b/wrap/utilities.cpp
@ -91,7 +91,8 @@ bool files_equal(const string& expected, const string& actual, bool skipheader)
      cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n";
      stringstream command;
      command << "diff --ignore-all-space " << expected << " " << actual << endl;
-      system(command.str().c_str());
+      if (system(command.str().c_str())<0)
+        throw "command '" + command.str() + "' failed";
      cerr << ">>>\n";
    }
    return equal;