diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index 29081efba..c624c900e 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -150,7 +150,8 @@ typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
 template<class Y, class X>
 typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
     double delta = 1e-5) {
-  return numericalDerivative11<Y, X>(boost::bind(h, boost::placeholders::_1), x, delta);
+  return numericalDerivative11<Y, X>(boost::bind(h, boost::placeholders::_1), x,
+                                     delta);
 }
 
 /**
@@ -169,14 +170,17 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      boost::bind(h, boost::placeholders::_1, boost::cref(x2)), x1, delta);
 }
 
 /** use a raw C++ function pointer */
 template<class Y, class X1, class X2>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
     const X2& x2, double delta = 1e-5) {
-  return numericalDerivative21<Y, X1, X2>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta);
+  return numericalDerivative21<Y, X1, X2>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
+      delta);
 }
 
 /**
@@ -195,14 +199,17 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::func
 //       "Template argument X1 must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
        "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      boost::bind(h, boost::cref(x1), boost::placeholders::_1), x2, delta);
 }
 
 /** use a raw C++ function pointer */
 template<class Y, class X1, class X2>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
     const X2& x2, double delta = 1e-5) {
-  return numericalDerivative22<Y, X1, X2>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta);
+  return numericalDerivative22<Y, X1, X2>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
+      delta);
 }
 
 /**
@@ -224,14 +231,18 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)),
+      x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
-  return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
-      x2, x3, delta);
+  return numericalDerivative31<Y, X1, X2, X3>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3),
+      x1, x2, x3, delta);
 }
 
 /**
@@ -253,14 +264,18 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
       "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)),
+      x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
-  return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
-      x2, x3, delta);
+  return numericalDerivative32<Y, X1, X2, X3>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3),
+      x1, x2, x3, delta);
 }
 
 /**
@@ -282,14 +297,18 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
       "Template argument X3 must be a manifold type.");
-  return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1), x3, delta);
+  return numericalDerivative11<Y, X3, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1),
+      x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
-  return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
-      x2, x3, delta);
+  return numericalDerivative33<Y, X1, X2, X3>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3),
+      x1, x2, x3, delta);
 }
 
 /**
@@ -311,13 +330,19 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
+                  boost::cref(x4)),
+      x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
-  return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4);
+  return numericalDerivative41<Y, X1, X2, X3, X4>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4),
+      x1, x2, x3, x4);
 }
 
 /**
@@ -339,13 +364,19 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
       "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4)), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
+                  boost::cref(x4)),
+      x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
   const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
-  return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4);
+  return numericalDerivative42<Y, X1, X2, X3, X4>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4),
+      x1, x2, x3, x4);
 }
 
 /**
@@ -367,13 +398,19 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
       "Template argument X3 must be a manifold type.");
-  return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4)), x3, delta);
+  return numericalDerivative11<Y, X3, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
+                  boost::cref(x4)),
+      x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
-  return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4);
+  return numericalDerivative43<Y, X1, X2, X3, X4>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4),
+      x1, x2, x3, x4);
 }
 
 /**
@@ -395,13 +432,19 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
       "Template argument X4 must be a manifold type.");
-  return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1), x4, delta);
+  return numericalDerivative11<Y, X4, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+                  boost::placeholders::_1),
+      x4, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
   const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
-  return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4);
+  return numericalDerivative44<Y, X1, X2, X3, X4>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4),
+      x1, x2, x3, x4);
 }
 
 /**
@@ -424,13 +467,20 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
+                  boost::cref(x4), boost::cref(x5)),
+      x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
-  return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
+  return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -453,13 +503,20 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
+                  boost::cref(x4), boost::cref(x5)),
+      x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
-  return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
+  return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -482,13 +539,20 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4), boost::cref(x5)), x3, delta);
+  return numericalDerivative11<Y, X3, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
+                  boost::cref(x4), boost::cref(x5)),
+      x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
-  return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
+  return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -511,13 +575,20 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1, boost::cref(x5)), x4, delta);
+  return numericalDerivative11<Y, X4, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+                  boost::placeholders::_1, boost::cref(x5)),
+      x4, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
-  return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
+  return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -540,13 +611,20 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::placeholders::_1), x5, delta);
+  return numericalDerivative11<Y, X5, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+                  boost::cref(x4), boost::placeholders::_1),
+      x5, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
-  return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
+  return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -570,13 +648,20 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
+                  boost::cref(x4), boost::cref(x5), boost::cref(x6)),
+      x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
-  return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5, boost::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -600,13 +685,20 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
+                  boost::cref(x4), boost::cref(x5), boost::cref(x6)),
+      x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
-  return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5, boost::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -630,13 +722,20 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
+  return numericalDerivative11<Y, X3, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
+                  boost::cref(x4), boost::cref(x5), boost::cref(x6)),
+      x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
-  return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5, boost::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -660,13 +759,20 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1, boost::cref(x5), boost::cref(x6)), x4, delta);
+  return numericalDerivative11<Y, X4, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+                  boost::placeholders::_1, boost::cref(x5), boost::cref(x6)),
+      x4, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
-  return numericalDerivative64<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5, boost::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -690,13 +796,20 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::placeholders::_1, boost::cref(x6)), x5, delta);
+  return numericalDerivative11<Y, X5, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+                  boost::cref(x4), boost::placeholders::_1, boost::cref(x6)),
+      x5, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
-  return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5, boost::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -721,13 +834,20 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X6, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::placeholders::_1), x6, delta);
+  return numericalDerivative11<Y, X6, N>(
+      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+                  boost::cref(x4), boost::cref(x5), boost::placeholders::_1),
+      x6, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
-  return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
+      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3, boost::placeholders::_4,
+                  boost::placeholders::_5, boost::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -747,8 +867,8 @@ inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::fun
   typedef boost::function<double(const X&)> F;
   typedef boost::function<VectorD(F, const X&, double)> G;
   G ng = static_cast<G>(numericalGradient<X> );
-  return numericalDerivative11<VectorD, X>(boost::bind(ng, f, boost::placeholders::_1, delta), x,
-      delta);
+  return numericalDerivative11<VectorD, X>(
+      boost::bind(ng, f, boost::placeholders::_1, delta), x, delta);
 }
 
 template<class X>
@@ -773,7 +893,8 @@ public:
       f_(f), x1_(x1), delta_(delta) {
   }
   Vector operator()(const X2& x2) {
-    return numericalGradient<X1>(boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_);
+    return numericalGradient<X1>(
+        boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_);
   }
 };
 
@@ -785,7 +906,8 @@ inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
   G_x1<X1, X2> g_x1(f, x1, delta);
   return numericalDerivative11<Vector, X2>(
       boost::function<Vector(const X2&)>(
-          boost::bind<Vector>(boost::ref(g_x1), boost::placeholders::_1)), x2, delta);
+          boost::bind<Vector>(boost::ref(g_x1), boost::placeholders::_1)),
+      x2, delta);
 }
 
 template<class X1, class X2>
@@ -804,10 +926,12 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
 
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(boost::bind(f, boost::placeholders::_1, boost::cref(x2)));
+  boost::function<double(const X1&)> f2(
+      boost::bind(f, boost::placeholders::_1, boost::cref(x2)));
 
   return numericalDerivative11<Vector, X1>(
-      boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      boost::function<Vector(const X1&)>(
+          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x1, delta);
 }
 
@@ -825,10 +949,12 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), boost::placeholders::_1));
+  boost::function<double(const X2&)> f2(
+      boost::bind(f, boost::cref(x1), boost::placeholders::_1));
 
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      boost::function<Vector(const X2&)>(
+          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x2, delta);
 }
 
@@ -850,10 +976,12 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)));
+  boost::function<double(const X1&)> f2(boost::bind(
+      f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)));
 
   return numericalDerivative11<Vector, X1>(
-      boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      boost::function<Vector(const X1&)>(
+          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x1, delta);
 }
 
@@ -873,10 +1001,12 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)));
+  boost::function<double(const X2&)> f2(boost::bind(
+      f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)));
 
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      boost::function<Vector(const X2&)>(
+          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x2, delta);
 }
 
@@ -896,10 +1026,12 @@ inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
   typedef typename internal::FixedSizeMatrix<X3>::type Vector;
   Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
       double) = &numericalGradient<X3>;
-  boost::function<double(const X3&)> f2(boost::bind(f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1));
+  boost::function<double(const X3&)> f2(boost::bind(
+      f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1));
 
   return numericalDerivative11<Vector, X3>(
-      boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      boost::function<Vector(const X3&)>(
+          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x3, delta);
 }
 
@@ -917,7 +1049,9 @@ inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X2>(
-      boost::function<double(const X1&, const X2&)>(boost::bind(f, boost::placeholders::_1, boost::placeholders::_2, boost::cref(x3))),
+      boost::function<double(const X1&, const X2&)>(
+          boost::bind(f, boost::placeholders::_1, boost::placeholders::_2,
+                      boost::cref(x3))),
       x1, x2, delta);
 }
 
@@ -926,7 +1060,9 @@ inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X3>(
-      boost::function<double(const X1&, const X3&)>(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::placeholders::_2)),
+      boost::function<double(const X1&, const X3&)>(
+          boost::bind(f, boost::placeholders::_1, boost::cref(x2),
+                      boost::placeholders::_2)),
       x1, x3, delta);
 }
 
@@ -935,7 +1071,9 @@ inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X2, X3>(
-      boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), boost::placeholders::_1, boost::placeholders::_2)),
+      boost::function<double(const X2&, const X3&)>(
+          boost::bind(f, boost::cref(x1), boost::placeholders::_1,
+                      boost::placeholders::_2)),
       x2, x3, delta);
 }
 
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
index 6a345a6b5..a6638c1d7 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -308,38 +308,68 @@ public:
     // TODO: Write analytical derivative calculations
     // Jacobian w.r.t. Pose1
     if (H1){
-      Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1);
-      Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1);
+      Matrix H1_Pose = numericalDerivative11<POSE, POSE>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
+          Pose1);
+      Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
+          Pose1);
       *H1 = stack(2, &H1_Pose, &H1_Vel);
     }
 
     // Jacobian w.r.t. Vel1
     if (H2){
       if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
-      Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1);
-      Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1);
+      Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
+          Vel1);
+      Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
+          Vel1);
       *H2 = stack(2, &H2_Pose, &H2_Vel);
     }
 
     // Jacobian w.r.t. IMUBias1
     if (H3){
-      Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1);
-      Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1);
+      Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
+          Bias1);
+      Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
+          Bias1);
       *H3 = stack(2, &H3_Pose, &H3_Vel);
     }
 
     // Jacobian w.r.t. Pose2
     if (H4){
-      Matrix H4_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2);
-      Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2);
+      Matrix H4_Pose = numericalDerivative11<POSE, POSE>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
+          Pose2);
+      Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
+          Pose2);
       *H4 = stack(2, &H4_Pose, &H4_Vel);
     }
 
     // Jacobian w.r.t. Vel2
     if (H5){
       if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
-      Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2);
-      Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2);
+      Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
+          Vel2);
+      Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(
+          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
+          Vel2);
       *H5 = stack(2, &H5_Pose, &H5_Vel);
     }
 
@@ -438,18 +468,45 @@ public:
     Matrix Z_3x3 = Z_3x3;
     Matrix I_3x3 = I_3x3;
 
-    Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, boost::placeholders::_1, delta_vel_in_t0), delta_pos_in_t0);
-    Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, boost::placeholders::_1), delta_vel_in_t0);
+    Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(
+        boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
+                    boost::placeholders::_1, delta_vel_in_t0),
+        delta_pos_in_t0);
+    Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(
+        boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
+                    delta_pos_in_t0, boost::placeholders::_1),
+        delta_vel_in_t0);
     Matrix H_pos_angles = Z_3x3;
     Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
 
-    Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, boost::placeholders::_1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
-    Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, boost::placeholders::_1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
-    Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, boost::placeholders::_1), Bias_t0);
+    Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(
+        boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
+                    msr_acc_t, msr_dt, delta_angles, boost::placeholders::_1,
+                    flag_use_body_P_sensor, body_P_sensor, Bias_t0),
+        delta_vel_in_t0);
+    Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(
+        boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
+                    msr_acc_t, msr_dt, boost::placeholders::_1, delta_vel_in_t0,
+                    flag_use_body_P_sensor, body_P_sensor, Bias_t0),
+        delta_angles);
+    Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(
+        boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
+                    msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0,
+                    flag_use_body_P_sensor, body_P_sensor,
+                    boost::placeholders::_1),
+        Bias_t0);
     Matrix H_vel_pos = Z_3x3;
 
-    Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, boost::placeholders::_1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
-    Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, boost::placeholders::_1), Bias_t0);
+    Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(
+        boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
+                    msr_dt, boost::placeholders::_1, flag_use_body_P_sensor,
+                    body_P_sensor, Bias_t0),
+        delta_angles);
+    Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(
+        boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
+                    msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor,
+                    boost::placeholders::_1),
+        Bias_t0);
     Matrix H_angles_pos = Z_3x3;
     Matrix H_angles_vel = Z_3x3;
 
diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
index 968810e0a..3f526e934 100644
--- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
+++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
@@ -235,38 +235,68 @@ public:
     // TODO: Write analytical derivative calculations
     // Jacobian w.r.t. Pose1
     if (H1){
-      Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1);
-      Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1);
+      Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
+          Pose1);
+      Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
+          Pose1);
       *H1 = stack(2, &H1_Pose, &H1_Vel);
     }
 
     // Jacobian w.r.t. Vel1
     if (H2){
       if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
-      Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1);
-      Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1);
+      Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
+          Vel1);
+      Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
+          Vel1);
       *H2 = stack(2, &H2_Pose, &H2_Vel);
     }
 
     // Jacobian w.r.t. IMUBias1
     if (H3){
-      Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1);
-      Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1);
+      Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
+          Bias1);
+      Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
+          Bias1);
       *H3 = stack(2, &H3_Pose, &H3_Vel);
     }
 
     // Jacobian w.r.t. Pose2
     if (H4){
-      Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2);
-      Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2);
+      Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
+          Pose2);
+      Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
+          Pose2);
       *H4 = stack(2, &H4_Pose, &H4_Vel);
     }
 
     // Jacobian w.r.t. Vel2
     if (H5){
       if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
-      Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2);
-      Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2);
+      Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
+          Vel2);
+      Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
+          boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
+          Vel2);
       *H5 = stack(2, &H5_Pose, &H5_Vel);
     }
 
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h
index 7292d702b..a9dcb8cef 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant1.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h
@@ -108,8 +108,9 @@ public:
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
-          boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, boost::placeholders::_1,
-              landmark), pose);
+          boost::bind(&InvDepthFactorVariant1::inverseDepthError, this,
+                      boost::placeholders::_1, landmark),
+          pose);
     }
     if (H2) {
       (*H2) = numericalDerivative11<Vector, Vector6>(
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h
index d8c790342..4595a934b 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant3.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h
@@ -110,10 +110,16 @@ public:
       boost::optional<Matrix&> H2=boost::none) const override {
 
     if(H1) {
-      (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, boost::placeholders::_1, landmark), pose);
+      (*H1) = numericalDerivative11<Vector, Pose3>(
+          boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this,
+                      boost::placeholders::_1, landmark),
+          pose);
     }
     if(H2) {
-      (*H2) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, boost::placeholders::_1), landmark);
+      (*H2) = numericalDerivative11<Vector, Vector3>(
+          boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose,
+                      boost::placeholders::_1),
+          landmark);
     }
 
     return inverseDepthError(pose, landmark);
@@ -231,13 +237,22 @@ public:
       boost::optional<Matrix&> H3=boost::none) const override {
 
     if(H1)
-      (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, boost::placeholders::_1, pose2, landmark), pose1);
+      (*H1) = numericalDerivative11<Vector, Pose3>(
+          boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this,
+                      boost::placeholders::_1, pose2, landmark),
+          pose1);
 
     if(H2)
-      (*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, boost::placeholders::_1, landmark), pose2);
+      (*H2) = numericalDerivative11<Vector, Pose3>(
+          boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
+                      boost::placeholders::_1, landmark),
+          pose2);
 
     if(H3)
-      (*H3) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, boost::placeholders::_1), landmark);
+      (*H3) = numericalDerivative11<Vector, Vector3>(
+          boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
+                      pose2, boost::placeholders::_1),
+          landmark);
 
     return inverseDepthError(pose1, pose2, landmark);
   }