diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 275d943e8..553a7fd5f 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -6,23 +6,24 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <gtsam/base/debug.h>
-#include <gtsam/base/TestableAssertions.h>
-#include <gtsam/base/treeTraversal-inst.h>
+#include <tests/smallExample.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/inference/Ordering.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianBayesTree.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
-#include <tests/smallExample.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianBayesTree.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/base/LieScalar.h>
 
 #include <boost/foreach.hpp>
 #include <boost/assign/list_of.hpp>
@@ -34,6 +35,9 @@ using namespace std;
 using namespace gtsam;
 using boost::shared_ptr;
 
+static const SharedNoiseModel model;
+static const LieScalar Zero(0);
+
 //  SETDEBUG("ISAM2 update", true);
 //  SETDEBUG("ISAM2 update verbose", true);
 //  SETDEBUG("ISAM2 recalculate", true);
@@ -202,11 +206,11 @@ struct ConsistencyVisitor
     consistent(true), isam(isam) {}
   int operator()(const ISAM2::sharedClique& node, int& parentData)
   {
-    if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
+    if(find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
     {
       if(node->parent_.expired())
         consistent = false;
-      if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
+      if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
         consistent = false;
     }
     BOOST_FOREACH(Key j, node->conditional()->frontals())
@@ -222,7 +226,7 @@ struct ConsistencyVisitor
 bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {
 
   TestResult& result_ = result;
-  const std::string name_ = test.getName();
+  const string name_ = test.getName();
 
   Values actual = isam.calculateEstimate();
   Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize());
@@ -284,19 +288,19 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
 TEST(ISAM2, AddFactorsStep1)
 {
   NonlinearFactorGraph nonlinearFactors;
-  nonlinearFactors += PriorFactor<double>(10, 0.0, gtsam::SharedNoiseModel());
+  nonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
   nonlinearFactors += NonlinearFactor::shared_ptr();
-  nonlinearFactors += PriorFactor<double>(11, 0.0, gtsam::SharedNoiseModel());
+  nonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
 
   NonlinearFactorGraph newFactors;
-  newFactors += PriorFactor<double>(1, 0.0, gtsam::SharedNoiseModel());
-  newFactors += PriorFactor<double>(2, 0.0, gtsam::SharedNoiseModel());
+  newFactors += PriorFactor<LieScalar>(1, Zero, model);
+  newFactors += PriorFactor<LieScalar>(2, Zero, model);
 
   NonlinearFactorGraph expectedNonlinearFactors;
-  expectedNonlinearFactors += PriorFactor<double>(10, 0.0, gtsam::SharedNoiseModel());
-  expectedNonlinearFactors += PriorFactor<double>(1, 0.0, gtsam::SharedNoiseModel());
-  expectedNonlinearFactors += PriorFactor<double>(11, 0.0, gtsam::SharedNoiseModel());
-  expectedNonlinearFactors += PriorFactor<double>(2, 0.0, gtsam::SharedNoiseModel());
+  expectedNonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
+  expectedNonlinearFactors += PriorFactor<LieScalar>(1, Zero, model);
+  expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
+  expectedNonlinearFactors += PriorFactor<LieScalar>(2, Zero, model);
 
   const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3);
 
@@ -693,18 +697,17 @@ namespace {
 TEST(ISAM2, marginalizeLeaves1)
 {
   ISAM2 isam;
-
   NonlinearFactorGraph factors;
-  factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
+  factors += PriorFactor<LieScalar>(0, Zero, model);
 
-  factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
+  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
 
   Values values;
-  values.insert(0, 0.0);
-  values.insert(1, 0.0);
-  values.insert(2, 0.0);
+  values.insert(0, Zero);
+  values.insert(1, Zero);
+  values.insert(2, Zero);
 
   FastMap<Key,int> constrainedKeys;
   constrainedKeys.insert(make_pair(0,0));
@@ -723,18 +726,18 @@ TEST(ISAM2, marginalizeLeaves2)
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
+  factors += PriorFactor<LieScalar>(0, Zero, model);
 
-  factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(2, 3, 0.0, noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
+  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(2, 3, Zero, model);
 
   Values values;
-  values.insert(0, 0.0);
-  values.insert(1, 0.0);
-  values.insert(2, 0.0);
-  values.insert(3, 0.0);
+  values.insert(0, Zero);
+  values.insert(1, Zero);
+  values.insert(2, Zero);
+  values.insert(3, Zero);
 
   FastMap<Key,int> constrainedKeys;
   constrainedKeys.insert(make_pair(0,0));
@@ -754,25 +757,25 @@ TEST(ISAM2, marginalizeLeaves3)
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
+  factors += PriorFactor<LieScalar>(0, Zero, model);
 
-  factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
+  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
 
-  factors += BetweenFactor<double>(2, 3, 0.0, noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(2, 3, Zero, model);
 
-  factors += BetweenFactor<double>(3, 4, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(4, 5, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(3, 5, 0.0, noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(3, 4, Zero, model);
+  factors += BetweenFactor<LieScalar>(4, 5, Zero, model);
+  factors += BetweenFactor<LieScalar>(3, 5, Zero, model);
 
   Values values;
-  values.insert(0, 0.0);
-  values.insert(1, 0.0);
-  values.insert(2, 0.0);
-  values.insert(3, 0.0);
-  values.insert(4, 0.0);
-  values.insert(5, 0.0);
+  values.insert(0, Zero);
+  values.insert(1, Zero);
+  values.insert(2, Zero);
+  values.insert(3, Zero);
+  values.insert(4, Zero);
+  values.insert(5, Zero);
 
   FastMap<Key,int> constrainedKeys;
   constrainedKeys.insert(make_pair(0,0));
@@ -794,14 +797,14 @@ TEST(ISAM2, marginalizeLeaves4)
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
-  factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
+  factors += PriorFactor<LieScalar>(0, Zero, model);
+  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
 
   Values values;
-  values.insert(0, 0.0);
-  values.insert(1, 0.0);
-  values.insert(2, 0.0);
+  values.insert(0, Zero);
+  values.insert(1, Zero);
+  values.insert(2, Zero);
 
   FastMap<Key,int> constrainedKeys;
   constrainedKeys.insert(make_pair(0,0));