From a637737d5e48bd58338a16b2e245bb0b6c5854f6 Mon Sep 17 00:00:00 2001
From: yetongumich <yetong@umich.edu>
Date: Wed, 12 May 2021 15:36:12 -0400
Subject: [PATCH 01/72] refactor tests and add comments

---
 gtsam/nonlinear/tests/testExpression.cpp | 75 +++++++++++++++---------
 tests/testExpressionFactor.cpp           | 59 ++++++++++---------
 2 files changed, 78 insertions(+), 56 deletions(-)

diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp
index 8fcf84a113..012a5ae9cd 100644
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@@ -58,22 +58,23 @@ TEST(Expression, Constant) {
 /* ************************************************************************* */
 // Leaf
 TEST(Expression, Leaf) {
-  Rot3_ R(100);
+  const Key key = 100;
+  Rot3_ R(key);
   Values values;
-  values.insert(100, someR);
+  values.insert(key, someR);
 
   Rot3 actual2 = R.value(values);
   EXPECT(assert_equal(someR, actual2));
 }
 
 /* ************************************************************************* */
-// Many Leaves
+// Test the function `createUnknowns` to create many leaves at once.
 TEST(Expression, Leaves) {
   Values values;
-  Point3 somePoint(1, 2, 3);
+  const Point3 somePoint(1, 2, 3);
   values.insert(Symbol('p', 10), somePoint);
-  std::vector<Point3_> points = createUnknowns<Point3>(10, 'p', 1);
-  EXPECT(assert_equal(somePoint, points.back().value(values)));
+  std::vector<Point3_> pointExpressions = createUnknowns<Point3>(10, 'p', 1);
+  EXPECT(assert_equal(somePoint, pointExpressions.back().value(values)));
 }
 
 /* ************************************************************************* */
@@ -88,29 +89,34 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) {
 Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
   return p;
 }
-Point3_ p(1);
+Point3_ pointExpression(1);
 set<Key> expected = list_of(1);
 }  // namespace unary
 
+// Create a unary expression that takes another expression as a single argument.
 TEST(Expression, Unary1) {
   using namespace unary;
-  Expression<Point2> e(f1, p);
-  EXPECT(expected == e.keys());
+  Expression<Point2> unaryExpression(f1, pointExpression);
+  EXPECT(expected == unaryExpression.keys());
 }
+
+// Check that also works with a scalar return value.
 TEST(Expression, Unary2) {
   using namespace unary;
-  Double_ e(f2, p);
-  EXPECT(expected == e.keys());
+  Double_ unaryExpression(f2, pointExpression);
+  EXPECT(expected == unaryExpression.keys());
 }
 
-/* ************************************************************************* */
 // Unary(Leaf), dynamic
 TEST(Expression, Unary3) {
   using namespace unary;
-  //  Expression<Vector> e(f3, p);
+  // TODO(yetongumich): dynamic output arguments do not work yet!
+  // Expression<Vector> unaryExpression(f3, pointExpression);
+  // EXPECT(expected == unaryExpression.keys());
 }
 
 /* ************************************************************************* */
+// Simple test class that implements the `VectorSpace` protocol.
 class Class : public Point3 {
  public:
   enum {dimension = 3};
@@ -133,16 +139,20 @@ template<> struct traits<Class> : public internal::VectorSpace<Class> {};
 // Nullary Method
 TEST(Expression, NullaryMethod) {
   // Create expression
-  Expression<Class> p(67);
-  Expression<double> norm_(p, &Class::norm);
+  const Key key(67);
+  Expression<Class> classExpression(key);
+
+  // Make expression from a class method, note how it differs from the function
+  // expressions by leading with the class expression in the constructor.
+  Expression<double> norm_(classExpression, &Class::norm);
 
   // Create Values
   Values values;
-  values.insert(67, Class(3, 4, 5));
+  values.insert(key, Class(3, 4, 5));
 
   // Check dims as map
   std::map<Key, int> map;
-  norm_.dims(map);
+  norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention.
   LONGS_EQUAL(1, map.size());
 
   // Get value and Jacobians
@@ -150,9 +160,10 @@ TEST(Expression, NullaryMethod) {
   double actual = norm_.value(values, H);
 
   // Check all
-  EXPECT(actual == sqrt(50));
+  const double norm = sqrt(3*3 + 4*4 + 5*5);
+  EXPECT(actual == norm);
   Matrix expected(1, 3);
-  expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
+  expected << 3.0 / norm, 4.0 / norm, 5.0 / norm;
   EXPECT(assert_equal(expected, H[0]));
 }
 
@@ -170,21 +181,21 @@ Point3_ p_cam(x, &Pose3::transformTo, p);
 }
 
 /* ************************************************************************* */
-// Check that creating an expression to double compiles
+// Check that creating an expression to double compiles.
 TEST(Expression, BinaryToDouble) {
   using namespace binary;
   Double_ p_cam(doubleF, x, p);
 }
 
 /* ************************************************************************* */
-// keys
+// Check keys of an expression created from class method.
 TEST(Expression, BinaryKeys) {
   set<Key> expected = list_of(1)(2);
   EXPECT(expected == binary::p_cam.keys());
 }
 
 /* ************************************************************************* */
-// dimensions
+// Check dimensions by calling `dims` method.
 TEST(Expression, BinaryDimensions) {
   map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
   binary::p_cam.dims(actual);
@@ -192,7 +203,7 @@ TEST(Expression, BinaryDimensions) {
 }
 
 /* ************************************************************************* */
-// dimensions
+// Check dimensions by calling `traceSize` method.
 TEST(Expression, BinaryTraceSize) {
   typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
   size_t expectedTraceSize = sizeof(Binary::Record);
@@ -247,6 +258,7 @@ TEST(Expression, TreeTraceSize) {
 }
 
 /* ************************************************************************* */
+// Test compose operation with * operator.
 TEST(Expression, compose1) {
   // Create expression
   Rot3_ R1(1), R2(2);
@@ -258,7 +270,7 @@ TEST(Expression, compose1) {
 }
 
 /* ************************************************************************* */
-// Test compose with arguments referring to the same rotation
+// Test compose with arguments referring to the same rotation.
 TEST(Expression, compose2) {
   // Create expression
   Rot3_ R1(1), R2(1);
@@ -270,7 +282,7 @@ TEST(Expression, compose2) {
 }
 
 /* ************************************************************************* */
-// Test compose with one arguments referring to constant rotation
+// Test compose with one arguments referring to constant rotation.
 TEST(Expression, compose3) {
   // Create expression
   Rot3_ R1(Rot3::identity()), R2(3);
@@ -282,7 +294,7 @@ TEST(Expression, compose3) {
 }
 
 /* ************************************************************************* */
-// Test with ternary function
+// 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) {
   // return dummy derivatives (not correct, but that's ok for testing here)
@@ -306,6 +318,7 @@ TEST(Expression, ternary) {
 }
 
 /* ************************************************************************* */
+// Test scalar multiplication with * operator.
 TEST(Expression, ScalarMultiply) {
   const Key key(67);
   const Point3_ expr = 23 * Point3_(key);
@@ -336,6 +349,7 @@ TEST(Expression, ScalarMultiply) {
 }
 
 /* ************************************************************************* */
+// Test sum with + operator.
 TEST(Expression, BinarySum) {
   const Key key(67);
   const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1));
@@ -366,6 +380,7 @@ TEST(Expression, BinarySum) {
 }
 
 /* ************************************************************************* */
+// Test sum of 3 variables with + operator.
 TEST(Expression, TripleSum) {
   const Key key(67);
   const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@@ -387,6 +402,7 @@ TEST(Expression, TripleSum) {
 }
 
 /* ************************************************************************* */
+// Test sum with += operator.
 TEST(Expression, PlusEqual) {
   const Key key(67);
   const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@@ -454,6 +470,7 @@ TEST(Expression, UnaryOfSum) {
 /* ************************************************************************* */
 TEST(Expression, WeightedSum) {
   const Key key1(42), key2(67);
+  const Point3 point1(1, 0, 0), point2(0, 1, 0);
   const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2);
 
   map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
@@ -461,11 +478,11 @@ TEST(Expression, WeightedSum) {
   EXPECT(actual_dims == expected_dims);
 
   Values values;
-  values.insert<Point3>(key1, Point3(1, 0, 0));
-  values.insert<Point3>(key2, Point3(0, 1, 0));
+  values.insert<Point3>(key1, point1);
+  values.insert<Point3>(key2, point2);
 
   // Check value
-  const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0);
+  const Point3 expected = 17 * point1 + 23 * point2;
   EXPECT(assert_equal(expected, weighted_sum_.value(values)));
 
   // Check value + Jacobians
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index e3e37e7c7d..657e777613 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -59,40 +59,42 @@ Point2_ p(2);
 TEST(ExpressionFactor, Leaf) {
   using namespace leaf;
 
-  // Create old-style factor to create expected value and derivatives
+  // Create old-style factor to create expected value and derivatives.
   PriorFactor<Point2> old(2, Point2(0, 0), model);
 
-  // Concise version
+  // Create the equivalent factor with expression.
   ExpressionFactor<Point2> f(model, Point2(0, 0), p);
+
+  // Check values and derivatives.
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
-// non-zero noise model
+// Test leaf expression with noise model of differnt variance.
 TEST(ExpressionFactor, Model) {
   using namespace leaf;
 
   SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
 
-  // Create old-style factor to create expected value and derivatives
+  // Create old-style factor to create expected value and derivatives.
   PriorFactor<Point2> old(2, Point2(0, 0), model);
 
-  // Concise version
+  // Create the equivalent factor with expression.
   ExpressionFactor<Point2> f(model, Point2(0, 0), p);
 
-  // Check values and derivatives
+  // Check values and derivatives.
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
   EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
 }
 
 /* ************************************************************************* */
-// Constrained noise model
+// Test leaf expression with constrained noise model.
 TEST(ExpressionFactor, Constrained) {
   using namespace leaf;
 
@@ -106,7 +108,7 @@ TEST(ExpressionFactor, Constrained) {
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -130,7 +132,7 @@ TEST(ExpressionFactor, Unary) {
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf, 1e-9));
+  EXPECT(assert_equal(expected, *jf, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -143,11 +145,13 @@ Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
   if (H) *H << I_3x3, I_3x3, I_3x3;
   return v;
 }
+
 typedef Eigen::Matrix<double,9,9> Matrix9;
 Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
   if (H) *H = Matrix9::Identity();
   return v;
 }
+
 TEST(ExpressionFactor, Wide) {
   // Create some values
   Values values;
@@ -208,6 +212,7 @@ TEST(ExpressionFactor, Binary) {
   EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
   EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
 }
+
 /* ************************************************************************* */
 // Unary(Binary(Leaf,Leaf))
 TEST(ExpressionFactor, Shallow) {
@@ -264,7 +269,7 @@ TEST(ExpressionFactor, Shallow) {
   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
-  EXPECT( assert_equal(*expected, *gf2, 1e-9));
+  EXPECT(assert_equal(*expected, *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -297,7 +302,7 @@ TEST(ExpressionFactor, tree) {
   EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
-  EXPECT( assert_equal(*expected, *gf, 1e-9));
+  EXPECT(assert_equal(*expected, *gf, 1e-9));
 
   // Concise version
   ExpressionFactor<Point2> f2(model, measured,
@@ -305,14 +310,14 @@ TEST(ExpressionFactor, tree) {
   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
-  EXPECT( assert_equal(*expected, *gf2, 1e-9));
+  EXPECT(assert_equal(*expected, *gf2, 1e-9));
 
   // Try ternary version
   ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
   EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f3.dim());
   boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
-  EXPECT( assert_equal(*expected, *gf3, 1e-9));
+  EXPECT(assert_equal(*expected, *gf3, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -333,15 +338,15 @@ TEST(ExpressionFactor, Compose1) {
   // Check unwhitenedError
   std::vector<Matrix> H(2);
   Vector actual = f.unwhitenedError(values, H);
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
-  EXPECT( assert_equal(I_3x3, H[1],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[1],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -363,14 +368,14 @@ TEST(ExpressionFactor, compose2) {
   std::vector<Matrix> H(1);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(1, H.size());
-  EXPECT( assert_equal(2*I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(2*I_3x3, H[0],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -392,14 +397,14 @@ TEST(ExpressionFactor, compose3) {
   std::vector<Matrix> H(1);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(1, H.size());
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
 
   // Check linearization
   JacobianFactor expected(3, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -435,16 +440,16 @@ TEST(ExpressionFactor, composeTernary) {
   std::vector<Matrix> H(3);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(3, H.size());
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
-  EXPECT( assert_equal(I_3x3, H[1],1e-9));
-  EXPECT( assert_equal(I_3x3, H[2],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[1],1e-9));
+  EXPECT(assert_equal(I_3x3, H[2],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 TEST(ExpressionFactor, tree_finite_differences) {
@@ -636,7 +641,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
 class TestNaryFactor
     : public gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
                                       gtsam::Rot3, gtsam::Point3, 
-                                      gtsam::Rot3,gtsam::Point3> {
+                                      gtsam::Rot3, gtsam::Point3> {
 private:
   using This = TestNaryFactor;
   using Base =

From 0fe12ec984dc487be74478701e60737eb3338e23 Mon Sep 17 00:00:00 2001
From: yetongumich <yetong@umich.edu>
Date: Wed, 12 May 2021 16:57:27 -0400
Subject: [PATCH 02/72] resolve some nits

---
 gtsam/nonlinear/tests/testExpression.cpp | 4 ++--
 tests/testExpressionFactor.cpp           | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp
index 012a5ae9cd..aa922aa9a9 100644
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@@ -203,7 +203,7 @@ TEST(Expression, BinaryDimensions) {
 }
 
 /* ************************************************************************* */
-// Check dimensions by calling `traceSize` method.
+// Check dimensions of execution trace.
 TEST(Expression, BinaryTraceSize) {
   typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
   size_t expectedTraceSize = sizeof(Binary::Record);
@@ -470,7 +470,6 @@ TEST(Expression, UnaryOfSum) {
 /* ************************************************************************* */
 TEST(Expression, WeightedSum) {
   const Key key1(42), key2(67);
-  const Point3 point1(1, 0, 0), point2(0, 1, 0);
   const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2);
 
   map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
@@ -478,6 +477,7 @@ TEST(Expression, WeightedSum) {
   EXPECT(actual_dims == expected_dims);
 
   Values values;
+  const Point3 point1(1, 0, 0), point2(0, 1, 0);
   values.insert<Point3>(key1, point1);
   values.insert<Point3>(key2, point2);
 
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index 657e777613..87211dbd46 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -73,7 +73,7 @@ TEST(ExpressionFactor, Leaf) {
 }
 
 /* ************************************************************************* */
-// Test leaf expression with noise model of differnt variance.
+// Test leaf expression with noise model of different variance.
 TEST(ExpressionFactor, Model) {
   using namespace leaf;
 

From 650e432f52f58505eba8c8f6a918f03e733a7acc Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Tue, 15 Jun 2021 13:07:08 -0400
Subject: [PATCH 03/72] update boost::bind usage

use <boost/bind/bind.hpp> instead of deprecated <boost/bind.hpp>

use boost::placeholders:: scope in appropriate files

remove and add <boost/bind/bind.hpp> in appropriate files
---
 gtsam/base/numericalDerivative.h              | 89 ++++++++++++-------
 gtsam/geometry/tests/testCalibratedCamera.cpp |  2 +
 gtsam/geometry/tests/testCameraSet.cpp        |  1 -
 gtsam/geometry/tests/testEssentialMatrix.cpp  |  3 +
 gtsam/geometry/tests/testOrientedPlane3.cpp   |  2 +
 gtsam/geometry/tests/testPinholeCamera.cpp    |  2 +
 gtsam/geometry/tests/testPinholeSet.cpp       |  1 -
 gtsam/geometry/tests/testPoint3.cpp           |  3 +
 gtsam/geometry/tests/testPose3.cpp            |  2 +
 gtsam/geometry/tests/testSO3.cpp              |  2 +
 gtsam/geometry/tests/testSimilarity3.cpp      |  3 +-
 gtsam/geometry/tests/testUnit3.cpp            |  3 +-
 gtsam/inference/EliminationTree-inst.h        |  1 -
 gtsam/inference/FactorGraph-inst.h            |  2 -
 gtsam/inference/FactorGraph.h                 |  1 -
 gtsam/inference/LabeledSymbol.cpp             | 10 +--
 gtsam/inference/Symbol.cpp                    |  4 +-
 gtsam/inference/inference-inst.h              |  1 -
 gtsam/linear/HessianFactor.cpp                |  8 --
 gtsam/linear/JacobianFactor.cpp               |  8 --
 gtsam/linear/VectorValues.cpp                 |  5 +-
 gtsam/linear/tests/testGaussianBayesNet.cpp   |  2 +
 gtsam/linear/tests/testGaussianBayesTree.cpp  |  2 +
 gtsam/navigation/tests/testAHRSFactor.cpp     |  3 +-
 gtsam/navigation/tests/testAttitudeFactor.cpp |  3 +
 .../tests/testCombinedImuFactor.cpp           |  1 -
 gtsam/navigation/tests/testGPSFactor.cpp      |  3 +
 gtsam/navigation/tests/testImuBias.cpp        |  3 +-
 gtsam/navigation/tests/testImuFactor.cpp      | 34 +++----
 gtsam/navigation/tests/testMagFactor.cpp      |  3 +
 gtsam/navigation/tests/testMagPoseFactor.cpp  |  3 +
 .../tests/testManifoldPreintegration.cpp      |  6 +-
 gtsam/navigation/tests/testNavState.cpp       |  3 +
 gtsam/navigation/tests/testScenario.cpp       |  3 +-
 .../tests/testTangentPreintegration.cpp       |  6 +-
 gtsam/nonlinear/Expression-inl.h              | 22 +++--
 gtsam/nonlinear/Expression.h                  |  1 -
 gtsam/nonlinear/NonlinearEquality.h           |  8 +-
 gtsam/nonlinear/Values-inl.h                  |  8 +-
 gtsam/nonlinear/Values.cpp                    |  8 --
 gtsam/nonlinear/Values.h                      |  9 --
 gtsam/nonlinear/tests/testValues.cpp          |  2 +
 gtsam/sam/tests/testRangeFactor.cpp           |  3 +-
 gtsam/sfm/tests/testTranslationFactor.cpp     |  2 +
 gtsam/slam/tests/testBetweenFactor.cpp        |  3 +
 .../tests/testEssentialMatrixConstraint.cpp   |  3 +
 .../slam/tests/testEssentialMatrixFactor.cpp  |  2 +
 gtsam/slam/tests/testOrientedPlane3Factor.cpp |  3 +-
 gtsam/slam/tests/testReferenceFrameFactor.cpp |  3 +-
 gtsam/slam/tests/testRotateFactor.cpp         |  3 +-
 .../tests/testSmartProjectionPoseFactor.cpp   |  2 +
 gtsam/slam/tests/testTriangulationFactor.cpp  |  2 +
 gtsam_unstable/dynamics/FullIMUFactor.h       |  6 +-
 gtsam_unstable/dynamics/IMUFactor.h           |  6 +-
 gtsam_unstable/dynamics/VelocityConstraint.h  |  8 +-
 .../dynamics/tests/testSimpleHelicopter.cpp   |  2 +
 .../examples/TimeOfArrivalExample.cpp         |  1 -
 gtsam_unstable/geometry/tests/testEvent.cpp   |  3 +-
 gtsam_unstable/linear/QPSParser.cpp           |  2 +
 .../slam/EquivInertialNavFactor_GlobalVel.h   |  5 ++
 .../slam/InertialNavFactor_GlobalVelocity.h   | 21 ++---
 gtsam_unstable/slam/InvDepthFactorVariant1.h  |  6 +-
 gtsam_unstable/slam/InvDepthFactorVariant2.h  |  8 +-
 gtsam_unstable/slam/InvDepthFactorVariant3.h  | 12 +--
 .../slam/tests/testBiasedGPSFactor.cpp        |  3 +
 .../testEquivInertialNavFactor_GlobalVel.cpp  |  3 +
 .../tests/testGaussMarkov1stOrderFactor.cpp   |  3 +
 .../testInertialNavFactor_GlobalVelocity.cpp  |  2 +
 .../tests/testLocalOrientedPlane3Factor.cpp   |  3 +-
 .../slam/tests/testPartialPriorFactor.cpp     |  3 +
 .../slam/tests/testPoseBetweenFactor.cpp      |  3 +
 .../slam/tests/testPosePriorFactor.cpp        |  3 +
 .../slam/tests/testProjectionFactorPPP.cpp    |  3 +-
 .../slam/tests/testProjectionFactorPPPC.cpp   |  3 +-
 .../tests/testRelativeElevationFactor.cpp     |  2 +
 gtsam_unstable/slam/tests/testTOAFactor.cpp   |  1 -
 gtsam_unstable/slam/tests/testTSAMFactors.cpp |  3 +
 .../testTransformBtwRobotsUnaryFactor.cpp     |  2 +
 .../testTransformBtwRobotsUnaryFactorEM.cpp   |  2 +
 tests/testDoglegOptimizer.cpp                 |  8 --
 tests/testExpressionFactor.cpp                |  2 +
 tests/testSimulated3D.cpp                     |  2 +
 82 files changed, 279 insertions(+), 164 deletions(-)

diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index fc5531cdc2..a05a1dda82 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -19,14 +19,7 @@
 #pragma once
 
 #include <boost/function.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/bind.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
+#include <boost/bind/bind.hpp>
 
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/JacobianFactor.h>
@@ -45,13 +38,13 @@ namespace gtsam {
  *   for a function with one relevant param and an optional derivative:
  *     Foo bar(const Obj& a, boost::optional<Matrix&> H1)
  *   Use boost.bind to restructure:
- *     boost::bind(bar, _1, boost::none)
+ *     boost::bind(bar, boost::placeholders::_1, boost::none)
  *   This syntax will fix the optional argument to boost::none, while using the first argument provided
  *
  * For member functions, such as below, with an instantiated copy instanceOfSomeClass
  *     Foo SomeClass::bar(const Obj& a)
  * Use boost bind as follows to create a function pointer that uses the member function:
- *       boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1)
+ *       boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), boost::placeholders::_1)
  *
  * For additional details, see the documentation:
  *     http://www.boost.org/doc/libs/release/libs/bind/bind.html
@@ -157,7 +150,7 @@ 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, _1), x, delta);
+  return numericalDerivative11<Y, X>(boost::bind(h, boost::placeholders::_1), x, delta);
 }
 
 /**
@@ -176,13 +169,14 @@ 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, _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) {
+  using namespace boost::placeholders;
   return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
 }
 
@@ -202,13 +196,14 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
 }
 
@@ -231,12 +226,13 @@ 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, _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) {
+  using namespace boost::placeholders;
   return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
       x2, x3, delta);
 }
@@ -260,12 +256,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
       x2, x3, delta);
 }
@@ -289,12 +286,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
       x2, x3, delta);
 }
@@ -318,12 +316,13 @@ 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, _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) {
+  using namespace boost::placeholders;
   return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
 }
 
@@ -346,12 +345,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
 }
 
@@ -374,12 +374,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
 }
 
@@ -402,12 +403,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
 }
 
@@ -431,12 +433,13 @@ 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, _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) {
+  using namespace boost::placeholders;
   return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
 }
 
@@ -460,12 +463,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
 }
 
@@ -489,12 +493,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
 }
 
@@ -518,12 +523,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
 }
 
@@ -547,12 +553,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
 }
 
@@ -577,12 +584,13 @@ 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, _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) {
+  using namespace boost::placeholders;
   return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
 }
 
@@ -607,12 +615,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
 }
 
@@ -637,12 +646,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
 }
 
@@ -667,12 +677,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative64<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
 }
 
@@ -697,12 +708,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
 }
 
@@ -728,12 +740,13 @@ 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), _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) {
+  using namespace boost::placeholders;
   return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
 }
 
@@ -754,7 +767,7 @@ 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, _1, delta), x,
+  return numericalDerivative11<VectorD, X>(boost::bind(ng, f, boost::placeholders::_1, delta), x,
       delta);
 }
 
@@ -780,7 +793,7 @@ class G_x1 {
       f_(f), x1_(x1), delta_(delta) {
   }
   Vector operator()(const X2& x2) {
-    return numericalGradient<X1>(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_);
+    return numericalGradient<X1>(boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_);
   }
 };
 
@@ -792,7 +805,7 @@ 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), _1)), x2, delta);
+          boost::bind<Vector>(boost::ref(g_x1), boost::placeholders::_1)), x2, delta);
 }
 
 template<class X1, class X2>
@@ -807,6 +820,7 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
     boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
 
+  using namespace boost::placeholders;
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
 
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
@@ -829,6 +843,7 @@ template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
     boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
+  using namespace boost::placeholders;
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
@@ -854,6 +869,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
+  using namespace boost::placeholders;
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
@@ -877,6 +893,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
+  using namespace boost::placeholders;
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
@@ -900,6 +917,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
+  using namespace boost::placeholders;
   typedef typename internal::FixedSizeMatrix<X3>::type Vector;
   Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
       double) = &numericalGradient<X3>;
@@ -923,6 +941,7 @@ template<class X1, class X2, class X3>
 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) {
+  using namespace boost::placeholders;
   return numericalHessian212<X1, X2>(
       boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, boost::cref(x3))),
       x1, x2, delta);
@@ -932,6 +951,7 @@ template<class X1, class X2, class X3>
 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) {
+  using namespace boost::placeholders;
   return numericalHessian212<X1, X3>(
       boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, boost::cref(x2), _2)),
       x1, x3, delta);
@@ -941,6 +961,7 @@ template<class X1, class X2, class X3>
 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) {
+  using namespace boost::placeholders;
   return numericalHessian212<X2, X3>(
       boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), _1, _2)),
       x2, x3, delta);
diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp
index 1982b8f505..1db1926bcd 100644
--- a/gtsam/geometry/tests/testCalibratedCamera.cpp
+++ b/gtsam/geometry/tests/testCalibratedCamera.cpp
@@ -20,10 +20,12 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <iostream>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp
index 60cee59ee1..761ef3a8ca 100644
--- a/gtsam/geometry/tests/testCameraSet.cpp
+++ b/gtsam/geometry/tests/testCameraSet.cpp
@@ -20,7 +20,6 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
 
 using namespace std;
 using namespace gtsam;
diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp
index 86a498cdc7..7362cf7bf8 100644
--- a/gtsam/geometry/tests/testEssentialMatrix.cpp
+++ b/gtsam/geometry/tests/testEssentialMatrix.cpp
@@ -9,9 +9,12 @@
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 #include <sstream>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp
index 2461cea1af..5c7c6142e1 100644
--- a/gtsam/geometry/tests/testOrientedPlane3.cpp
+++ b/gtsam/geometry/tests/testOrientedPlane3.cpp
@@ -21,8 +21,10 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/vector.hpp>
+#include <boost/bind/bind.hpp>
 
 using namespace boost::assign;
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace std;
 using boost::none;
diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp
index ad6f4e9210..92deda6a53 100644
--- a/gtsam/geometry/tests/testPinholeCamera.cpp
+++ b/gtsam/geometry/tests/testPinholeCamera.cpp
@@ -22,11 +22,13 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <cmath>
 #include <iostream>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp
index f32fe60ed1..9ca8847f85 100644
--- a/gtsam/geometry/tests/testPinholeSet.cpp
+++ b/gtsam/geometry/tests/testPinholeSet.cpp
@@ -20,7 +20,6 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
 
 using namespace std;
 using namespace gtsam;
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index a655011a03..79e44c0b35 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -17,8 +17,11 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Point3)
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 594d15c91b..9ed76d4a67 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -22,6 +22,8 @@
 
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
+#include <boost/bind/bind.hpp>
+using namespace boost::placeholders;
 
 #include <CppUnitLite/TestHarness.h>
 #include <cmath>
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index b2c781b35f..58ad967d21 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -20,8 +20,10 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
 
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp
index 8f466e21bc..10a9b2ac49 100644
--- a/gtsam/geometry/tests/testSimilarity3.cpp
+++ b/gtsam/geometry/tests/testSimilarity3.cpp
@@ -30,9 +30,10 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/bind/bind.hpp>
 #include <boost/function.hpp>
-#include <boost/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace std;
 using symbol_shorthand::X;
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index 9929df21ae..4d609380c8 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -31,13 +31,14 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind.hpp>
 #include <boost/assign/std/vector.hpp>
+#include <boost/bind/bind.hpp>
 
 #include <cmath>
 #include <random>
 
 using namespace boost::assign;
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace std;
 using gtsam::symbol_shorthand::U;
diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h
index 3390a3aac8..b8d446e493 100644
--- a/gtsam/inference/EliminationTree-inst.h
+++ b/gtsam/inference/EliminationTree-inst.h
@@ -18,7 +18,6 @@
 #pragma once
 
 #include <boost/make_shared.hpp>
-#include <boost/bind.hpp>
 #include <stack>
 
 #include <gtsam/base/timing.h>
diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h
index e1c18274a8..166ae41f41 100644
--- a/gtsam/inference/FactorGraph-inst.h
+++ b/gtsam/inference/FactorGraph-inst.h
@@ -23,8 +23,6 @@
 
 #include <gtsam/inference/FactorGraph.h>
 
-#include <boost/bind.hpp>
-
 #include <stdio.h>
 #include <algorithm>
 #include <iostream>  // for cout :-(
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index a9ca7f84d3..e337e3249f 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -29,7 +29,6 @@
 #include <Eigen/Core>  // for Eigen::aligned_allocator
 
 #include <boost/assign/list_inserter.hpp>
-#include <boost/bind.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/vector.hpp>
diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp
index e67c56e5e2..17ff6fd22e 100644
--- a/gtsam/inference/LabeledSymbol.cpp
+++ b/gtsam/inference/LabeledSymbol.cpp
@@ -17,8 +17,8 @@
 
 #include <iostream>
 
+#include <boost/bind/bind.hpp>
 #include <boost/format.hpp>
-#include <boost/bind.hpp>
 
 #include <boost/lexical_cast.hpp>
 
@@ -110,16 +110,16 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const {
 static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);}
 
 boost::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
-  return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c;
+  return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c;
 }
 
 boost::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
-  return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
+  return boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label;
 }
 
 boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
-  return boost::bind(&LabeledSymbol::chr,   boost::bind(make, _1)) == c &&
-      boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
+  return boost::bind(&LabeledSymbol::chr,   boost::bind(make, boost::placeholders::_1)) == c &&
+      boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp
index e6a2beee01..61e397f403 100644
--- a/gtsam/inference/Symbol.cpp
+++ b/gtsam/inference/Symbol.cpp
@@ -18,8 +18,8 @@
 
 #include <gtsam/inference/Symbol.h>
 
+#include <boost/bind/bind.hpp>
 #include <boost/format.hpp>
-#include <boost/bind.hpp>
 
 #include <limits.h>
 #include <list>
@@ -63,7 +63,7 @@ Symbol::operator std::string() const {
 static Symbol make(gtsam::Key key) { return Symbol(key);}
 
 boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
-  return bind(&Symbol::chr, bind(make, _1)) == c;
+  return boost::bind(&Symbol::chr, boost::bind(make, boost::placeholders::_1)) == c;
 }
 
 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) {
diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h
index cc5dc4b881..770b48f63c 100644
--- a/gtsam/inference/inference-inst.h
+++ b/gtsam/inference/inference-inst.h
@@ -20,7 +20,6 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
-#include <boost/bind.hpp>
 #include <utility>
 
 #include <gtsam/base/treeTraversal-inst.h>
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 7bf65353b8..8646a9cae1 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -32,14 +32,6 @@
 #include <boost/format.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/tuple/tuple.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/bind.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
 #include <boost/assign/list_of.hpp>
 #include <boost/range/adaptor/transformed.hpp>
 #include <boost/range/adaptor/map.hpp>
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 2e0ffb0344..6354766874 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -35,14 +35,6 @@
 #include <boost/format.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/array.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/bind.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
 #include <boost/range/algorithm/copy.hpp>
 #include <boost/range/adaptor/indirected.hpp>
 #include <boost/range/adaptor/map.hpp>
diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index 7462758476..f72799c0aa 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -18,7 +18,7 @@
 
 #include <gtsam/linear/VectorValues.h>
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 #include <boost/range/combine.hpp>
 #include <boost/range/numeric.hpp>
 #include <boost/range/adaptor/transformed.hpp>
@@ -38,7 +38,8 @@ namespace gtsam {
   {
     // Merge using predicate for comparing first of pair
     merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()),
-      boost::bind(&less<Key>::operator(), less<Key>(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2)));
+      boost::bind(&less<Key>::operator(), less<Key>(), boost::bind(&KeyValuePair::first, boost::placeholders::_1),
+          boost::bind(&KeyValuePair::first, boost::placeholders::_2)));
     if(size() != first.size() + second.size())
       throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
   }
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index eafefb3cb6..09a741d0b2 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -26,6 +26,8 @@
 #include <boost/assign/list_of.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
+#include <boost/bind/bind.hpp>
+using namespace boost::placeholders;
 
 // STL/C++
 #include <iostream>
diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp
index 17dc6a425c..99d916123c 100644
--- a/gtsam/linear/tests/testGaussianBayesTree.cpp
+++ b/gtsam/linear/tests/testGaussianBayesTree.cpp
@@ -22,6 +22,8 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/assign/std/set.hpp> // for operator +=
 using namespace boost::assign;
+#include <boost/bind/bind.hpp>
+using namespace boost::placeholders;
 
 #include <gtsam/base/debug.h>
 #include <gtsam/base/numericalDerivative.h>
diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp
index 74acf69da2..828e264f46 100644
--- a/gtsam/navigation/tests/testAHRSFactor.cpp
+++ b/gtsam/navigation/tests/testAHRSFactor.cpp
@@ -25,9 +25,10 @@
 #include <gtsam/base/debug.h>
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 #include <list>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index 38f16f55f1..2ab60fe6ae 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -21,8 +21,11 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/serialization.h>
 #include <gtsam/base/serializationTestHelpers.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp
index 3ef810cad9..2bbc2cc7c7 100644
--- a/gtsam/navigation/tests/testCombinedImuFactor.cpp
+++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp
@@ -29,7 +29,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind.hpp>
 #include <list>
 
 #include "imuFactorTesting.h"
diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index 9457f501d0..b486272abf 100644
--- a/gtsam/navigation/tests/testGPSFactor.cpp
+++ b/gtsam/navigation/tests/testGPSFactor.cpp
@@ -20,11 +20,14 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
+
 #include <CppUnitLite/TestHarness.h>
 
 #include <GeographicLib/Config.h>
 #include <GeographicLib/LocalCartesian.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp
index 596b76f6ac..e7da2c81c6 100644
--- a/gtsam/navigation/tests/testImuBias.cpp
+++ b/gtsam/navigation/tests/testImuBias.cpp
@@ -19,8 +19,9 @@
 #include <gtsam/base/numericalDerivative.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp
index 474b00add2..f19862772c 100644
--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/gtsam/navigation/tests/testImuFactor.cpp
@@ -30,7 +30,7 @@
 #include <gtsam/base/numericalDerivative.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 #include <list>
 
 #include "imuFactorTesting.h"
@@ -147,7 +147,8 @@ TEST(ImuFactor, PreintegratedMeasurements) {
   Matrix96 aH3;
   actual.computeError(x1, x2, bias, aH1, aH2, aH3);
   boost::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
-      boost::bind(&PreintegrationBase::computeError, actual, _1, _2, _3,
+      boost::bind(&PreintegrationBase::computeError, actual,
+                  boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3,
                   boost::none, boost::none, boost::none);
   EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
   EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
@@ -203,20 +204,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
   Matrix96 actualH;
   pim.biasCorrectedDelta(kZeroBias, actualH);
   Matrix expectedH = numericalDerivative11<Vector9, Bias>(
-      boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
-          boost::none), kZeroBias);
+      boost::bind(&PreintegrationBase::biasCorrectedDelta, pim,
+          boost::placeholders::_1, boost::none), kZeroBias);
   EXPECT(assert_equal(expectedH, actualH));
 
   Matrix9 aH1;
   Matrix96 aH2;
   NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
   Matrix eH1 = numericalDerivative11<NavState, NavState>(
-      boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none,
-          boost::none), state1);
+      boost::bind(&PreintegrationBase::predict, pim, boost::placeholders::_1,
+          kZeroBias, boost::none, boost::none), state1);
   EXPECT(assert_equal(eH1, aH1));
   Matrix eH2 = numericalDerivative11<NavState, Bias>(
-      boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
-          boost::none), kZeroBias);
+      boost::bind(&PreintegrationBase::predict, pim, state1,
+          boost::placeholders::_1, boost::none, boost::none), kZeroBias);
   EXPECT(assert_equal(eH2, aH2));
 }
 
@@ -277,12 +278,12 @@ TEST(ImuFactor, ErrorAndJacobians) {
   // Make sure rotation part is correct when error is interpreted as axis-angle
   // Jacobians are around zero, so the rotation part is the same as:
   Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias),
+      boost::bind(&evaluateRotationError, factor, boost::placeholders::_1, v1, x2, v2, kZeroBias),
       x1);
   EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
 
   Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias),
+      boost::bind(&evaluateRotationError, factor, x1, v1, boost::placeholders::_1, v2, kZeroBias),
       x2);
   EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
 
@@ -332,8 +333,8 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
   Matrix96 actualH;
   pim.biasCorrectedDelta(bias, actualH);
   Matrix expectedH = numericalDerivative11<Vector9, Bias>(
-      boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
-          boost::none), bias);
+      boost::bind(&PreintegrationBase::biasCorrectedDelta, pim,
+          boost::placeholders::_1, boost::none), bias);
   EXPECT(assert_equal(expectedH, actualH));
 
   // Create factor
@@ -521,7 +522,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega,
       boost::none, D_correctedAcc_measuredOmega, boost::none);
   Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
-      boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
+      boost::bind(correctedAcc, pim, measuredAcc, boost::placeholders::_1),
+      measuredOmega, 1e-6);
   EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
 
   double dt = 0.1;
@@ -532,13 +534,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
 //      pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
 //
 //  Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
-//      boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
+//      boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
+//          boost::placeholders::_1, boost::placeholders::_2,
 //          dt, boost::none, boost::none, boost::none), measuredAcc,
 //      measuredOmega, 1e-6);
 //  EXPECT(assert_equal(expectedG1, G1, 1e-5));
 //
 //  Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
-//      boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
+//      boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
+//          boost::placeholders::_1, boost::placeholders::_2,
 //          dt, boost::none, boost::none, boost::none), measuredAcc,
 //      measuredOmega, 1e-6);
 //  EXPECT(assert_equal(expectedG2, G2, 1e-5));
diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp
index 45db58e33c..ad193b5036 100644
--- a/gtsam/navigation/tests/testMagFactor.cpp
+++ b/gtsam/navigation/tests/testMagFactor.cpp
@@ -20,10 +20,13 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
+
 #include <CppUnitLite/TestHarness.h>
 
 #include <GeographicLib/LocalCartesian.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp
index a1cc1c6eb9..1a3c5b2a97 100644
--- a/gtsam/navigation/tests/testMagPoseFactor.cpp
+++ b/gtsam/navigation/tests/testMagPoseFactor.cpp
@@ -17,6 +17,9 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/MagPoseFactor.h>
 
+#include <boost/bind/bind.hpp>
+
+using namespace boost::placeholders;
 using namespace gtsam;
 
 // *****************************************************************************
diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp
index a3f688dda8..16084ecbe2 100644
--- a/gtsam/navigation/tests/testManifoldPreintegration.cpp
+++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp
@@ -22,7 +22,7 @@
 #include <gtsam/nonlinear/expressionTesting.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
 #include "imuFactorTesting.h"
 
@@ -98,7 +98,9 @@ TEST(ManifoldPreintegration, computeError) {
   pim.computeError(x1, x2, bias, aH1, aH2, aH3);
   boost::function<Vector9(const NavState&, const NavState&,
                           const imuBias::ConstantBias&)> f =
-      boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
+      boost::bind(&ManifoldPreintegration::computeError, pim,
+                  boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3,
                   boost::none, boost::none, boost::none);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
   EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index 5bafe43923..86c708f5e8 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/gtsam/navigation/tests/testNavState.cpp
@@ -19,8 +19,11 @@
 #include <gtsam/navigation/NavState.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp
index 9c080929dd..d0bae36900 100644
--- a/gtsam/navigation/tests/testScenario.cpp
+++ b/gtsam/navigation/tests/testScenario.cpp
@@ -19,9 +19,10 @@
 #include <gtsam/navigation/Scenario.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 #include <cmath>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp
index 65fd55fa32..60a646f6ce 100644
--- a/gtsam/navigation/tests/testTangentPreintegration.cpp
+++ b/gtsam/navigation/tests/testTangentPreintegration.cpp
@@ -22,7 +22,7 @@
 #include <gtsam/nonlinear/expressionTesting.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
 #include "imuFactorTesting.h"
 
@@ -105,7 +105,9 @@ TEST(TangentPreintegration, computeError) {
   pim.computeError(x1, x2, bias, aH1, aH2, aH3);
   boost::function<Vector9(const NavState&, const NavState&,
                           const imuBias::ConstantBias&)> f =
-      boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
+      boost::bind(&TangentPreintegration::computeError, pim,
+                  boost::placeholders::_1, boost::placeholders::_2,
+                  boost::placeholders::_3,
                   boost::none, boost::none, boost::none);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
   EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h
index 0ba2ad4463..85f2f14bc3 100644
--- a/gtsam/nonlinear/Expression-inl.h
+++ b/gtsam/nonlinear/Expression-inl.h
@@ -21,6 +21,7 @@
 
 #include <gtsam/nonlinear/internal/ExpressionNode.h>
 
+#include <boost/bind/bind.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/range/adaptor/map.hpp>
 #include <boost/range/algorithm.hpp>
@@ -82,7 +83,8 @@ 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),
+        new internal::UnaryExpression<T, A>(boost::bind(method,
+                boost::placeholders::_1, boost::placeholders::_2),
             expression)) {
 }
 
@@ -95,7 +97,10 @@ Expression<T>::Expression(const Expression<A1>& expression1,
     const Expression<A2>& expression2) :
     root_(
         new internal::BinaryExpression<T, A1, A2>(
-            boost::bind(method, _1, _2, _3, _4), expression1, expression2)) {
+            boost::bind(method, boost::placeholders::_1,
+                boost::placeholders::_2, boost::placeholders::_3,
+                boost::placeholders::_4),
+            expression1, expression2)) {
 }
 
 /// Construct a binary method expression
@@ -109,8 +114,11 @@ Expression<T>::Expression(const Expression<A1>& expression1,
     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)) {
+            boost::bind(method, boost::placeholders::_1,
+                boost::placeholders::_2, boost::placeholders::_3,
+                boost::placeholders::_4, boost::placeholders::_5,
+                boost::placeholders::_6),
+            expression1, expression2, expression3)) {
 }
 
 template<typename T>
@@ -247,8 +255,10 @@ 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);
+      boost::bind(internal::apply_compose<T>(), boost::placeholders::_1,
+          boost::placeholders::_2, boost::placeholders::_3,
+          boost::placeholders::_4),
+      expression1, expression2);
 }
 
 /// Construct an array of leaves
diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h
index 9e8aa3f291..bda4575959 100644
--- a/gtsam/nonlinear/Expression.h
+++ b/gtsam/nonlinear/Expression.h
@@ -24,7 +24,6 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/VectorSpace.h>
 
-#include <boost/bind.hpp>
 #include <boost/make_shared.hpp>
 #include <map>
 
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index d7f8b0ef89..b709291794 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
 #include <limits>
 #include <iostream>
@@ -87,7 +87,8 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
    * Constructor - forces exact evaluation
    */
   NonlinearEquality(Key j, const T& feasible,
-      const CompareFunction &_compare = boost::bind(traits<T>::Equals,_1,_2,1e-9)) :
+      const CompareFunction &_compare = boost::bind(traits<T>::Equals,
+          boost::placeholders::_1,boost::placeholders::_2,1e-9)) :
       Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
           j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
       compare_(_compare) {
@@ -97,7 +98,8 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
    * Constructor - allows inexact evaluation
    */
   NonlinearEquality(Key j, const T& feasible, double error_gain,
-      const CompareFunction &_compare = boost::bind(traits<T>::Equals,_1,_2,1e-9)) :
+      const CompareFunction &_compare = boost::bind(traits<T>::Equals,
+          boost::placeholders::_1,boost::placeholders::_2,1e-9)) :
       Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
           j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
       compare_(_compare) {
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 19813adbae..eca7416c94 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -26,6 +26,8 @@
 
 #include <utility>
 
+#include <boost/bind/bind.hpp>
+
 #include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
 
 namespace gtsam {
@@ -242,7 +244,8 @@ namespace gtsam {
   template<class ValueType>
   Values::Filtered<ValueType>
   Values::filter(const boost::function<bool(Key)>& filterFcn) {
-    return Filtered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
+    return Filtered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn,
+      boost::placeholders::_1), *this);
   }
 
   /* ************************************************************************* */
@@ -255,7 +258,8 @@ namespace gtsam {
   template<class ValueType>
   Values::ConstFiltered<ValueType>
   Values::filter(const boost::function<bool(Key)>& filterFcn) const {
-    return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
+    return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>,
+      filterFcn, boost::placeholders::_1), *this);
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 9b8f7645a9..ebc9c51f67 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -25,14 +25,6 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/VectorValues.h>
 
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/bind.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
 #include <boost/iterator/transform_iterator.hpp>
 
 #include <list>
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 893d5572fc..3b447ede11 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -29,15 +29,6 @@
 #include <gtsam/inference/Key.h>
 #include <boost/iterator/transform_iterator.hpp>
 #include <boost/iterator/filter_iterator.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
-#endif
-#include <boost/bind.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
 #include <boost/ptr_container/serialize_ptr_map.hpp>
 #include <boost/shared_ptr.hpp>
 
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index aacceb2767..707d57aaef 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -29,6 +29,8 @@
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/list_of.hpp>
 using namespace boost::assign;
+#include <boost/bind/bind.hpp>
+using namespace boost::placeholders;
 #include <stdexcept>
 #include <limits>
 #include <type_traits>
diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp
index 673d4e0520..2af5825db8 100644
--- a/gtsam/sam/tests/testRangeFactor.cpp
+++ b/gtsam/sam/tests/testRangeFactor.cpp
@@ -26,8 +26,9 @@
 #include <gtsam/base/TestableAssertions.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp
index 3ff76ac5c3..05ae76faa3 100644
--- a/gtsam/sfm/tests/testTranslationFactor.cpp
+++ b/gtsam/sfm/tests/testTranslationFactor.cpp
@@ -20,8 +20,10 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/sfm/TranslationFactor.h>
 
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp
index d995bf8e7f..e99dba3bc0 100644
--- a/gtsam/slam/tests/testBetweenFactor.cpp
+++ b/gtsam/slam/tests/testBetweenFactor.cpp
@@ -9,8 +9,11 @@
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/BetweenFactor.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 using namespace gtsam::noiseModel;
diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
index 16197ab036..fb2172107c 100644
--- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
@@ -22,8 +22,11 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
index 95db611d76..0c9f5c42d5 100644
--- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
@@ -17,8 +17,10 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
index a0ef7b91d7..022dc859ea 100644
--- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp
+++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
@@ -27,9 +27,10 @@
 
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std.hpp>
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
 using namespace boost::assign;
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace std;
 
diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp
index 217ff21788..075710ae73 100644
--- a/gtsam/slam/tests/testReferenceFrameFactor.cpp
+++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp
@@ -16,7 +16,7 @@
 
 #include <iostream>
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -32,6 +32,7 @@
 
 using namespace std;
 using namespace boost;
+using namespace boost::placeholders;
 using namespace gtsam;
 
 typedef gtsam::ReferenceFrameFactor<gtsam::Point2, gtsam::Pose2> PointReferenceFrameFactor;
diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp
index caf3d31dfc..e4ecafd425 100644
--- a/gtsam/slam/tests/testRotateFactor.cpp
+++ b/gtsam/slam/tests/testRotateFactor.cpp
@@ -12,12 +12,13 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind.hpp>
 #include <boost/assign/std/vector.hpp>
+#include <boost/bind/bind.hpp>
 #include <vector>
 
 using namespace std;
 using namespace boost::assign;
+using namespace boost::placeholders;
 using namespace gtsam;
 
 static const double kDegree = M_PI / 180;
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index 0cc5e8f554..26caa6b75d 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -27,9 +27,11 @@
 #include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/map.hpp>
+#include <boost/bind/bind.hpp>
 #include <iostream>
 
 using namespace boost::assign;
+using namespace boost::placeholders;
 
 static const double rankTol = 1.0;
 // Create a noise model for the pixel error
diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp
index 4bbef6530f..9fe087c2ff 100644
--- a/gtsam/slam/tests/testTriangulationFactor.cpp
+++ b/gtsam/slam/tests/testTriangulationFactor.cpp
@@ -27,10 +27,12 @@
 
 #include <boost/assign.hpp>
 #include <boost/assign/std/vector.hpp>
+#include <boost/bind/bind.hpp>
 
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
+using namespace boost::placeholders;
 
 // Some common constants
 static const boost::shared_ptr<Cal3_S2> sharedCal = //
diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h
index 531d54af16..337f2bc430 100644
--- a/gtsam_unstable/dynamics/FullIMUFactor.h
+++ b/gtsam_unstable/dynamics/FullIMUFactor.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <boost/bind/bind.hpp>
+
 namespace gtsam {
 
 /**
@@ -89,9 +91,9 @@ class FullIMUFactor : public NoiseModelFactor2<POSE, POSE> {
     z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
     z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang
     if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
-        boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
+        boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5);
     if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
-        boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
+        boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5);
     return z - predict_proxy(x1, x2, dt_);
   }
 
diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h
index 62a6abcd9f..e082dee82b 100644
--- a/gtsam_unstable/dynamics/IMUFactor.h
+++ b/gtsam_unstable/dynamics/IMUFactor.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <boost/bind/bind.hpp>
+
 namespace gtsam {
 
 /**
@@ -79,9 +81,9 @@ class IMUFactor : public NoiseModelFactor2<POSE, POSE> {
       boost::optional<Matrix&> H2 = boost::none) const override {
     const Vector6 meas = z();
     if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
-        boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
+        boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5);
     if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
-        boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
+        boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5);
     return predict_proxy(x1, x2, dt_, meas);
   }
 
diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h
index 56171a7287..840b7bba72 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <boost/bind/bind.hpp>
+
 namespace gtsam {
 
 namespace dynamics {
@@ -84,9 +86,11 @@ class VelocityConstraint : public gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> {
       boost::optional<gtsam::Matrix&> H1=boost::none,
       boost::optional<gtsam::Matrix&> H2=boost::none) const override {
     if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
-        boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
+        boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1,
+            boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);
     if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
-        boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
+        boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1,
+            boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);
     return evaluateError_(x1, x2, dt_, integration_mode_);
   }
 
diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
index fe21d5e00b..ede11c7fb5 100644
--- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
+++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
@@ -3,12 +3,14 @@
  * @author Duy-Nguyen Ta
  */
 
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam_unstable/dynamics/SimpleHelicopter.h>
 
 /* ************************************************************************* */
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 
diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp
index f72b2cf952..8d496a30ec 100644
--- a/gtsam_unstable/examples/TimeOfArrivalExample.cpp
+++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp
@@ -24,7 +24,6 @@
 #include <gtsam_unstable/geometry/Event.h>
 #include <gtsam_unstable/slam/TOAFactor.h>
 
-#include <boost/bind.hpp>
 #include <boost/format.hpp>
 
 #include <vector>
diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp
index 0349f32930..3a599e6c5f 100644
--- a/gtsam_unstable/geometry/tests/testEvent.cpp
+++ b/gtsam_unstable/geometry/tests/testEvent.cpp
@@ -23,8 +23,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp
index df21c0132b..88697e075d 100644
--- a/gtsam_unstable/linear/QPSParser.cpp
+++ b/gtsam_unstable/linear/QPSParser.cpp
@@ -24,6 +24,7 @@
 #include <gtsam_unstable/linear/QPSParser.h>
 #include <gtsam_unstable/linear/QPSParserException.h>
 
+#include <boost/bind/bind.hpp>
 #include <boost/fusion/include/vector.hpp>
 #include <boost/fusion/sequence.hpp>
 #include <boost/lambda/lambda.hpp>
@@ -39,6 +40,7 @@
 #include <vector>
 
 using boost::fusion::at_c;
+using namespace boost::placeholders;
 using namespace std;
 
 namespace bf = boost::fusion;
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
index 165135114d..c7b82ba985 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -27,6 +27,7 @@
 // Using numerical derivative to calculate d(Pose3::Expmap)/dw
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
 #include <boost/optional.hpp>
 
 #include <ostream>
@@ -304,6 +305,8 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
       boost::optional<Matrix&> H4 = boost::none,
       boost::optional<Matrix&> H5 = boost::none) const override {
 
+    using namespace boost::placeholders;
+
     // TODO: Write analytical derivative calculations
     // Jacobian w.r.t. Pose1
     if (H1){
@@ -420,6 +423,8 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     // Note: all delta terms refer to an IMU\sensor system at t0
     // Note: Earth-related terms are not accounted here but are incorporated in predict functions.
 
+    using namespace boost::placeholders;
+
     POSE body_P_sensor = POSE();
     bool flag_use_body_P_sensor = false;
     if (p_body_P_sensor){
diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
index f031f4884a..968810e0a3 100644
--- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
+++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
@@ -26,6 +26,7 @@
 // Using numerical derivative to calculate d(Pose3::Expmap)/dw
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
 #include <boost/optional.hpp>
 
 #include <ostream>
@@ -234,38 +235,38 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
     // 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, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
-      Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, _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, _1, Bias1, Pose2, Vel2), Vel1);
-      Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _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, _1, Pose2, Vel2), Bias1);
-      Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, _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, _1, Vel2), Pose2);
-      Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, _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, _1), Vel2);
-      Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _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 7a34ab1bc8..7292d702b0 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant1.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h
@@ -17,6 +17,8 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
+
 namespace gtsam {
 
 /**
@@ -106,13 +108,13 @@ class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
-          boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1,
+          boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, boost::placeholders::_1,
               landmark), pose);
     }
     if (H2) {
       (*H2) = numericalDerivative11<Vector, Vector6>(
           boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
-              _1), landmark);
+              boost::placeholders::_1), landmark);
     }
 
     return inverseDepthError(pose, landmark);
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h
index 0462aefad5..d1fc92b90e 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant2.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h
@@ -18,6 +18,8 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
+
 namespace gtsam {
 
 /**
@@ -109,13 +111,13 @@ class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
-          boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1,
-              landmark), pose);
+          boost::bind(&InvDepthFactorVariant2::inverseDepthError, this,
+              boost::placeholders::_1, landmark), pose);
     }
     if (H2) {
       (*H2) = numericalDerivative11<Vector, Vector3>(
           boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
-              _1), landmark);
+              boost::placeholders::_1), landmark);
     }
 
     return inverseDepthError(pose, landmark);
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h
index 454358a0e3..d8c790342c 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant3.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h
@@ -17,6 +17,8 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
+
 namespace gtsam {
 
 /**
@@ -108,10 +110,10 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
       boost::optional<Matrix&> H2=boost::none) const override {
 
     if(H1) {
-      (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _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, _1), landmark);
+      (*H2) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, boost::placeholders::_1), landmark);
     }
 
     return inverseDepthError(pose, landmark);
@@ -229,13 +231,13 @@ class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
       boost::optional<Matrix&> H3=boost::none) const override {
 
     if(H1)
-      (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _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, _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, _1), landmark);
+      (*H3) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, boost::placeholders::_1), landmark);
 
     return inverseDepthError(pose1, pose2, landmark);
   }
diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
index e4112f53d8..0eb8b274b3 100644
--- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
+++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
@@ -9,8 +9,11 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam_unstable/slam/BiasedGPSFactor.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 using namespace gtsam::noiseModel;
diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
index 35bf5790e7..95b9b2f88f 100644
--- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
+++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
@@ -21,9 +21,12 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/numericalDerivative.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 #include <iostream>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
index 2093266723..74134612d8 100644
--- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
+++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
@@ -23,6 +23,9 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/deprecated/LieVector.h>
 
+#include <boost/bind/bind.hpp>
+
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
index b84f7e0807..aae59035be 100644
--- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
+++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
@@ -16,6 +16,7 @@
  */
 
 #include <iostream>
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
@@ -25,6 +26,7 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
index 9f6fe5b50d..6bbfb55ae1 100644
--- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
+++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
@@ -24,8 +24,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace std;
 
diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
index ae8074f432..2b99b8779a 100644
--- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
@@ -15,8 +15,11 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
index f589e932fb..6f7725eed1 100644
--- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
@@ -21,8 +21,11 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
index 166f058e3a..cc26155172 100644
--- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
@@ -21,8 +21,11 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
index 4e9fdcb50d..8a65e5e571 100644
--- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
+++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
@@ -28,8 +28,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
index 5eadf5fd69..232f8de17e 100644
--- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
+++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
@@ -28,8 +28,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
index 210018ed33..2fda9debb8 100644
--- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
+++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
@@ -5,12 +5,14 @@
  * @author Alex Cunningham
  */
 
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam_unstable/slam/RelativeElevationFactor.h>
 
 #include <gtsam/base/numericalDerivative.h>
 
+using namespace boost::placeholders;
 using namespace gtsam;
 
 SharedNoiseModel model1 = noiseModel::Unit::Create(1);
diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp
index 042130a249..bc5c553e0e 100644
--- a/gtsam_unstable/slam/tests/testTOAFactor.cpp
+++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp
@@ -25,7 +25,6 @@
 #include <gtsam_unstable/slam/TOAFactor.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind.hpp>
 #include <boost/format.hpp>
 
 using namespace std;
diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp
index 098a0ef565..77f82bca49 100644
--- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp
+++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp
@@ -18,8 +18,11 @@
 
 #include <gtsam_unstable/slam/TSAMFactors.h>
 #include <gtsam/base/numericalDerivative.h>
+
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
index 01d4b152d8..d9e945b78c 100644
--- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
+++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
@@ -18,7 +18,9 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 
+#include <boost/bind/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
index b200a3e583..2fd282091a 100644
--- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
+++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
@@ -18,7 +18,9 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 
+#include <boost/bind/bind.hpp>
 
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp
index b0978feb9c..ec41bf6786 100644
--- a/tests/testDoglegOptimizer.cpp
+++ b/tests/testDoglegOptimizer.cpp
@@ -29,14 +29,6 @@
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/bind.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
 #include <boost/assign/list_of.hpp> // for 'list_of()'
 #include <functional>
 #include <boost/iterator/counting_iterator.hpp>
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index c31baeadfe..6bb5751bf6 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -31,6 +31,8 @@
 
 #include <boost/assign/list_of.hpp>
 using boost::assign::list_of;
+#include <boost/bind/bind.hpp>
+using namespace boost::placeholders;
 
 using namespace std;
 using namespace gtsam;
diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp
index 02f6ed7629..342c353bca 100644
--- a/tests/testSimulated3D.cpp
+++ b/tests/testSimulated3D.cpp
@@ -21,10 +21,12 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <iostream>
 
+using namespace boost::placeholders;
 using namespace gtsam;
 
 // Convenience for named keys

From 6dea8667fdd8dce8518b552486a7b889907a8255 Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Fri, 18 Jun 2021 13:45:59 -0400
Subject: [PATCH 04/72] explicitly use boost::placeholders:_X for compilers
 that do not respect function scope

---
 gtsam/base/numericalDerivative.h              | 94 +++++++------------
 .../slam/EquivInertialNavFactor_GlobalVel.h   | 34 +++----
 2 files changed, 50 insertions(+), 78 deletions(-)

diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index a05a1dda82..29081efba7 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -176,8 +176,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
+  return numericalDerivative21<Y, X1, X2>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta);
 }
 
 /**
@@ -203,8 +202,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::func
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
+  return numericalDerivative22<Y, X1, X2>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta);
 }
 
 /**
@@ -232,8 +230,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
+  return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
       x2, x3, delta);
 }
 
@@ -262,8 +259,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
+  return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
       x2, x3, delta);
 }
 
@@ -292,8 +288,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
+  return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
       x2, x3, delta);
 }
 
@@ -322,8 +317,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _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);
 }
 
 /**
@@ -351,8 +345,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _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);
 }
 
 /**
@@ -380,8 +373,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _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);
 }
 
 /**
@@ -409,8 +401,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _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);
 }
 
 /**
@@ -439,8 +430,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _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);
 }
 
 /**
@@ -469,8 +459,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _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);
 }
 
 /**
@@ -499,8 +488,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _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);
 }
 
 /**
@@ -529,8 +517,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _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);
 }
 
 /**
@@ -559,8 +546,7 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _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);
 }
 
 /**
@@ -590,8 +576,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _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);
 }
 
 /**
@@ -621,8 +606,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _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);
 }
 
 /**
@@ -652,8 +636,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _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);
 }
 
 /**
@@ -683,8 +666,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative64<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5, _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);
 }
 
 /**
@@ -714,8 +696,7 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _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);
 }
 
 /**
@@ -746,8 +727,7 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
 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) {
-  using namespace boost::placeholders;
-  return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _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);
 }
 
 /**
@@ -820,15 +800,14 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
     boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
 
-  using namespace boost::placeholders;
   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, _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, _1, delta)),
+      boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x1, delta);
 }
 
@@ -843,14 +822,13 @@ template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
     boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
-  using namespace boost::placeholders;
   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), _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, _1, delta)),
+      boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x2, delta);
 }
 
@@ -869,14 +847,13 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
-  using namespace boost::placeholders;
   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, _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, _1, delta)),
+      boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x1, delta);
 }
 
@@ -893,14 +870,13 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
-  using namespace boost::placeholders;
   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), _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, _1, delta)),
+      boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x2, delta);
 }
 
@@ -917,14 +893,13 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
-  using namespace boost::placeholders;
   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), _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, _1, delta)),
+      boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
       x3, delta);
 }
 
@@ -941,9 +916,8 @@ template<class X1, class X2, class X3>
 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) {
-  using namespace boost::placeholders;
   return numericalHessian212<X1, X2>(
-      boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _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);
 }
 
@@ -951,9 +925,8 @@ template<class X1, class X2, class X3>
 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) {
-  using namespace boost::placeholders;
   return numericalHessian212<X1, X3>(
-      boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, boost::cref(x2), _2)),
+      boost::function<double(const X1&, const X3&)>(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::placeholders::_2)),
       x1, x3, delta);
 }
 
@@ -961,9 +934,8 @@ template<class X1, class X2, class X3>
 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) {
-  using namespace boost::placeholders;
   return numericalHessian212<X2, X3>(
-      boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), _1, _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 c7b82ba985..3e6dbf6db3 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -310,38 +310,38 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     // TODO: Write analytical derivative calculations
     // Jacobian w.r.t. Pose1
     if (H1){
-      Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
-      Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _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, _1, Bias1, Pose2, Vel2), Vel1);
-      Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _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, _1, Pose2, Vel2), Bias1);
-      Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _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, _1, Vel2), Pose2);
-      Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _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, _1), Vel2);
-      Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _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);
     }
 
@@ -442,18 +442,18 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     Matrix Z_3x3 = Z_3x3;
     Matrix I_3x3 = I_3x3;
 
-    Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _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, _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, _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, _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, _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, _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, _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;
 

From 7aeb386dbd50f86f1bb7f7491baed334ecf4ec4b Mon Sep 17 00:00:00 2001
From: Akash Patel <17132214+acxz@users.noreply.github.com>
Date: Sun, 20 Jun 2021 18:04:28 -0400
Subject: [PATCH 05/72] formatting

remove extraneous `using`
---
 gtsam/nonlinear/NonlinearEquality.h                    | 5 ++---
 gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h | 4 ----
 2 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index b709291794..f10ba93aee 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -88,7 +88,7 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
    */
   NonlinearEquality(Key j, const T& feasible,
       const CompareFunction &_compare = boost::bind(traits<T>::Equals,
-          boost::placeholders::_1,boost::placeholders::_2,1e-9)) :
+          boost::placeholders::_1, boost::placeholders::_2, 1e-9)) :
       Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
           j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
       compare_(_compare) {
@@ -99,7 +99,7 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
    */
   NonlinearEquality(Key j, const T& feasible, double error_gain,
       const CompareFunction &_compare = boost::bind(traits<T>::Equals,
-          boost::placeholders::_1,boost::placeholders::_2,1e-9)) :
+          boost::placeholders::_1, boost::placeholders::_2, 1e-9)) :
       Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
           j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
       compare_(_compare) {
@@ -361,4 +361,3 @@ struct traits<NonlinearEquality2<VALUE> > : Testable<NonlinearEquality2<VALUE> >
 
 
 }// namespace gtsam
-
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
index 3e6dbf6db3..6a345a6b5d 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -305,8 +305,6 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
       boost::optional<Matrix&> H4 = boost::none,
       boost::optional<Matrix&> H5 = boost::none) const override {
 
-    using namespace boost::placeholders;
-
     // TODO: Write analytical derivative calculations
     // Jacobian w.r.t. Pose1
     if (H1){
@@ -423,8 +421,6 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     // Note: all delta terms refer to an IMU\sensor system at t0
     // Note: Earth-related terms are not accounted here but are incorporated in predict functions.
 
-    using namespace boost::placeholders;
-
     POSE body_P_sensor = POSE();
     bool flag_use_body_P_sensor = false;
     if (p_body_P_sensor){

From 42c0eb6aadb1057b198af35657c979ccd7365c42 Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Sun, 20 Jun 2021 18:31:07 -0400
Subject: [PATCH 06/72] formatting

---
 gtsam/linear/tests/testGaussianBayesNet.cpp  | 4 ++--
 gtsam/linear/tests/testGaussianBayesTree.cpp | 4 ++--
 gtsam/nonlinear/tests/testValues.cpp         | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 09a741d0b2..c88bf87314 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -25,14 +25,14 @@
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/list_of.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
-using namespace boost::assign;
 #include <boost/bind/bind.hpp>
-using namespace boost::placeholders;
 
 // STL/C++
 #include <iostream>
 #include <sstream>
 
+using namespace boost::assign;
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp
index 99d916123c..a6a60c19ce 100644
--- a/gtsam/linear/tests/testGaussianBayesTree.cpp
+++ b/gtsam/linear/tests/testGaussianBayesTree.cpp
@@ -21,9 +21,7 @@
 #include <boost/assign/list_of.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/assign/std/set.hpp> // for operator +=
-using namespace boost::assign;
 #include <boost/bind/bind.hpp>
-using namespace boost::placeholders;
 
 #include <gtsam/base/debug.h>
 #include <gtsam/base/numericalDerivative.h>
@@ -31,6 +29,8 @@ using namespace boost::placeholders;
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/linear/GaussianConditional.h>
 
+using namespace boost::assign;
+using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 707d57aaef..e9076a7d7a 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -28,13 +28,13 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/list_of.hpp>
-using namespace boost::assign;
 #include <boost/bind/bind.hpp>
-using namespace boost::placeholders;
 #include <stdexcept>
 #include <limits>
 #include <type_traits>
 
+using namespace boost::assign;
+using namespace boost::placeholders;
 using namespace gtsam;
 using namespace std;
 static double inf = std::numeric_limits<double>::infinity();

From 155fafabf2faa9b3be5336a554b54e6071799b1d Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Sun, 20 Jun 2021 18:38:52 -0400
Subject: [PATCH 07/72] using using for boost placeholders in tests

---
 gtsam/navigation/tests/testManifoldPreintegration.cpp | 6 +++---
 gtsam/navigation/tests/testTangentPreintegration.cpp  | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp
index 16084ecbe2..625689ed79 100644
--- a/gtsam/navigation/tests/testManifoldPreintegration.cpp
+++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp
@@ -26,6 +26,8 @@
 
 #include "imuFactorTesting.h"
 
+using namespace boost::placeholders;
+
 namespace testing {
 // Create default parameters with Z-down and above noise parameters
 static boost::shared_ptr<PreintegrationParams> Params() {
@@ -98,9 +100,7 @@ TEST(ManifoldPreintegration, computeError) {
   pim.computeError(x1, x2, bias, aH1, aH2, aH3);
   boost::function<Vector9(const NavState&, const NavState&,
                           const imuBias::ConstantBias&)> f =
-      boost::bind(&ManifoldPreintegration::computeError, pim,
-                  boost::placeholders::_1, boost::placeholders::_2,
-                  boost::placeholders::_3,
+      boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
                   boost::none, boost::none, boost::none);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
   EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp
index 60a646f6ce..9bb988b429 100644
--- a/gtsam/navigation/tests/testTangentPreintegration.cpp
+++ b/gtsam/navigation/tests/testTangentPreintegration.cpp
@@ -26,6 +26,8 @@
 
 #include "imuFactorTesting.h"
 
+using namespace boost::placeholders;
+
 static const double kDt = 0.1;
 
 Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
@@ -105,9 +107,7 @@ TEST(TangentPreintegration, computeError) {
   pim.computeError(x1, x2, bias, aH1, aH2, aH3);
   boost::function<Vector9(const NavState&, const NavState&,
                           const imuBias::ConstantBias&)> f =
-      boost::bind(&TangentPreintegration::computeError, pim,
-                  boost::placeholders::_1, boost::placeholders::_2,
-                  boost::placeholders::_3,
+      boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
                   boost::none, boost::none, boost::none);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
   EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));

From 944b3aea297a3f0dc7b9508feb52e758725df692 Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Sun, 20 Jun 2021 19:29:27 -0400
Subject: [PATCH 08/72] formatting

---
 gtsam/base/numericalDerivative.h              | 260 ++++++++++++++----
 .../slam/EquivInertialNavFactor_GlobalVel.h   |  91 ++++--
 .../slam/InertialNavFactor_GlobalVelocity.h   |  50 +++-
 gtsam_unstable/slam/InvDepthFactorVariant1.h  |   5 +-
 gtsam_unstable/slam/InvDepthFactorVariant3.h  |  25 +-
 5 files changed, 336 insertions(+), 95 deletions(-)

diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index 29081efba7..c624c900eb 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 @@ class G_x1 {
       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 6a345a6b5d..a6638c1d71 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -308,38 +308,68 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     // 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 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     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 968810e0a3..3f526e934f 100644
--- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
+++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
@@ -235,38 +235,68 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
     // 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 7292d702b0..a9dcb8ceff 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant1.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h
@@ -108,8 +108,9 @@ class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
 
     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 d8c790342c..4595a934b1 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant3.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h
@@ -110,10 +110,16 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
       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 @@ class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
       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);
   }

From 9967c59ed0c9a1112bbffa7d84908154439e22cf Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Thu, 8 Jul 2021 14:30:16 +0200
Subject: [PATCH 09/72] Forward declaration for Set of Fisheye Cameras

Forward declaration of camera vector for PinholeCamera<Cal3Fisheye> and PinholeCamera<Cal3Unified>.
---
 gtsam/geometry/triangulation.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 1df9efd22e..b18c55818d 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -20,6 +20,8 @@
 
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
@@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 // Vector of Cameras - used by the Python/MATLAB wrapper
 using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
+using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
+using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 
 } // \namespace gtsam
 

From dfd50f98c26791e68ab670c1768605fba9f20209 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Thu, 8 Jul 2021 15:13:29 +0200
Subject: [PATCH 10/72] Extend python wrapper to include fisheye models.

Extend python wrapper to include fisheye camera models Cal3Fisheye and Cal3Unified.
---
 gtsam/gtsam.i | 102 ++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 90 insertions(+), 12 deletions(-)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 94d10953bb..327574bf89 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -231,7 +231,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };
@@ -977,6 +977,52 @@ class Cal3Bundler {
   void pickle() const;
 };
 
+#include <gtsam/geometry/Cal3Fisheye.h>
+class Cal3Fisheye {
+  // Standard Constructors
+  Cal3Fisheye();
+  Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
+  Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4, double tol);
+  Cal3Fisheye(Vector v);
+
+  // Testable
+  void print(string s = "Cal3Fisheye") const;
+  bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3Fisheye retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Fisheye& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double k1() const;
+  double k2() const;
+  double k3() const;
+  double k4() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  Vector vector() const;
+  Vector k() const;
+  Matrix K() const;
+  Matrix inverse() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+
 #include <gtsam/geometry/CalibratedCamera.h>
 class CalibratedCamera {
   // Standard Constructors and Named Constructors
@@ -1085,6 +1131,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
 typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
 
 template<T>
 class CameraSet {
@@ -1145,7 +1192,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
     const gtsam::Point2Vector& measurements, double rank_tol,
     bool optimize);
-    
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
+    const gtsam::Point2Vector& measurements, double rank_tol,
+    bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
+    const gtsam::Point2Vector& measurements, double rank_tol,
+    bool optimize);
+
 //*************************************************************************
 // Symbolic
 //*************************************************************************
@@ -2118,8 +2171,11 @@ class NonlinearFactorGraph {
   template <T = {double, Vector, gtsam::Point2, gtsam::StereoPoint2,
                  gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4,
                  gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,
+                 gtsam::Cal3Fisheye, gtsam::Cal3Unified,
                  gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2,
-                 gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                 gtsam::PinholeCameraCal3Bundler,
+                 gtsam::PinholeCameraCal3Fisheye,
+                 gtsam::PinholeCameraCal3Unified,
                  gtsam::imuBias::ConstantBias}>
   void addPrior(size_t key, const T& prior,
                 const gtsam::noiseModel::Base* noiseModel);
@@ -2252,9 +2308,13 @@ class Values {
   void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
   void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
+  void insert(size_t j, const gtsam::Cal3Unified& cal3unified);
   void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
   void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
-  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
+  void insert(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera);
+  void insert(size_t j, const gtsam::PinholeCameraCal3Unified& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
@@ -2272,9 +2332,13 @@ class Values {
   void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
   void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
+  void update(size_t j, const gtsam::Cal3Unified& cal3unified);
   void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
   void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
-  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
+  void update(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera);
+  void update(size_t j, const gtsam::PinholeCameraCal3Unified& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
@@ -2294,10 +2358,14 @@ class Values {
                  gtsam::Cal3_S2,
                  gtsam::Cal3DS2,
                  gtsam::Cal3Bundler,
-                 gtsam::EssentialMatrix,
+                 gtsam::Cal3Fisheye,
+                 gtsam::Cal3Unified,
+		 gtsam::EssentialMatrix,
                  gtsam::PinholeCameraCal3_S2,
-                 gtsam::PinholeCamera<gtsam::Cal3Bundler>,
-                 gtsam::imuBias::ConstantBias,
+                 gtsam::PinholeCameraCal3Bundler,
+                 gtsam::PinholeCameraCal3Fisheye,
+                 gtsam::PinholeCameraCal3Unified,
+		 gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  Vector,
                  Matrix,
@@ -2603,7 +2671,9 @@ class ISAM2 {
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Cal3Bundler, gtsam::EssentialMatrix,
-                     gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, 
+                     gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler,
+                     gtsam::Cal3Fisheye, gtsam::Cal3Unified,
+                     gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified,
                      Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
   gtsam::Values calculateBestEstimate() const;
@@ -2655,10 +2725,14 @@ template <T = {double,
                gtsam::Cal3_S2,
                gtsam::Cal3DS2,
                gtsam::Cal3Bundler,
+               gtsam::Cal3Fisheye,
+               gtsam::Cal3Unified,
                gtsam::CalibratedCamera,
                gtsam::PinholeCameraCal3_S2,
                gtsam::imuBias::ConstantBias,
-               gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
+               gtsam::PinholeCameraCal3Bundler,
+               gtsam::PinholeCameraCal3Fisheye,
+               gtsam::PinholeCameraCal3Unified}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2800,6 +2874,8 @@ virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
 };
 typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
 typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
+typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Fisheye> GenericProjectionFactorCal3Fisheye;
+typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Unified> GenericProjectionFactorCal3Unified;
 
 
 #include <gtsam/slam/GeneralSFMFactor.h>
@@ -2810,9 +2886,11 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
 };
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
-typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Bundler, gtsam::Point3> GeneralSFMFactorCal3Bundler;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Fisheye, gtsam::Point3> GeneralSFMFactorCal3Fisheye;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Unified, gtsam::Point3> GeneralSFMFactorCal3Unified;
 
-template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
+template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
   gtsam::Point2 measured() const;

From 55c12743fc29191c0e3d63c12b7c52f8284c402f Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Thu, 8 Jul 2021 15:45:11 +0200
Subject: [PATCH 11/72] Forward declaration of fisheye camera.

Forward declaration of PinholeCal3Fisheye needed by Python wrapper.
---
 gtsam/geometry/SimpleCamera.h | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index aa00222c77..2c34bdfa73 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -22,6 +22,7 @@
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
 
@@ -33,7 +34,8 @@ namespace gtsam {
   using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
   using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
   using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
-
+  using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
+  
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /**
  * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x

From c8fc3cd216020487be3850d58b56eca9ff74ec99 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Thu, 8 Jul 2021 19:53:41 +0200
Subject: [PATCH 12/72] Unit test for equidistant fisheye

---
 python/gtsam/tests/test_Cal3Fisheye.py | 105 +++++++++++++++++++++++++
 1 file changed, 105 insertions(+)
 create mode 100644 python/gtsam/tests/test_Cal3Fisheye.py

diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py
new file mode 100644
index 0000000000..18c8ecd80e
--- /dev/null
+++ b/python/gtsam/tests/test_Cal3Fisheye.py
@@ -0,0 +1,105 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Cal3Unified unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestCal3Fisheye(GtsamTestCase):
+
+    def test_Cal3Fisheye(self):
+        K = gtsam.Cal3Fisheye()
+        self.assertEqual(K.fx(), 1.)
+        self.assertEqual(K.fy(), 1.)
+
+    def test_distortion(self):
+        "Equidistant fisheye model of focal length f, defined as r/f = tan(theta)"
+        equidistant = gtsam.Cal3Fisheye()
+        x, y, z = 1, 0, 1
+        u, v = equidistant.uncalibrate([x, y])
+        x2, y2 = equidistant.calibrate([u, v])
+        self.assertAlmostEqual(u, np.arctan2(x, z))
+        self.assertAlmostEqual(v, 0)
+        self.assertAlmostEqual(x2, x)
+        self.assertAlmostEqual(y2, 0)
+
+    def test_pinhole(self):
+        "Spatial equidistant camera projection"
+        x, y, z = 1.0, 0.0, 1.0
+        u, v = np.arctan2(x, z), 0.0
+        camera = gtsam.PinholeCameraCal3Fisheye()
+
+        pt1 = camera.Project([x, y, z])
+        self.gtsamAssertEquals(pt1, np.array([x/z, y/z]))
+
+        pt2 = camera.project([x, y, z])
+        self.gtsamAssertEquals(pt2, np.array([u, v]))
+
+        obj1 = camera.backproject([u, v], z)
+        self.gtsamAssertEquals(obj1, np.array([x, y, z]))
+
+        r1 = camera.range(np.array([x, y, z]))
+        self.assertEqual(r1, np.linalg.norm([x, y, z]))
+
+    def test_generic_factor(self):
+        "Evaluate residual using pose and point as state variables"
+        objPoint = np.array([1, 0, 1])
+        imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0])
+        graph = gtsam.NonlinearFactorGraph()
+        state = gtsam.Values()
+        measured = imgPoint
+        noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1)
+        poseKey = gtsam.symbol_shorthand.P(0)
+        pointKey = gtsam.symbol_shorthand.L(0)
+        k = gtsam.Cal3Fisheye()
+        state.insert_pose3(poseKey, gtsam.Pose3())
+        state.insert_point3(pointKey, gtsam.Point3(objPoint))
+        factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k)
+        graph.add(factor)
+        score = graph.error(state)
+        self.assertAlmostEqual(score, 0)
+
+    def test_sfm_factor2(self):
+        "Evaluate residual with camera, pose and point as state variables"
+        objPoint = np.array([1, 0, 1])
+        imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0])
+        graph = gtsam.NonlinearFactorGraph()
+        state = gtsam.Values()
+        measured = imgPoint
+        noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1)
+        cameraKey = gtsam.symbol_shorthand.K(0)
+        poseKey = gtsam.symbol_shorthand.P(0)
+        landmarkKey = gtsam.symbol_shorthand.L(0)
+        k = gtsam.Cal3Fisheye()
+        state.insert_cal3fisheye(cameraKey, k)
+        state.insert_pose3(poseKey, gtsam.Pose3())
+        state.insert_point3(landmarkKey, gtsam.Point3(objPoint))
+        factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey)
+        graph.add(factor)
+        score = graph.error(state)
+        self.assertAlmostEqual(score, 0)
+
+    def test_retract(self):
+        expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
+                                     1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10)
+        K = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240,
+                              1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3)
+        d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F')
+        actual = K.retract(d)
+        self.gtsamAssertEquals(actual, expected)
+        np.testing.assert_allclose(d, K.localCoordinates(actual))
+
+
+if __name__ == "__main__":
+    unittest.main()

From 19e8cde733c40e2e6dc0beb3c313c55b34e354fa Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Thu, 8 Jul 2021 19:59:56 +0200
Subject: [PATCH 13/72] Extend unit testing of omnidirectional projection

Test projection function and factors using a stereoscopic (xi=1) reference model, i.e the image height is given by y = 2 f tan(theta/2).
---
 python/gtsam/tests/test_Cal3Unified.py | 83 ++++++++++++++++++++++++++
 1 file changed, 83 insertions(+)

diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index fbf5f3565c..ff9d659607 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -23,6 +23,89 @@ def test_Cal3Unified(self):
         self.assertEqual(K.fx(), 1.)
         self.assertEqual(K.fx(), 1.)
 
+    def test_distortion(self):
+        "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)"
+        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
+        k1, k2, p1, p2 = 0, 0, 0, 0
+        xi = 1
+        stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
+        x, y, z = 1, 0, 1
+        u, v = stereographic.uncalibrate([x, y])
+        r = np.linalg.norm([x, y, z])
+        # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2))
+        self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r))
+        self.assertAlmostEqual(u, 2/(1+r))
+        x2, y2 = stereographic.calibrate([u, v])
+        self.assertAlmostEqual(x2, x)
+
+    def test_pinhole(self):
+        "Spatial stereographic camera projection"
+        x, y, z = 1.0, 0.0, 1.0
+        r = np.linalg.norm([x, y, z])
+        u, v = 2/(1+r), 0.0
+        objPoint = np.array([x, y, z])
+        imgPoint = np.array([u, v])
+        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
+        k1, k2, p1, p2 = 0, 0, 0, 0
+        xi = 1
+        stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
+        pose = gtsam.Pose3()
+        camera = gtsam.PinholeCameraCal3Unified(pose, stereographic)
+        pt1 = camera.Project(objPoint)
+        self.gtsamAssertEquals(pt1, np.array([x/z, y/z]))
+        pt2 = camera.project(objPoint)
+        self.gtsamAssertEquals(pt2, np.array([u, v]))
+        obj1 = camera.backproject([u, v], z)
+        self.gtsamAssertEquals(obj1, np.array([x, y, z]))
+        r1 = camera.range(np.array([x, y, z]))
+        self.assertEqual(r1, r)
+
+    def test_generic_factor(self):
+        "Evaluate residual using pose and point as state variables"
+        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
+        k1, k2, p1, p2 = 0, 0, 0, 0
+        xi = 1
+        objPoint = np.array([1, 0, 1])
+        r = np.linalg.norm(objPoint)
+        imgPoint = np.array([2/(1+r), 0])
+        graph = gtsam.NonlinearFactorGraph()
+        state = gtsam.Values()
+        measured = imgPoint
+        noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1)
+        poseKey = gtsam.symbol_shorthand.P(0)
+        pointKey = gtsam.symbol_shorthand.L(0)
+        k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
+        state.insert_pose3(poseKey, gtsam.Pose3())
+        state.insert_point3(pointKey, gtsam.Point3(objPoint))
+        factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k)
+        graph.add(factor)
+        score = graph.error(state)
+        self.assertAlmostEqual(score, 0)
+
+    def test_sfm_factor2(self):
+        "Evaluate residual with camera, pose and point as state variables"
+        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
+        k1, k2, p1, p2 = 0, 0, 0, 0
+        xi = 1
+        objPoint = np.array([1, 0, 1])
+        r = np.linalg.norm(objPoint)
+        imgPoint = np.array([2/(1+r), 0])
+        graph = gtsam.NonlinearFactorGraph()
+        state = gtsam.Values()
+        measured = imgPoint
+        noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1)
+        cameraKey = gtsam.symbol_shorthand.K(0)
+        poseKey = gtsam.symbol_shorthand.P(0)
+        landmarkKey = gtsam.symbol_shorthand.L(0)
+        k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
+        state.insert_cal3unified(cameraKey, k)
+        state.insert_pose3(poseKey, gtsam.Pose3())
+        state.insert_point3(landmarkKey, gtsam.Point3(objPoint))
+        factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey)
+        graph.add(factor)
+        score = graph.error(state)
+        self.assertAlmostEqual(score, 0)
+
     def test_retract(self):
         expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
                                      1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)

From 2ecad47b9ef092379926f9784257c55dd245c24b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 8 Jul 2021 19:41:01 -0400
Subject: [PATCH 14/72] Added lots of tests for BetweenFactor

---
 gtsam/slam/tests/testBetweenFactor.cpp | 59 ++++++++++++++++++++++----
 1 file changed, 51 insertions(+), 8 deletions(-)

diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp
index e99dba3bc0..31842bf800 100644
--- a/gtsam/slam/tests/testBetweenFactor.cpp
+++ b/gtsam/slam/tests/testBetweenFactor.cpp
@@ -1,18 +1,19 @@
 /**
- * @file    testBetweenFactor.cpp
+ * @file testBetweenFactor.cpp
  * @brief
- * @author Duy-Nguyen Ta
- * @date    Aug 2, 2013
+ * @author Duy-Nguyen Ta, Varun Agrawal
+ * @date Aug 2, 2013
  */
 
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/slam/BetweenFactor.h>
 
-#include <boost/bind/bind.hpp>
-#include <CppUnitLite/TestHarness.h>
-
 using namespace boost::placeholders;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
@@ -48,7 +49,6 @@ TEST(BetweenFactor, Rot3) {
 }
 
 /* ************************************************************************* */
-/*
 // Constructor scalar
 TEST(BetweenFactor, ConstructorScalar) {
   SharedNoiseModel model;
@@ -56,6 +56,7 @@ TEST(BetweenFactor, ConstructorScalar) {
   BetweenFactor<double> factor(1, 2, measured_value, model);
 }
 
+/* ************************************************************************* */
 // Constructor vector3
 TEST(BetweenFactor, ConstructorVector3) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
@@ -63,13 +64,55 @@ TEST(BetweenFactor, ConstructorVector3) {
   BetweenFactor<Vector3> factor(1, 2, measured_value, model);
 }
 
+/* ************************************************************************* */
 // Constructor dynamic sized vector
 TEST(BetweenFactor, ConstructorDynamicSizeVector) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
   Vector measured_value(5); measured_value << 1, 2, 3, 4, 5;
   BetweenFactor<Vector> factor(1, 2, measured_value, model);
 }
-*/
+
+/* ************************************************************************* */
+TEST(BetweenFactor, Point3Jacobians) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
+  Point3 measured_value(1, 2, 3);
+  BetweenFactor<Point3> factor(1, 2, measured_value, model);
+  
+  Values values;
+  values.insert(1, Point3(0, 0, 0));
+  values.insert(2, Point3(1, 2, 3));
+  Vector3 error = factor.evaluateError(Point3(0, 0, 0), Point3(1, 2, 3));
+  EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(BetweenFactor, Rot3Jacobians) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
+  Rot3 measured_value = Rot3::Ry(M_PI_2);
+  BetweenFactor<Rot3> factor(1, 2, measured_value, model);
+  
+  Values values;
+  values.insert(1, Rot3());
+  values.insert(2, Rot3::Ry(M_PI_2));
+  Vector3 error = factor.evaluateError(Rot3(), Rot3::Ry(M_PI_2));
+  EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(BetweenFactor, Pose3Jacobians) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0);
+  Pose3 measured_value(Rot3(), Point3(1, 2, 3));
+  BetweenFactor<Pose3> factor(1, 2, measured_value, model);
+
+  Values values;
+  values.insert(1, Pose3());
+  values.insert(2, Pose3(Rot3(), Point3(1, 2, 3)));
+  Vector3 error = factor.evaluateError(Pose3(), Pose3(Rot3(), Point3(1, 2, 3)));
+  EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
 
 /* ************************************************************************* */
 int main() {

From a12b49de40aebb61966407f24411cf9b4d237478 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 8 Jul 2021 19:43:09 -0400
Subject: [PATCH 15/72] add Pose3 expmap to wrapper

---
 gtsam/gtsam.i | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 94d10953bb..049a0110c6 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -690,6 +690,7 @@ class Pose3 {
   // Lie Group
   static gtsam::Pose3 Expmap(Vector v);
   static Vector Logmap(const gtsam::Pose3& pose);
+  gtsam::Pose3 expmap(Vector v);
   Vector logmap(const gtsam::Pose3& pose);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;

From d7d9ac0f0623f4d5d07bfa692df600a3729fc05b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 8 Jul 2021 19:43:25 -0400
Subject: [PATCH 16/72] typo fix

---
 gtsam/geometry/Pose3.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 318baab3d1..d76e1b41a1 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -142,7 +142,7 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
   static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
 
   /**
-   * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
+   * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame
    * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
    */
   Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect

From bdeb60679b7ea8f9b1a7a53166da65590b007bcb Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Fri, 9 Jul 2021 11:14:10 +0200
Subject: [PATCH 17/72] Introduce setUpClass, python snake_case variables

Test case fails if object depth z is not equal 1.
---
 python/gtsam/tests/test_Cal3Fisheye.py | 103 +++++++++++++------------
 1 file changed, 53 insertions(+), 50 deletions(-)

diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py
index 18c8ecd80e..23f7a9b8c3 100644
--- a/python/gtsam/tests/test_Cal3Fisheye.py
+++ b/python/gtsam/tests/test_Cal3Fisheye.py
@@ -5,8 +5,9 @@
 
 See LICENSE for the license information
 
-Cal3Unified unit tests.
+Cal3Fisheye unit tests.
 Author: Frank Dellaert & Duy Nguyen Ta (Python)
+Refactored: Roderick Koehle
 """
 import unittest
 
@@ -14,78 +15,80 @@
 
 import gtsam
 from gtsam.utils.test_case import GtsamTestCase
-
+from gtsam.symbol_shorthand import K, L, P
 
 class TestCal3Fisheye(GtsamTestCase):
-
+    
+    @classmethod
+    def setUpClass(cls):
+        """
+        Equidistant fisheye projection
+        
+        An equidistant fisheye projection with focal length f is defined
+        as the relation r/f = tan(theta), with r being the radius in the 
+        image plane and theta the incident angle of the object point.
+        """
+        x, y, z = 1.0, 0.0, 1.0
+        # x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails!
+        u, v = np.arctan2(x, z), 0.0
+        cls.obj_point = np.array([x, y, z])
+        cls.img_point = np.array([u, v])
+        
     def test_Cal3Fisheye(self):
         K = gtsam.Cal3Fisheye()
         self.assertEqual(K.fx(), 1.)
         self.assertEqual(K.fy(), 1.)
 
     def test_distortion(self):
-        "Equidistant fisheye model of focal length f, defined as r/f = tan(theta)"
+        """Fisheye distortion and rectification"""
         equidistant = gtsam.Cal3Fisheye()
-        x, y, z = 1, 0, 1
-        u, v = equidistant.uncalibrate([x, y])
-        x2, y2 = equidistant.calibrate([u, v])
-        self.assertAlmostEqual(u, np.arctan2(x, z))
-        self.assertAlmostEqual(v, 0)
-        self.assertAlmostEqual(x2, x)
-        self.assertAlmostEqual(y2, 0)
+        perspective_pt = self.obj_point[0:2]/self.obj_point[2]
+        distorted_pt = equidistant.uncalibrate(perspective_pt)
+        rectified_pt = equidistant.calibrate(distorted_pt)
+        self.gtsamAssertEquals(distorted_pt, self.img_point)
+        self.gtsamAssertEquals(rectified_pt, perspective_pt)
 
     def test_pinhole(self):
-        "Spatial equidistant camera projection"
-        x, y, z = 1.0, 0.0, 1.0
-        u, v = np.arctan2(x, z), 0.0
+        """Spatial equidistant camera projection"""
         camera = gtsam.PinholeCameraCal3Fisheye()
-
-        pt1 = camera.Project([x, y, z])
+        pt1 = camera.Project(self.obj_point) # Perspective projection
+        pt2 = camera.project(self.obj_point) # Equidistant projection
+        x, y, z = self.obj_point
+        obj1 = camera.backproject(self.img_point, z)
+        r1 = camera.range(self.obj_point)
+        r = np.linalg.norm(self.obj_point)
         self.gtsamAssertEquals(pt1, np.array([x/z, y/z]))
-
-        pt2 = camera.project([x, y, z])
-        self.gtsamAssertEquals(pt2, np.array([u, v]))
-
-        obj1 = camera.backproject([u, v], z)
-        self.gtsamAssertEquals(obj1, np.array([x, y, z]))
-
-        r1 = camera.range(np.array([x, y, z]))
-        self.assertEqual(r1, np.linalg.norm([x, y, z]))
+        self.gtsamAssertEquals(pt2, self.img_point)
+        self.gtsamAssertEquals(obj1, self.obj_point)
+        self.assertEqual(r1, r)
 
     def test_generic_factor(self):
-        "Evaluate residual using pose and point as state variables"
-        objPoint = np.array([1, 0, 1])
-        imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0])
+        """Evaluate residual using pose and point as state variables"""
         graph = gtsam.NonlinearFactorGraph()
         state = gtsam.Values()
-        measured = imgPoint
-        noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1)
-        poseKey = gtsam.symbol_shorthand.P(0)
-        pointKey = gtsam.symbol_shorthand.L(0)
+        measured = self.img_point
+        noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
+        pose_key, point_key = P(0), L(0)
         k = gtsam.Cal3Fisheye()
-        state.insert_pose3(poseKey, gtsam.Pose3())
-        state.insert_point3(pointKey, gtsam.Point3(objPoint))
-        factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k)
+        state.insert_pose3(pose_key, gtsam.Pose3())
+        state.insert_point3(point_key, self.obj_point)
+        factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k)
         graph.add(factor)
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)
 
     def test_sfm_factor2(self):
-        "Evaluate residual with camera, pose and point as state variables"
-        objPoint = np.array([1, 0, 1])
-        imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0])
+        """Evaluate residual with camera, pose and point as state variables"""
         graph = gtsam.NonlinearFactorGraph()
         state = gtsam.Values()
-        measured = imgPoint
-        noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1)
-        cameraKey = gtsam.symbol_shorthand.K(0)
-        poseKey = gtsam.symbol_shorthand.P(0)
-        landmarkKey = gtsam.symbol_shorthand.L(0)
+        measured = self.img_point
+        noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
+        camera_key, pose_key, landmark_key = K(0), P(0), L(0)
         k = gtsam.Cal3Fisheye()
-        state.insert_cal3fisheye(cameraKey, k)
-        state.insert_pose3(poseKey, gtsam.Pose3())
-        state.insert_point3(landmarkKey, gtsam.Point3(objPoint))
-        factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey)
+        state.insert_cal3fisheye(camera_key, k)
+        state.insert_pose3(pose_key, gtsam.Pose3())
+        state.insert_point3(landmark_key, gtsam.Point3(self.obj_point))
+        factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key)
         graph.add(factor)
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)
@@ -93,12 +96,12 @@ def test_sfm_factor2(self):
     def test_retract(self):
         expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
                                      1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10)
-        K = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240,
+        k = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240,
                               1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3)
         d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F')
-        actual = K.retract(d)
+        actual = k.retract(d)
         self.gtsamAssertEquals(actual, expected)
-        np.testing.assert_allclose(d, K.localCoordinates(actual))
+        np.testing.assert_allclose(d, k.localCoordinates(actual))
 
 
 if __name__ == "__main__":

From 6205057ccbd63c3705ac109b47104d14e10b41e9 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Fri, 9 Jul 2021 11:17:38 +0200
Subject: [PATCH 18/72] Use of common setUpClass method

---
 python/gtsam/tests/test_Cal3Unified.py | 121 ++++++++++++-------------
 1 file changed, 60 insertions(+), 61 deletions(-)

diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index ff9d659607..f2c1ada488 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -14,94 +14,93 @@
 
 import gtsam
 from gtsam.utils.test_case import GtsamTestCase
+from gtsam.symbol_shorthand import K, L, P
 
 
 class TestCal3Unified(GtsamTestCase):
 
+    @classmethod
+    def setUpClass(cls):
+        """
+        Stereographic fisheye projection
+        
+        An equidistant fisheye projection with focal length f is defined
+        as the relation r/f = 2*tan(theta/2), with r being the radius in the 
+        image plane and theta the incident angle of the object point.
+        """
+        x, y, z = 1.0, 0.0, 1.0
+        r = np.linalg.norm([x, y, z])
+        u, v = 2*x/(z+r), 0.0
+        #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0
+        cls.obj_point = np.array([x, y, z])
+        cls.img_point = np.array([u, v])
+
+        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
+        k1, k2, p1, p2 = 0, 0, 0, 0
+        xi = 1
+        cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
+
     def test_Cal3Unified(self):
         K = gtsam.Cal3Unified()
         self.assertEqual(K.fx(), 1.)
         self.assertEqual(K.fx(), 1.)
 
     def test_distortion(self):
-        "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)"
-        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
-        k1, k2, p1, p2 = 0, 0, 0, 0
-        xi = 1
-        stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
-        x, y, z = 1, 0, 1
-        u, v = stereographic.uncalibrate([x, y])
+        """Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)"""
+        x, y, z = self.obj_point
         r = np.linalg.norm([x, y, z])
-        # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2))
-        self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r))
-        self.assertAlmostEqual(u, 2/(1+r))
-        x2, y2 = stereographic.calibrate([u, v])
-        self.assertAlmostEqual(x2, x)
+        # Note: 2*tan(atan2(x, z)/2) = 2*x/(z+sqrt(x^2+z^2))
+        self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2*x/(z+r))
+        perspective_pt = self.obj_point[0:2]/self.obj_point[2]
+        distorted_pt = self.stereographic.uncalibrate(perspective_pt)
+        rectified_pt = self.stereographic.calibrate(distorted_pt)
+        self.gtsamAssertEquals(distorted_pt, self.img_point)
+        self.gtsamAssertEquals(rectified_pt, perspective_pt)
 
     def test_pinhole(self):
-        "Spatial stereographic camera projection"
-        x, y, z = 1.0, 0.0, 1.0
-        r = np.linalg.norm([x, y, z])
-        u, v = 2/(1+r), 0.0
-        objPoint = np.array([x, y, z])
-        imgPoint = np.array([u, v])
-        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
-        k1, k2, p1, p2 = 0, 0, 0, 0
-        xi = 1
-        stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
+        """Spatial stereographic camera projection"""
+        x, y, z = self.obj_point
+        u, v = self.img_point
+        r = np.linalg.norm(self.obj_point)
         pose = gtsam.Pose3()
-        camera = gtsam.PinholeCameraCal3Unified(pose, stereographic)
-        pt1 = camera.Project(objPoint)
+        camera = gtsam.PinholeCameraCal3Unified(pose, self.stereographic)
+        pt1 = camera.Project(self.obj_point)
         self.gtsamAssertEquals(pt1, np.array([x/z, y/z]))
-        pt2 = camera.project(objPoint)
-        self.gtsamAssertEquals(pt2, np.array([u, v]))
-        obj1 = camera.backproject([u, v], z)
-        self.gtsamAssertEquals(obj1, np.array([x, y, z]))
-        r1 = camera.range(np.array([x, y, z]))
+        pt2 = camera.project(self.obj_point)
+        self.gtsamAssertEquals(pt2, self.img_point)
+        obj1 = camera.backproject(self.img_point, z)
+        self.gtsamAssertEquals(obj1, self.obj_point)
+        r1 = camera.range(self.obj_point)
         self.assertEqual(r1, r)
 
     def test_generic_factor(self):
-        "Evaluate residual using pose and point as state variables"
-        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
-        k1, k2, p1, p2 = 0, 0, 0, 0
-        xi = 1
-        objPoint = np.array([1, 0, 1])
-        r = np.linalg.norm(objPoint)
-        imgPoint = np.array([2/(1+r), 0])
+        """Evaluate residual using pose and point as state variables"""
         graph = gtsam.NonlinearFactorGraph()
         state = gtsam.Values()
-        measured = imgPoint
-        noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1)
-        poseKey = gtsam.symbol_shorthand.P(0)
-        pointKey = gtsam.symbol_shorthand.L(0)
-        k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
-        state.insert_pose3(poseKey, gtsam.Pose3())
-        state.insert_point3(pointKey, gtsam.Point3(objPoint))
-        factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k)
+        measured = self.img_point
+        noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
+        pose_key, point_key = P(0), L(0)
+        k = self.stereographic
+        state.insert_pose3(pose_key, gtsam.Pose3())
+        state.insert_point3(point_key, self.obj_point)
+        factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k)
         graph.add(factor)
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)
 
     def test_sfm_factor2(self):
-        "Evaluate residual with camera, pose and point as state variables"
-        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
-        k1, k2, p1, p2 = 0, 0, 0, 0
-        xi = 1
-        objPoint = np.array([1, 0, 1])
-        r = np.linalg.norm(objPoint)
-        imgPoint = np.array([2/(1+r), 0])
+        """Evaluate residual with camera, pose and point as state variables"""
+        r = np.linalg.norm(self.obj_point)
         graph = gtsam.NonlinearFactorGraph()
         state = gtsam.Values()
-        measured = imgPoint
-        noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1)
-        cameraKey = gtsam.symbol_shorthand.K(0)
-        poseKey = gtsam.symbol_shorthand.P(0)
-        landmarkKey = gtsam.symbol_shorthand.L(0)
-        k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
-        state.insert_cal3unified(cameraKey, k)
-        state.insert_pose3(poseKey, gtsam.Pose3())
-        state.insert_point3(landmarkKey, gtsam.Point3(objPoint))
-        factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey)
+        measured = self.img_point
+        noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
+        camera_key, pose_key, landmark_key = K(0), P(0), L(0)      
+        k = self.stereographic
+        state.insert_cal3unified(camera_key, k)
+        state.insert_pose3(pose_key, gtsam.Pose3())
+        state.insert_point3(landmark_key, self.obj_point)
+        factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key)
         graph.add(factor)
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)

From a411b664a198776f21aa6fd9a43732841e44add7 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Fri, 9 Jul 2021 11:25:18 +0200
Subject: [PATCH 19/72] Correct tab to spaces to fix formatting

---
 gtsam/gtsam.i | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 327574bf89..ce251802ce 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -2360,12 +2360,12 @@ class Values {
                  gtsam::Cal3Bundler,
                  gtsam::Cal3Fisheye,
                  gtsam::Cal3Unified,
-		 gtsam::EssentialMatrix,
+                 gtsam::EssentialMatrix,
                  gtsam::PinholeCameraCal3_S2,
                  gtsam::PinholeCameraCal3Bundler,
                  gtsam::PinholeCameraCal3Fisheye,
                  gtsam::PinholeCameraCal3Unified,
-		 gtsam::imuBias::ConstantBias,
+                 gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  Vector,
                  Matrix,

From 66af0079baa5de84618fdc1927dc55e8e9fcff81 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Fri, 9 Jul 2021 12:39:31 +0200
Subject: [PATCH 20/72] Improved accuracy for analytic undistortion

---
 gtsam/geometry/Cal3Fisheye.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index b9e60aceec..3ee036efff 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -110,7 +110,9 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
   const double u = uv.x(), v = uv.y();
   const double yd = (v - v0_) / fy_;
   const double xd = (u - s_ * yd - u0_) / fx_;
-  Point2 pi(xd, yd);
+  const double theta = sqrt(xd * xd + yd * yd);
+  const double scale = (theta > 0) ? tan(theta) / theta;
+  Point2 pi(scale * xd, scale * yd);
 
   // Perform newtons method, break when solution converges past tol_,
   // throw exception if max iterations are reached

From 8b9e60156c5c895430624dfbdb27ae9fb1c5b472 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 9 Jul 2021 14:06:59 -0400
Subject: [PATCH 21/72] cleaner variables

---
 gtsam/slam/tests/testBetweenFactor.cpp | 31 +++++++++++++-------------
 1 file changed, 16 insertions(+), 15 deletions(-)

diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp
index 31842bf800..206d2b590e 100644
--- a/gtsam/slam/tests/testBetweenFactor.cpp
+++ b/gtsam/slam/tests/testBetweenFactor.cpp
@@ -52,31 +52,31 @@ TEST(BetweenFactor, Rot3) {
 // Constructor scalar
 TEST(BetweenFactor, ConstructorScalar) {
   SharedNoiseModel model;
-  double measured_value = 0.0;
-  BetweenFactor<double> factor(1, 2, measured_value, model);
+  double measured = 0.0;
+  BetweenFactor<double> factor(1, 2, measured, model);
 }
 
 /* ************************************************************************* */
 // Constructor vector3
 TEST(BetweenFactor, ConstructorVector3) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
-  Vector3 measured_value(1, 2, 3);
-  BetweenFactor<Vector3> factor(1, 2, measured_value, model);
+  Vector3 measured(1, 2, 3);
+  BetweenFactor<Vector3> factor(1, 2, measured, model);
 }
 
 /* ************************************************************************* */
 // Constructor dynamic sized vector
 TEST(BetweenFactor, ConstructorDynamicSizeVector) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
-  Vector measured_value(5); measured_value << 1, 2, 3, 4, 5;
-  BetweenFactor<Vector> factor(1, 2, measured_value, model);
+  Vector measured(5); measured << 1, 2, 3, 4, 5;
+  BetweenFactor<Vector> factor(1, 2, measured, model);
 }
 
 /* ************************************************************************* */
 TEST(BetweenFactor, Point3Jacobians) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
-  Point3 measured_value(1, 2, 3);
-  BetweenFactor<Point3> factor(1, 2, measured_value, model);
+  Point3 measured(1, 2, 3);
+  BetweenFactor<Point3> factor(1, 2, measured, model);
   
   Values values;
   values.insert(1, Point3(0, 0, 0));
@@ -89,8 +89,8 @@ TEST(BetweenFactor, Point3Jacobians) {
 /* ************************************************************************* */
 TEST(BetweenFactor, Rot3Jacobians) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
-  Rot3 measured_value = Rot3::Ry(M_PI_2);
-  BetweenFactor<Rot3> factor(1, 2, measured_value, model);
+  Rot3 measured = Rot3::Ry(M_PI_2);
+  BetweenFactor<Rot3> factor(1, 2, measured, model);
   
   Values values;
   values.insert(1, Rot3());
@@ -103,13 +103,14 @@ TEST(BetweenFactor, Rot3Jacobians) {
 /* ************************************************************************* */
 TEST(BetweenFactor, Pose3Jacobians) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0);
-  Pose3 measured_value(Rot3(), Point3(1, 2, 3));
-  BetweenFactor<Pose3> factor(1, 2, measured_value, model);
+  Pose3 measured(Rot3(), Point3(1, 2, 3));
+  BetweenFactor<Pose3> factor(1, 2, measured, model);
 
+  Pose3 pose1, pose2(Rot3(), Point3(1, 2, 3));
   Values values;
-  values.insert(1, Pose3());
-  values.insert(2, Pose3(Rot3(), Point3(1, 2, 3)));
-  Vector3 error = factor.evaluateError(Pose3(), Pose3(Rot3(), Point3(1, 2, 3)));
+  values.insert(1, pose1);
+  values.insert(2, pose2);
+  Vector3 error = factor.evaluateError(pose1, pose2);
   EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }

From 2e4016932491bc7888cd9108e7667f7faf00e819 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 9 Jul 2021 14:07:19 -0400
Subject: [PATCH 22/72] fix dimension for Pose3 test

---
 gtsam/slam/tests/testBetweenFactor.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp
index 206d2b590e..a1f8774e5a 100644
--- a/gtsam/slam/tests/testBetweenFactor.cpp
+++ b/gtsam/slam/tests/testBetweenFactor.cpp
@@ -110,8 +110,8 @@ TEST(BetweenFactor, Pose3Jacobians) {
   Values values;
   values.insert(1, pose1);
   values.insert(2, pose2);
-  Vector3 error = factor.evaluateError(pose1, pose2);
-  EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
+  Vector6 error = factor.evaluateError(pose1, pose2);
+  EXPECT(assert_equal<Vector6>(Vector6::Zero(), error, 1e-9));
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }
 

From df579ec6a764f95f483a9f4acfa15b3ad008a58e Mon Sep 17 00:00:00 2001
From: Scott <scott.t.mcmichael@nasa.gov>
Date: Fri, 9 Jul 2021 13:07:08 -0700
Subject: [PATCH 23/72] Fix serialization of ISAM2 class

---
 gtsam/inference/VariableIndex.h | 10 ++++++++++
 gtsam/nonlinear/ISAM2.h         | 20 ++++++++++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h
index f2ba3e31cb..47438ecd68 100644
--- a/gtsam/inference/VariableIndex.h
+++ b/gtsam/inference/VariableIndex.h
@@ -182,6 +182,16 @@ class GTSAM_EXPORT VariableIndex {
     return item->second; 
   }
 
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(index_);
+    ar & BOOST_SERIALIZATION_NVP(nFactors_);
+    ar & BOOST_SERIALIZATION_NVP(nEntries_);
+  }
+
   /// @}
 };
 
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 9f33e757ff..92c2142a73 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
   void removeVariables(const KeySet& unusedKeys);
 
   void updateDelta(bool forceFullSolve = false) const;
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
+      ar & BOOST_SERIALIZATION_NVP(theta_);
+      ar & BOOST_SERIALIZATION_NVP(variableIndex_);
+      ar & BOOST_SERIALIZATION_NVP(delta_);
+      ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
+      ar & BOOST_SERIALIZATION_NVP(RgProd_);
+      ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
+      ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
+      ar & BOOST_SERIALIZATION_NVP(linearFactors_);
+      ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
+      ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
+      ar & BOOST_SERIALIZATION_NVP(update_count_);
+  }
+
 };  // ISAM2
 
 /// traits

From 15478bf2783fbf4bac53e570bcd3bb61376a7d68 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Fri, 9 Jul 2021 17:33:33 -0400
Subject: [PATCH 24/72] Update ShonanAveraging.h

---
 gtsam/sfm/ShonanAveraging.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h
index f00109caee..bff5ab471a 100644
--- a/gtsam/sfm/ShonanAveraging.h
+++ b/gtsam/sfm/ShonanAveraging.h
@@ -430,6 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> {
                    const Parameters &parameters = Parameters());
   explicit ShonanAveraging2(std::string g2oFile,
                             const Parameters &parameters = Parameters());
+  ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
+                   const gtsam::ShonanAveragingParameters2 &parameters);
 };
 
 class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> {

From 3c1823349b6b90a7fce71bfeca229a56e26bed70 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Fri, 9 Jul 2021 17:38:10 -0400
Subject: [PATCH 25/72] add interface in C++, and helper
 extractRot2Measurements()

---
 gtsam/sfm/ShonanAveraging.cpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index a982ef7da9..f933f368d8 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -944,6 +944,20 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
                                      parameters.getUseHuber()),
                          parameters) {}
 
+static ShonanAveraging2::Measurements extractRot2Measurements(
+    const BetweenFactorPose2s &factors) {
+  ShonanAveraging2::Measurements result;
+  result.reserve(factors.size());
+  for (auto f : factors) result.push_back(convert(f));
+  return result;
+}
+
+ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors,
+                                   const Parameters &parameters)
+    : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors),
+                                     parameters.getUseHuber()),
+                         parameters) {}    
+    
 /* ************************************************************************* */
 // Explicit instantiation for d=3
 template class ShonanAveraging<3>;

From 0e0d630c917ec6f80a8a037f61bd030842f7feb0 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Fri, 9 Jul 2021 17:39:38 -0400
Subject: [PATCH 26/72] fix typo

---
 gtsam/sfm/ShonanAveraging.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index f933f368d8..0de37be1e6 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -954,7 +954,7 @@ static ShonanAveraging2::Measurements extractRot2Measurements(
 
 ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors,
                                    const Parameters &parameters)
-    : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors),
+    : ShonanAveraging<2>(maybeRobust(extractRot2Measurements(factors),
                                      parameters.getUseHuber()),
                          parameters) {}    
     

From 3c8cdb4eeee70f0a95a8ccea54373262ea24b618 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Fri, 9 Jul 2021 17:42:59 -0400
Subject: [PATCH 27/72] add ShonanAveraging2 constructor to wrapper, that
 accepts BetweenFactorPose2s as input

---
 gtsam/gtsam.i | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 94d10953bb..f17679739f 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -3123,6 +3123,8 @@ class ShonanAveraging2 {
   ShonanAveraging2(string g2oFile);
   ShonanAveraging2(string g2oFile,
                    const gtsam::ShonanAveragingParameters2 &parameters);
+  ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
+                   const gtsam::ShonanAveragingParameters2 &parameters);
 
   // Query properties
   size_t nrUnknowns() const;

From 4bf2308ec595a96270952d81ce25e8d2d2d447fa Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Fri, 9 Jul 2021 17:48:26 -0400
Subject: [PATCH 28/72] add conversion function for Pose2 ->
 BinaryMeasurement<Rot2>

---
 gtsam/sfm/ShonanAveraging.cpp | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index 0de37be1e6..7810d0d470 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -944,11 +944,27 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
                                      parameters.getUseHuber()),
                          parameters) {}
 
+// Extract Rot2 measurement from Pose2 betweenfactors
+// Modeled after similar function in dataset.cpp
+static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
+    const BetweenFactor<Pose2>::shared_ptr &f) {
+  auto gaussian =
+      boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
+  if (!gaussian)
+    throw std::invalid_argument(
+        "parseMeasurements<Rot2> can only convert Pose2 measurements "
+        "with Gaussian noise models.");
+  const Matrix6 M = gaussian->covariance();
+  auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2));
+  return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
+                                 model);
+}
+    
 static ShonanAveraging2::Measurements extractRot2Measurements(
     const BetweenFactorPose2s &factors) {
   ShonanAveraging2::Measurements result;
   result.reserve(factors.size());
-  for (auto f : factors) result.push_back(convert(f));
+  for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f));
   return result;
 }
 

From 641a01c72620c6736d917c470e85db26ae471980 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Fri, 9 Jul 2021 17:54:14 -0400
Subject: [PATCH 29/72] fix typo on 3x3 matrix def

---
 gtsam/sfm/ShonanAveraging.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index 7810d0d470..88c393f16d 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -954,7 +954,7 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
     throw std::invalid_argument(
         "parseMeasurements<Rot2> can only convert Pose2 measurements "
         "with Gaussian noise models.");
-  const Matrix6 M = gaussian->covariance();
+  const Matrix3 M = gaussian->covariance();
   auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2));
   return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
                                  model);

From 8c68e215215f640a40cbf4b2d863fea64124f450 Mon Sep 17 00:00:00 2001
From: Scott <scott.t.mcmichael@nasa.gov>
Date: Fri, 9 Jul 2021 18:30:39 -0700
Subject: [PATCH 30/72] Added ISAM2 serialize test

---
 .../tests/testSerializationNonlinear.cpp      | 79 +++++++++++++++++++
 1 file changed, 79 insertions(+)

diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
index 90ebcbbba6..0375826545 100644
--- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
+++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
@@ -31,6 +32,30 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
+
+/* ************************************************************************* */
+// Create GUIDs for Noisemodels
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+/* ************************************************************************* */
+// Create GUIDs for factors
+BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>");
+BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional");
+
+
 /* ************************************************************************* */
 // Export all classes derived from Value
 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
@@ -82,6 +107,60 @@ TEST (Serialization, TemplatedValues) {
   EXPECT(equalsBinary(values));
 }
 
+TEST(Serialization, ISAM2) {
+
+  gtsam::ISAM2Params parameters;
+  gtsam::ISAM2 solver(parameters);
+  gtsam::NonlinearFactorGraph graph;
+  gtsam::Values initialValues;
+  initialValues.clear();
+
+  gtsam::Vector6 temp6;
+  for (int i = 0; i < 6; ++i) {
+    temp6[i] = 0.0001;
+  }
+  gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6);
+
+  gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0));
+  gtsam::Symbol symbol0('x', 0);
+  graph.add(gtsam::PriorFactor<gtsam::Pose3>(symbol0, pose0, noiseModel));
+  initialValues.insert(symbol0, pose0);
+
+  solver.update(graph, initialValues, gtsam::FastVector<size_t>());
+
+  std::string binaryPath = "saved_solver.dat";
+  try {
+    std::ofstream outputStream(binaryPath);
+    boost::archive::binary_oarchive outputArchive(outputStream);
+    outputArchive << solver;
+  } catch(...) {
+    EXPECT(false);
+  }
+
+  gtsam::ISAM2 solverFromDisk;
+  try {
+    std::ifstream ifs(binaryPath);
+    boost::archive::binary_iarchive inputArchive(ifs);
+    inputArchive >> solverFromDisk;
+  } catch(...) {
+    EXPECT(false);
+  }
+
+  gtsam::Pose3 p1, p2;
+  try {
+    p1 = solver.calculateEstimate<gtsam::Pose3>(symbol0);
+  } catch(std::exception &e) {
+    EXPECT(false);
+  }
+
+  try {
+    p2 = solverFromDisk.calculateEstimate<gtsam::Pose3>(symbol0);
+  } catch(std::exception &e) {
+    EXPECT(false);
+  }
+  EXPECT(assert_equal(p1, p2));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

From 7fc8f23367eeba02c7ac20464eaeee78ce9d5702 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Fri, 9 Jul 2021 23:34:55 -0400
Subject: [PATCH 31/72] use default parameters if none provided, and remove
 gtsam namespace prefix in .h file

---
 gtsam/sfm/ShonanAveraging.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h
index bff5ab471a..de12de4782 100644
--- a/gtsam/sfm/ShonanAveraging.h
+++ b/gtsam/sfm/ShonanAveraging.h
@@ -430,8 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> {
                    const Parameters &parameters = Parameters());
   explicit ShonanAveraging2(std::string g2oFile,
                             const Parameters &parameters = Parameters());
-  ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
-                   const gtsam::ShonanAveragingParameters2 &parameters);
+  ShonanAveraging2(const BetweenFactorPose2s &factors,
+                   const Parameters &parameters = Parameters());
 };
 
 class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> {

From 64514387b42e04249b467c9d2b5bbb6807ded4b5 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 10 Jul 2021 00:57:01 -0400
Subject: [PATCH 32/72] check in python unit test for new functionality

---
 python/gtsam/tests/test_ShonanAveraging.py | 66 +++++++++++++++++++++-
 1 file changed, 64 insertions(+), 2 deletions(-)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index 4c423574d8..fe0d7fc2b0 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -13,14 +13,25 @@
 import unittest
 
 import gtsam
-from gtsam import ShonanAveraging3, ShonanAveragingParameters3
+from gtsam import (
+    BetweenFactorPose2,
+    LevenbergMarquardtParams,
+    Rot2,
+    Pose2,
+    ShonanAveraging2,
+    ShonanAveragingParameters2,
+    ShonanAveraging3,
+    ShonanAveragingParameters3
+)
 from gtsam.utils.test_case import GtsamTestCase
 
 DEFAULT_PARAMS = ShonanAveragingParameters3(
     gtsam.LevenbergMarquardtParams.CeresDefaults())
 
 
-def fromExampleName(name: str, parameters=DEFAULT_PARAMS):
+def fromExampleName(
+    name: str, parameters: ShonanAveragingParameters3 = DEFAULT_PARAMS
+) -> ShonanAveraging3:
     g2oFile = gtsam.findExampleDataFile(name)
     return ShonanAveraging3(g2oFile, parameters)
 
@@ -133,6 +144,57 @@ def test_PriorWeights(self):
         self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3)
         result, _lambdaMin = shonan.run(initial, 3, 3)
         self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
+    
+    def test_constructorBetweenFactorPose2s(self) -> None:
+        """Check if ShonanAveraging2 constructor works when not initialized from g2o file."""
+        # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1
+        i2Ri1_dict = {
+            (1, 2):  -43.86342,
+            (1, 5):  -135.06351,
+            (2, 4):  72.64527,
+            (3, 5):  117.75967,
+            (6, 7):  -31.73934,
+            (7, 10): 177.45901,
+            (6, 9):  -133.58713,
+            (7, 8):  -90.58668,
+            (0, 3):  127.02978,
+            (8, 10): -92.16361,
+            (4, 8):  51.63781,
+            (4, 6):  173.96384,
+            (4, 10): 139.59445,
+            (2, 3):  151.04022,
+            (3, 4):  -78.39495,
+            (1, 4):  28.78185,
+            (6, 8):  -122.32602,
+            (0, 2):  -24.01045,
+            (5, 7):  -53.93014,
+            (4, 5):  -163.84535,
+            (2, 5):  -91.20009,
+            (1, 3):  107.17680,
+            (7, 9):  -102.35615,
+            (0, 1):  19.85297,
+            (5, 8):  -144.51682,
+            (5, 6):  -22.19079,
+            (5, 10): -56.56016,
+        }
+        i2Ri1_dict = {(i1,i2): Rot2.fromDegrees(theta_deg) for (i1,i2), theta_deg in i2Ri1_dict.items()}
+        lm_params = LevenbergMarquardtParams.CeresDefaults()
+        shonan_params = ShonanAveragingParameters2(lm_params)
+        shonan_params.setUseHuber(False)
+        shonan_params.setCertifyOptimality(True)
+        
+        noise_model = gtsam.noiseModel.Unit.Create(3)
+        between_factors = gtsam.BetweenFactorPose2s()
+        for (i1, i2), i2Ri1 in i2Ri1_dict.items():
+            i2Ti1 = Pose2(i2Ri1, np.zeros(2))
+            between_factors.append(BetweenFactorPose2(i2, i1, i2Ti1, noise_model))
+        
+        obj = ShonanAveraging2(between_factors, shonan_params)
+        initial = obj.initializeRandomly()
+        result_values, _ = obj.run(initial, p_min, self._p_max)
+        
+        for i in range(11):
+            wRi = result_values.atRot2(i)
 
 
 if __name__ == '__main__':

From 03049929a55d430bc67404bf8663d792ee9d62fb Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Sat, 10 Jul 2021 15:21:22 +0200
Subject: [PATCH 33/72] Add comment about initial guess in undistortion

For the equidistant fisheye model, r/f = tan(theta), the Gauss-Newton search to model radial distortion is expected to converge faster by mapping the angular coordinate space into the respective tangent space of the perspective plane. This is consistent to the nPlaneToSpace initial projection used in the calibrate() function of the omnidirectional model (Cal3Unified).
---
 gtsam/geometry/Cal3Fisheye.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index 3ee036efff..52d475d5d1 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -106,12 +106,20 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 /* ************************************************************************* */
 Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
-  // initial gues just inverts the pinhole model
+  // Apply inverse camera matrix to map the pixel coordinate (u, v) 
+  // of the equidistant fisheye image to angular coordinate space (xd, yd)
+  // with radius theta given in radians.
   const double u = uv.x(), v = uv.y();
   const double yd = (v - v0_) / fy_;
   const double xd = (u - s_ * yd - u0_) / fx_;
   const double theta = sqrt(xd * xd + yd * yd);
-  const double scale = (theta > 0) ? tan(theta) / theta;
+  
+  // Provide initial guess for the Gauss-Newton search.
+  // The angular coordinates given by (xd, yd) are mapped back to
+  // the focal plane of the perspective undistorted projection pi.
+  // See Cal3Unified.calibrate() using the same pattern for the 
+  // undistortion of omnidirectional fisheye projection.
+  const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
   Point2 pi(scale * xd, scale * yd);
 
   // Perform newtons method, break when solution converges past tol_,

From db801f164d2ff7c6c65d79058959c6dfb7832ef4 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 10 Jul 2021 10:31:27 -0400
Subject: [PATCH 34/72] add missing import to python unit test

---
 python/gtsam/tests/test_ShonanAveraging.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index fe0d7fc2b0..a873a64306 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -13,6 +13,7 @@
 import unittest
 
 import gtsam
+import numpy as np
 from gtsam import (
     BetweenFactorPose2,
     LevenbergMarquardtParams,

From 8b86d7a51c803b6dbf52e67d5438e1b792c328d5 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 10 Jul 2021 11:36:36 -0400
Subject: [PATCH 35/72] improve docs about compiling without TBB

---
 INSTALL.md | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/INSTALL.md b/INSTALL.md
index 520bddf3ca..64857774ab 100644
--- a/INSTALL.md
+++ b/INSTALL.md
@@ -20,9 +20,10 @@ $ make install
     Optional dependent libraries:
      - If TBB is installed and detectable by CMake GTSAM will use it automatically.
        Ensure that CMake prints "Use Intel TBB : Yes".  To disable the use of TBB,
-       disable the CMake flag GTSAM_WITH_TBB (enabled by default).  On Ubuntu, TBB
-       may be installed from the Ubuntu repositories, and for other platforms it
-       may be downloaded from https://www.threadingbuildingblocks.org/
+       disable the CMake flag `GTSAM_WITH_TBB` (enabled by default) by providing
+       the argument `-DGTSAM_WITH_TBB=OFF` to `cmake`.  On Ubuntu, TBB may be
+       installed from the Ubuntu repositories, and for other platforms it may be
+       downloaded from https://www.threadingbuildingblocks.org/
      - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
        `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
        achieved with MKL disabled. We therefore advise you to benchmark your problem 

From 63236cf7afa2da29850ac45e310bc166f20e6f5a Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 10 Jul 2021 11:37:59 -0400
Subject: [PATCH 36/72] improve wrapper compilation instructions, when TBB not
 installed

---
 python/README.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/python/README.md b/python/README.md
index ec9808ce0d..54436df93b 100644
--- a/python/README.md
+++ b/python/README.md
@@ -24,6 +24,7 @@ For instructions on updating the version of the [wrap library](https://github.co
   ```bash
   cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10
   ```
+  If you do not have TBB installed, you should also provide the argument `-DGTSAM_WITH_TBB=OFF`.
 - Build GTSAM and the wrapper with `make` (or `ninja` if you use `-GNinja`).
 
 - To install, simply run `make python-install` (`ninja python-install`).

From aebb90573ab688287cf703f6ed4af97f5a2be962 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 10 Jul 2021 11:41:46 -0400
Subject: [PATCH 37/72] set pmin and pmax in unit test

---
 python/gtsam/tests/test_ShonanAveraging.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index a873a64306..69e7055453 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -148,6 +148,7 @@ def test_PriorWeights(self):
     
     def test_constructorBetweenFactorPose2s(self) -> None:
         """Check if ShonanAveraging2 constructor works when not initialized from g2o file."""
+        num_images = 11
         # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1
         i2Ri1_dict = {
             (1, 2):  -43.86342,
@@ -192,9 +193,9 @@ def test_constructorBetweenFactorPose2s(self) -> None:
         
         obj = ShonanAveraging2(between_factors, shonan_params)
         initial = obj.initializeRandomly()
-        result_values, _ = obj.run(initial, p_min, self._p_max)
+        result_values, _ = obj.run(initial, min_p=2, max_p=10)
         
-        for i in range(11):
+        for i in range(num_images):
             wRi = result_values.atRot2(i)
 
 

From d54e234f93128c3c612e0ef0a2c65cb9a1182fae Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Sat, 10 Jul 2021 22:03:17 +0200
Subject: [PATCH 38/72] Add ambiguous calibrate/uncalibrate declarations.

Without declaring calibrate / uncalibrated in the interface specification, the functions of the Base class Cal3DS2_Base is called.
The layout of the optional Jacobian matrix is 2x10 in Cal3Unified and 2x9 in Cal3DS2_Base, so this are different function calls.
---
 gtsam/gtsam.i | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index ce251802ce..97012c2910 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -910,6 +910,12 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
   gtsam::Cal3Unified retract(Vector v) const;
   Vector localCoordinates(const gtsam::Cal3Unified& c) const;
 
+  // Action on Point2
+  // Note: the signature of this functions differ from the functions
+  // with equal name in the base class.
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
   // enabling serialization functionality
   void serialize() const;
 

From 3118fde6d3dce098ffbca641afb18488f97a9dbc Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Sat, 10 Jul 2021 23:00:24 +0200
Subject: [PATCH 39/72] Missing CameraSet binding specialisations

Add pybind specialisations for CameraSetCal3Unified and CameraSetCal3Fisheye.
---
 python/gtsam/specializations.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h
index be8eb8a6cf..2e8612fec6 100644
--- a/python/gtsam/specializations.h
+++ b/python/gtsam/specializations.h
@@ -32,4 +32,6 @@ py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified> > >(m_, "CameraSetCal3Unified");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Fisheye> > >(m_, "CameraSetCal3Fisheye");
 py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

From 0a73961f5a2762100d30147db25fde150434d1a6 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Sat, 10 Jul 2021 23:05:53 +0200
Subject: [PATCH 40/72] Update ignore list in CMakeFile

---
 python/CMakeLists.txt | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 5f51368e6a..528732f4bd 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -102,6 +102,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::BinaryMeasurementsUnit3
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler
+            gtsam::CameraSetCal3Unified
+            gtsam::CameraSetCal3Fisheye
             gtsam::KeyPairDoubleMap)
             
     pybind_wrap(gtsam_unstable_py # target

From dc8b5e58ff2b8b48ba5b5fc329271daf3b8c2468 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 10 Jul 2021 21:01:20 -0400
Subject: [PATCH 41/72] replaced boost with std for placeholders, bind and
 function

---
 gtsam/base/Matrix.h                           |   4 +-
 gtsam/base/Testable.h                         |   7 +-
 gtsam/base/lieProxies.h                       |   2 +-
 gtsam/base/numericalDerivative.h              | 345 +++++++++---------
 gtsam/discrete/DecisionTree-inl.h             |   4 +-
 gtsam/discrete/DecisionTree.h                 |  12 +-
 gtsam/geometry/Cyclic.h                       |   4 +-
 gtsam/inference/EliminateableFactorGraph.h    |   4 +-
 gtsam/inference/LabeledSymbol.cpp             |  41 ++-
 gtsam/inference/LabeledSymbol.h               |   8 +-
 gtsam/inference/Symbol.cpp                    |   7 +-
 gtsam/inference/Symbol.h                      |   7 +-
 gtsam/linear/VectorValues.cpp                 |   4 +-
 gtsam/nonlinear/Expression-inl.h              |  24 +-
 gtsam/nonlinear/Expression.h                  |   8 +-
 gtsam/nonlinear/NonlinearEquality.h           |  10 +-
 gtsam/nonlinear/Values-inl.h                  |  26 +-
 gtsam/nonlinear/Values.h                      |  18 +-
 gtsam/nonlinear/internal/ExpressionNode.h     |   2 +-
 gtsam_unstable/base/BTree.h                   |   8 +-
 gtsam_unstable/base/DSF.h                     |   2 +-
 gtsam_unstable/dynamics/FullIMUFactor.h       |   4 +-
 gtsam_unstable/dynamics/IMUFactor.h           |   4 +-
 gtsam_unstable/dynamics/SimpleHelicopter.h    |  12 +-
 gtsam_unstable/dynamics/VelocityConstraint.h  |   8 +-
 gtsam_unstable/linear/QPSParser.cpp           |  47 ++-
 .../slam/EquivInertialNavFactor_GlobalVel.h   |  68 ++--
 .../EquivInertialNavFactor_GlobalVel_NoBias.h |  26 +-
 .../slam/InertialNavFactor_GlobalVelocity.h   |  40 +-
 gtsam_unstable/slam/InvDepthFactorVariant1.h  |   8 +-
 gtsam_unstable/slam/InvDepthFactorVariant2.h  |   8 +-
 gtsam_unstable/slam/InvDepthFactorVariant3.h  |  20 +-
 32 files changed, 408 insertions(+), 384 deletions(-)

diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index a3a14c6c32..013947bbde 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -29,7 +29,7 @@
 #include <gtsam/config.h>
 
 #include <boost/format.hpp>
-#include <boost/function.hpp>
+#include <functional>
 #include <boost/tuple/tuple.hpp>
 #include <boost/math/special_functions/fpclassify.hpp>
 
@@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction {
 
   // The function phi should calculate f(a)*b, with derivatives in a and b.
   // Naturally, the derivative in b is f(a).
-  typedef boost::function<VectorN(
+  typedef std::function<VectorN(
       const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
       Operator;
 
diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h
index e8cdbd8e09..6062c7ae12 100644
--- a/gtsam/base/Testable.h
+++ b/gtsam/base/Testable.h
@@ -34,8 +34,9 @@
 #pragma once
 
 #include <boost/concept_check.hpp>
-#include <boost/shared_ptr.hpp>
+#include <functional>
 #include <iostream>
+#include <memory>
 #include <string>
 
 #define GTSAM_PRINT(x)((x).print(#x))
@@ -119,10 +120,10 @@ namespace gtsam {
    * Binary predicate on shared pointers
    */
   template<class V>
-  struct equals_star : public std::function<bool(const boost::shared_ptr<V>&, const boost::shared_ptr<V>&)> {
+  struct equals_star : public std::function<bool(const std::shared_ptr<V>&, const std::shared_ptr<V>&)> {
     double tol_;
     equals_star(double tol = 1e-9) : tol_(tol) {}
-    bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
+    bool operator()(const std::shared_ptr<V>& expected, const std::shared_ptr<V>& actual) {
       if (!actual && !expected) return true;
       return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
     }
diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h
index 820fc73337..c811eb58a7 100644
--- a/gtsam/base/lieProxies.h
+++ b/gtsam/base/lieProxies.h
@@ -24,7 +24,7 @@
  *
  * These should not be used outside of tests, as they are just remappings
  * of the original functions.  We use these to avoid needing to do
- * too much boost::bind magic or writing a bunch of separate proxy functions.
+ * too much std::bind magic or writing a bunch of separate proxy functions.
  *
  * Don't expect all classes to work for all of these functions.
  */
diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index c624c900eb..05b60033f8 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -18,8 +18,7 @@
 // \callgraph
 #pragma once
 
-#include <boost/function.hpp>
-#include <boost/bind/bind.hpp>
+#include <functional>
 
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/JacobianFactor.h>
@@ -38,13 +37,13 @@ namespace gtsam {
  *   for a function with one relevant param and an optional derivative:
  *     Foo bar(const Obj& a, boost::optional<Matrix&> H1)
  *   Use boost.bind to restructure:
- *     boost::bind(bar, boost::placeholders::_1, boost::none)
+ *     std::bind(bar, std::placeholders::_1, boost::none)
  *   This syntax will fix the optional argument to boost::none, while using the first argument provided
  *
  * For member functions, such as below, with an instantiated copy instanceOfSomeClass
  *     Foo SomeClass::bar(const Obj& a)
  * Use boost bind as follows to create a function pointer that uses the member function:
- *       boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), boost::placeholders::_1)
+ *       std::bind(&SomeClass::bar, ref(instanceOfSomeClass), std::placeholders::_1)
  *
  * For additional details, see the documentation:
  *     http://www.boost.org/doc/libs/release/libs/bind/bind.html
@@ -69,7 +68,7 @@ struct FixedSizeMatrix {
 
 template <class X, int N = traits<X>::dimension>
 typename Eigen::Matrix<double, N, 1> numericalGradient(
-    boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
+    std::function<double(const X&)> h, const X& x, double delta = 1e-5) {
   double factor = 1.0 / (2.0 * delta);
 
   BOOST_STATIC_ASSERT_MSG(
@@ -109,7 +108,7 @@ typename Eigen::Matrix<double, N, 1> numericalGradient(
 template <class Y, class X, int N = traits<X>::dimension>
 // TODO Should compute fixed-size matrix
 typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
-    boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
+    std::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
 
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
@@ -150,7 +149,7 @@ 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,
+  return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
                                      delta);
 }
 
@@ -164,14 +163,14 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const
  * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
  */
 template<class Y, class X1, class X2, int N = traits<X1>::dimension>
-typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const std::function<Y(const X1&, const X2&)>& h,
     const X1& x1, const X2& x2, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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);
+      std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
 }
 
 /** use a raw C++ function pointer */
@@ -179,7 +178,7 @@ 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,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
       delta);
 }
 
@@ -193,14 +192,14 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(cons
  * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
  */
 template<class Y, class X1, class X2, int N = traits<X2>::dimension>
-typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
+typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(std::function<Y(const X1&, const X2&)> h,
     const X1& x1, const X2& x2, double delta = 1e-5) {
 //  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.");
   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);
+      std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
 }
 
 /** use a raw C++ function pointer */
@@ -208,7 +207,7 @@ 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,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
       delta);
 }
 
@@ -225,14 +224,14 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(cons
  */
 template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
-    boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
       x1, delta);
 }
 
@@ -240,8 +239,8 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3),
       x1, x2, x3, delta);
 }
 
@@ -258,14 +257,14 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(cons
  */
 template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
-    boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
       x2, delta);
 }
 
@@ -273,8 +272,8 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3),
       x1, x2, x3, delta);
 }
 
@@ -291,14 +290,14 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*
  */
 template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
-    boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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),
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
       x3, delta);
 }
 
@@ -306,8 +305,8 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3),
       x1, x2, x3, delta);
 }
 
@@ -324,15 +323,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
+                  std::cref(x4)),
       x1, delta);
 }
 
@@ -340,8 +339,8 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4),
       x1, x2, x3, x4);
 }
 
@@ -358,15 +357,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
+                  std::cref(x4)),
       x2, delta);
 }
 
@@ -374,8 +373,8 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4),
       x1, x2, x3, x4);
 }
 
@@ -392,15 +391,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
+                  std::cref(x4)),
       x3, delta);
 }
 
@@ -408,8 +407,8 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4),
       x1, x2, x3, x4);
 }
 
@@ -426,15 +425,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::placeholders::_1),
       x4, delta);
 }
 
@@ -442,8 +441,8 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4),
       x1, x2, x3, x4);
 }
 
@@ -461,15 +460,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::cref(x5)),
       x1, delta);
 }
 
@@ -477,9 +476,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -497,15 +496,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
+                  std::cref(x4), std::cref(x5)),
       x2, delta);
 }
 
@@ -513,9 +512,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -533,15 +532,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
+                  std::cref(x4), std::cref(x5)),
       x3, delta);
 }
 
@@ -549,9 +548,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -569,15 +568,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::placeholders::_1, std::cref(x5)),
       x4, delta);
 }
 
@@ -585,9 +584,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -605,15 +604,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::placeholders::_1),
       x5, delta);
 }
 
@@ -621,9 +620,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -642,15 +641,15 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
       x1, delta);
 }
 
@@ -658,9 +657,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -679,15 +678,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
       x2, delta);
 }
 
@@ -695,9 +694,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -716,15 +715,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
       x3, delta);
 }
 
@@ -732,9 +731,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -753,15 +752,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::placeholders::_1, std::cref(x5), std::cref(x6)),
       x4, delta);
 }
 
@@ -769,9 +768,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -790,15 +789,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "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)),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::placeholders::_1, std::cref(x6)),
       x5, delta);
 }
 
@@ -806,9 +805,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -827,7 +826,7 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
 typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
     double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
@@ -835,8 +834,8 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
   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),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::cref(x5), std::placeholders::_1),
       x6, delta);
 }
 
@@ -844,9 +843,9 @@ 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),
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -859,22 +858,22 @@ inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*
  * @return n*n Hessian matrix computed via central differencing
  */
 template<class X>
-inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
+inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(std::function<double(const X&)> f, const X& x,
     double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
       "Template argument X must be a manifold type.");
   typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
-  typedef boost::function<double(const X&)> F;
-  typedef boost::function<VectorD(F, const X&, double)> G;
+  typedef std::function<double(const X&)> F;
+  typedef std::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);
+      std::bind(ng, f, std::placeholders::_1, delta), x, delta);
 }
 
 template<class X>
 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
     1e-5) {
-  return numericalHessian(boost::function<double(const X&)>(f), x, delta);
+  return numericalHessian(std::function<double(const X&)>(f), x, delta);
 }
 
 /** Helper class that computes the derivative of f w.r.t. x1, centered about
@@ -882,86 +881,86 @@ inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f
  */
 template<class X1, class X2>
 class G_x1 {
-  const boost::function<double(const X1&, const X2&)>& f_;
+  const std::function<double(const X1&, const X2&)>& f_;
   X1 x1_;
   double delta_;
 public:
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
 
-  G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
+  G_x1(const std::function<double(const X1&, const X2&)>& f, const X1& x1,
       double delta) :
       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_);
+        std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_);
   }
 };
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
-    boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
+    std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
   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)),
+      std::function<Vector(const X2&)>(
+          std::bind<Vector>(std::ref(g_x1), std::placeholders::_1)),
       x2, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
-  return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
+  return numericalHessian212(std::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
-    boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
+    std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
 
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
 
-  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
+  Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(
-      boost::bind(f, boost::placeholders::_1, boost::cref(x2)));
+  std::function<double(const X1&)> f2(
+      std::bind(f, std::placeholders::_1, std::cref(x2)));
 
   return numericalDerivative11<Vector, X1>(
-      boost::function<Vector(const X1&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      std::function<Vector(const X1&)>(
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x1, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
-  return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
+  return numericalHessian211(std::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
-    boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
+    std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
-  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
+  Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(
-      boost::bind(f, boost::cref(x1), boost::placeholders::_1));
+  std::function<double(const X2&)> f2(
+      std::bind(f, std::cref(x1), std::placeholders::_1));
 
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      std::function<Vector(const X2&)>(
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x2, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
-  return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
+  return numericalHessian222(std::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
 }
 
@@ -971,17 +970,17 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(doubl
 /* **************************************************************** */
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
-  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
+  Vector (*numGrad)(std::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)));
+  std::function<double(const X1&)> f2(std::bind(
+      f, std::placeholders::_1, std::cref(x2), std::cref(x3)));
 
   return numericalDerivative11<Vector, X1>(
-      boost::function<Vector(const X1&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      std::function<Vector(const X1&)>(
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x1, delta);
 }
 
@@ -989,24 +988,24 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian311(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
-  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
+  Vector (*numGrad)(std::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)));
+  std::function<double(const X2&)> f2(std::bind(
+      f, std::cref(x1), std::placeholders::_1, std::cref(x3)));
 
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      std::function<Vector(const X2&)>(
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x2, delta);
 }
 
@@ -1014,24 +1013,24 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian322(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X3>::type Vector;
-  Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
+  Vector (*numGrad)(std::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));
+  std::function<double(const X3&)> f2(std::bind(
+      f, std::cref(x1), std::cref(x2), std::placeholders::_1));
 
   return numericalDerivative11<Vector, X3>(
-      boost::function<Vector(const X3&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+      std::function<Vector(const X3&)>(
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x3, delta);
 }
 
@@ -1039,41 +1038,41 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian333(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::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))),
+      std::function<double(const X1&, const X2&)>(
+          std::bind(f, std::placeholders::_1, std::placeholders::_2,
+                      std::cref(x3))),
       x1, x2, delta);
 }
 
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::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)),
+      std::function<double(const X1&, const X3&)>(
+          std::bind(f, std::placeholders::_1, std::cref(x2),
+                      std::placeholders::_2)),
       x1, x3, delta);
 }
 
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::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)),
+      std::function<double(const X2&, const X3&)>(
+          std::bind(f, std::cref(x1), std::placeholders::_1,
+                      std::placeholders::_2)),
       x2, x3, delta);
 }
 
@@ -1082,7 +1081,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian312(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
@@ -1090,7 +1089,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian313(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
@@ -1098,7 +1097,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian323(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 80b226e3a4..439889ebfc 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -450,7 +450,7 @@ namespace gtsam {
   template<typename L, typename Y>
   template<typename M, typename X>
   DecisionTree<L, Y>::DecisionTree(const DecisionTree<M, X>& other,
-      const std::map<M, L>& map, boost::function<Y(const X&)> op)  {
+      const std::map<M, L>& map, std::function<Y(const X&)> op)  {
     root_ = convert(other.root_, map, op);
   }
 
@@ -568,7 +568,7 @@ namespace gtsam {
   template<typename M, typename X>
   typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::convert(
       const typename DecisionTree<M, X>::NodePtr& f, const std::map<M, L>& map,
-      boost::function<Y(const X&)> op) {
+      std::function<Y(const X&)> op) {
 
     typedef DecisionTree<M, X> MX;
     typedef typename MX::Leaf MXLeaf;
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index ecf03ad5d0..0ee0b8be0c 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -20,10 +20,12 @@
 #pragma once
 
 #include <gtsam/discrete/Assignment.h>
+
 #include <boost/function.hpp>
+#include <functional>
 #include <iostream>
-#include <vector>
 #include <map>
+#include <vector>
 
 namespace gtsam {
 
@@ -38,8 +40,8 @@ namespace gtsam {
   public:
 
     /** Handy typedefs for unary and binary function types */
-    typedef boost::function<Y(const Y&)> Unary;
-    typedef boost::function<Y(const Y&, const Y&)> Binary;
+    typedef std::function<Y(const Y&)> Unary;
+    typedef std::function<Y(const Y&, const Y&)> Binary;
 
     /** A label annotated with cardinality */
     typedef std::pair<L,size_t> LabelC;
@@ -107,7 +109,7 @@ namespace gtsam {
     /** Convert to a different type */
     template<typename M, typename X> NodePtr
     convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
-        L>& map, boost::function<Y(const X&)> op);
+        L>& map, std::function<Y(const X&)> op);
 
     /** Default constructor */
     DecisionTree();
@@ -143,7 +145,7 @@ namespace gtsam {
     /** Convert from a different type */
     template<typename M, typename X>
     DecisionTree(const DecisionTree<M, X>& other,
-        const std::map<M, L>& map, boost::function<Y(const X&)> op);
+        const std::map<M, L>& map, std::function<Y(const X&)> op);
 
     /// @}
     /// @name Testable
diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h
index 0b81ff0f92..35578ffe0c 100644
--- a/gtsam/geometry/Cyclic.h
+++ b/gtsam/geometry/Cyclic.h
@@ -17,7 +17,9 @@
 
 #include <gtsam/base/Group.h>
 #include <gtsam/base/Testable.h>
-#include <iostream> // for cout :-(
+
+#include <cassert>
+#include <iostream>  // for cout :-(
 
 namespace gtsam {
 
diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h
index 316ca8ee4d..edc4883e71 100644
--- a/gtsam/inference/EliminateableFactorGraph.h
+++ b/gtsam/inference/EliminateableFactorGraph.h
@@ -19,7 +19,7 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
-#include <boost/function.hpp>
+#include <functional>
 #include <boost/variant.hpp>
 #include <boost/optional.hpp>
 
@@ -86,7 +86,7 @@ namespace gtsam {
     typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
 
     /// The function type that does a single dense elimination step on a subgraph.
-    typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
+    typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
 
     /// Typedef for an optional variable index as an argument to elimination functions
     typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp
index 17ff6fd22e..6a1739e201 100644
--- a/gtsam/inference/LabeledSymbol.cpp
+++ b/gtsam/inference/LabeledSymbol.cpp
@@ -15,14 +15,11 @@
  * @author: Alex Cunningham
  */
 
-#include <iostream>
+#include <gtsam/inference/LabeledSymbol.h>
 
-#include <boost/bind/bind.hpp>
 #include <boost/format.hpp>
-
 #include <boost/lexical_cast.hpp>
-
-#include <gtsam/inference/LabeledSymbol.h>
+#include <iostream>
 
 namespace gtsam {
 
@@ -109,17 +106,37 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const {
 /* ************************************************************************* */
 static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);}
 
-boost::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
-  return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c;
+std::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
+  // Use lambda function to check equality
+  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
+  return std::bind(
+      equals,
+      std::bind(&LabeledSymbol::chr, std::bind(make, std::placeholders::_1)),
+      c);
 }
 
-boost::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
-  return boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label;
+std::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
+  // Use lambda function to check equality
+  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
+  return std::bind(
+      equals,
+      std::bind(&LabeledSymbol::label, std::bind(make, std::placeholders::_1)),
+      label);
 }
 
-boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
-  return boost::bind(&LabeledSymbol::chr,   boost::bind(make, boost::placeholders::_1)) == c &&
-      boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label;
+std::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
+  // Use lambda functions for && and ==
+  auto logical_and = [](bool is_type, bool is_label) { return is_type == is_label; };
+  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
+  return std::bind(logical_and,
+                   std::bind(equals,
+                             std::bind(&LabeledSymbol::chr,
+                                       std::bind(make, std::placeholders::_1)),
+                             c),
+                   std::bind(equals,
+                             std::bind(&LabeledSymbol::label,
+                                       std::bind(make, std::placeholders::_1)),
+                             label));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h
index 15a2a25014..5aee4a0e23 100644
--- a/gtsam/inference/LabeledSymbol.h
+++ b/gtsam/inference/LabeledSymbol.h
@@ -19,8 +19,8 @@
 
 #pragma once
 
+#include <functional>
 #include <gtsam/inference/Symbol.h>
-#include <boost/function.hpp>
 
 namespace gtsam {
 
@@ -89,13 +89,13 @@ class GTSAM_EXPORT LabeledSymbol {
    */
 
   // Checks only the type
-  static boost::function<bool(gtsam::Key)> TypeTest(unsigned char c);
+  static std::function<bool(gtsam::Key)> TypeTest(unsigned char c);
 
   // Checks only the robot ID (label_)
-  static boost::function<bool(gtsam::Key)> LabelTest(unsigned char label);
+  static std::function<bool(gtsam::Key)> LabelTest(unsigned char label);
 
   // Checks both type and the robot ID
-  static boost::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
+  static std::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
 
   // Converts to upper/lower versions of labels
   LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }
diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp
index 61e397f403..925ba9ce3f 100644
--- a/gtsam/inference/Symbol.cpp
+++ b/gtsam/inference/Symbol.cpp
@@ -62,8 +62,11 @@ Symbol::operator std::string() const {
 
 static Symbol make(gtsam::Key key) { return Symbol(key);}
 
-boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
-  return boost::bind(&Symbol::chr, boost::bind(make, boost::placeholders::_1)) == c;
+std::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
+  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
+  return std::bind(
+      equals, std::bind(&Symbol::chr, std::bind(make, std::placeholders::_1)),
+      c);
 }
 
 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) {
diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h
index 89fb0d161e..017d5134af 100644
--- a/gtsam/inference/Symbol.h
+++ b/gtsam/inference/Symbol.h
@@ -18,11 +18,12 @@
 
 #pragma once
 
-#include <gtsam/inference/Key.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/inference/Key.h>
+
 #include <boost/serialization/nvp.hpp>
-#include <boost/function.hpp>
 #include <cstdint>
+#include <functional>
 
 namespace gtsam {
 
@@ -114,7 +115,7 @@ class GTSAM_EXPORT Symbol {
    * Values::filter() function to retrieve all key-value pairs with the
    * requested character.
    */
-  static boost::function<bool(Key)> ChrTest(unsigned char c);
+  static std::function<bool(Key)> ChrTest(unsigned char c);
 
   /// Output stream operator that can be used with key_formatter (see Key.h).
   GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const Symbol &);
diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index f72799c0aa..6a2514b358 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -38,8 +38,8 @@ namespace gtsam {
   {
     // Merge using predicate for comparing first of pair
     merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()),
-      boost::bind(&less<Key>::operator(), less<Key>(), boost::bind(&KeyValuePair::first, boost::placeholders::_1),
-          boost::bind(&KeyValuePair::first, boost::placeholders::_2)));
+      std::bind(&less<Key>::operator(), less<Key>(), std::bind(&KeyValuePair::first, std::placeholders::_1),
+          std::bind(&KeyValuePair::first, std::placeholders::_2)));
     if(size() != first.size() + second.size())
       throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
   }
diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h
index 85f2f14bc3..cf2462dfc8 100644
--- a/gtsam/nonlinear/Expression-inl.h
+++ b/gtsam/nonlinear/Expression-inl.h
@@ -83,8 +83,8 @@ 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,
-                boost::placeholders::_1, boost::placeholders::_2),
+        new internal::UnaryExpression<T, A>(std::bind(method,
+                std::placeholders::_1, std::placeholders::_2),
             expression)) {
 }
 
@@ -97,9 +97,9 @@ Expression<T>::Expression(const Expression<A1>& expression1,
     const Expression<A2>& expression2) :
     root_(
         new internal::BinaryExpression<T, A1, A2>(
-            boost::bind(method, boost::placeholders::_1,
-                boost::placeholders::_2, boost::placeholders::_3,
-                boost::placeholders::_4),
+            std::bind(method, std::placeholders::_1,
+                std::placeholders::_2, std::placeholders::_3,
+                std::placeholders::_4),
             expression1, expression2)) {
 }
 
@@ -114,10 +114,10 @@ Expression<T>::Expression(const Expression<A1>& expression1,
     const Expression<A2>& expression2, const Expression<A3>& expression3) :
     root_(
         new internal::TernaryExpression<T, A1, A2, A3>(
-            boost::bind(method, boost::placeholders::_1,
-                boost::placeholders::_2, boost::placeholders::_3,
-                boost::placeholders::_4, boost::placeholders::_5,
-                boost::placeholders::_6),
+            std::bind(method, std::placeholders::_1,
+                std::placeholders::_2, std::placeholders::_3,
+                std::placeholders::_4, std::placeholders::_5,
+                std::placeholders::_6),
             expression1, expression2, expression3)) {
 }
 
@@ -255,9 +255,9 @@ template<typename T>
 Expression<T> operator*(const Expression<T>& expression1,
     const Expression<T>& expression2) {
   return Expression<T>(
-      boost::bind(internal::apply_compose<T>(), boost::placeholders::_1,
-          boost::placeholders::_2, boost::placeholders::_3,
-          boost::placeholders::_4),
+      std::bind(internal::apply_compose<T>(), std::placeholders::_1,
+          std::placeholders::_2, std::placeholders::_3,
+          std::placeholders::_4),
       expression1, expression2);
 }
 
diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h
index bda4575959..eb828760d4 100644
--- a/gtsam/nonlinear/Expression.h
+++ b/gtsam/nonlinear/Expression.h
@@ -68,20 +68,20 @@ class Expression {
   //   Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
   template<class A1>
   struct UnaryFunction {
-    typedef boost::function<
+    typedef std::function<
         T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
   };
 
   template<class A1, class A2>
   struct BinaryFunction {
-    typedef boost::function<
+    typedef std::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<
+    typedef std::function<
         T(const A1&, const A2&, const A3&,
             typename MakeOptionalJacobian<T, A1>::type,
             typename MakeOptionalJacobian<T, A2>::type,
@@ -239,7 +239,7 @@ class BinarySumExpression : public Expression<T> {
  */
 template <typename T, typename A>
 Expression<T> linearExpression(
-    const boost::function<T(A)>& f, const Expression<A>& expression,
+    const std::function<T(A)>& f, const Expression<A>& expression,
     const Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension>& dTdA) {
   // Use lambda to endow f with a linear Jacobian
   typename Expression<T>::template UnaryFunction<A>::type g =
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index f10ba93aee..6b99721565 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -69,7 +69,7 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
   /**
    * Function that compares two values
    */
-  typedef boost::function<bool(const T&, const T&)> CompareFunction;
+  typedef std::function<bool(const T&, const T&)> CompareFunction;
   CompareFunction compare_;
 //  bool (*compare_)(const T& a, const T& b);
 
@@ -87,8 +87,8 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
    * Constructor - forces exact evaluation
    */
   NonlinearEquality(Key j, const T& feasible,
-      const CompareFunction &_compare = boost::bind(traits<T>::Equals,
-          boost::placeholders::_1, boost::placeholders::_2, 1e-9)) :
+      const CompareFunction &_compare = std::bind(traits<T>::Equals,
+          std::placeholders::_1, std::placeholders::_2, 1e-9)) :
       Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
           j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
       compare_(_compare) {
@@ -98,8 +98,8 @@ class NonlinearEquality: public NoiseModelFactor1<VALUE> {
    * Constructor - allows inexact evaluation
    */
   NonlinearEquality(Key j, const T& feasible, double error_gain,
-      const CompareFunction &_compare = boost::bind(traits<T>::Equals,
-          boost::placeholders::_1, boost::placeholders::_2, 1e-9)) :
+      const CompareFunction &_compare = std::bind(traits<T>::Equals,
+          std::placeholders::_1, std::placeholders::_2, 1e-9)) :
       Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
           j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
       compare_(_compare) {
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index eca7416c94..8ebdcab17c 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -103,7 +103,7 @@ namespace gtsam {
       boost::transform_iterator<
       KeyValuePair(*)(Values::KeyValuePair),
       boost::filter_iterator<
-      boost::function<bool(const Values::ConstKeyValuePair&)>,
+      std::function<bool(const Values::ConstKeyValuePair&)>,
       Values::iterator> >
       iterator;
 
@@ -113,7 +113,7 @@ namespace gtsam {
       boost::transform_iterator<
       ConstKeyValuePair(*)(Values::ConstKeyValuePair),
       boost::filter_iterator<
-      boost::function<bool(const Values::ConstKeyValuePair&)>,
+      std::function<bool(const Values::ConstKeyValuePair&)>,
       Values::const_iterator> >
       const_const_iterator;
 
@@ -134,7 +134,7 @@ namespace gtsam {
 
   private:
     Filtered(
-        const boost::function<bool(const Values::ConstKeyValuePair&)>& filter,
+        const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
         Values& values) :
         begin_(
             boost::make_transform_iterator(
@@ -205,7 +205,7 @@ namespace gtsam {
     const_iterator begin_;
     const_iterator end_;
     ConstFiltered(
-        const boost::function<bool(const Values::ConstKeyValuePair&)>& filter,
+        const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
         const Values& values) {
       // We remove the const from values to create a non-const Filtered
       // view, then pull the const_iterators out of it.
@@ -236,35 +236,35 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Values::Filtered<Value>
-  inline Values::filter(const boost::function<bool(Key)>& filterFcn) {
+  inline Values::filter(const std::function<bool(Key)>& filterFcn) {
     return filter<Value>(filterFcn);
   }
 
   /* ************************************************************************* */
   template<class ValueType>
   Values::Filtered<ValueType>
-  Values::filter(const boost::function<bool(Key)>& filterFcn) {
-    return Filtered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn,
-      boost::placeholders::_1), *this);
+  Values::filter(const std::function<bool(Key)>& filterFcn) {
+    return Filtered<ValueType>(std::bind(&filterHelper<ValueType>, filterFcn,
+      std::placeholders::_1), *this);
   }
 
   /* ************************************************************************* */
   Values::ConstFiltered<Value>
-  inline Values::filter(const boost::function<bool(Key)>& filterFcn) const {
+  inline Values::filter(const std::function<bool(Key)>& filterFcn) const {
     return filter<Value>(filterFcn);
   }
 
   /* ************************************************************************* */
   template<class ValueType>
   Values::ConstFiltered<ValueType>
-  Values::filter(const boost::function<bool(Key)>& filterFcn) const {
-    return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>,
-      filterFcn, boost::placeholders::_1), *this);
+  Values::filter(const std::function<bool(Key)>& filterFcn) const {
+    return ConstFiltered<ValueType>(std::bind(&filterHelper<ValueType>,
+      filterFcn, std::placeholders::_1), *this);
   }
 
   /* ************************************************************************* */
    template<>
-   inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter,
+   inline bool Values::filterHelper<Value>(const std::function<bool(Key)> filter,
        const ConstKeyValuePair& key_value) {
      // Filter and check the type
      return filter(key_value.key);
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 3b447ede11..33e9e7d82d 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -108,19 +108,19 @@ namespace gtsam {
 
     /// Mutable forward iterator, with value type KeyValuePair
     typedef boost::transform_iterator<
-        boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
+        std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::iterator> iterator;
 
     /// Const forward iterator, with value type ConstKeyValuePair
     typedef boost::transform_iterator<
-        boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_iterator> const_iterator;
+        std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_iterator> const_iterator;
 
     /// Mutable reverse iterator, with value type KeyValuePair
     typedef boost::transform_iterator<
-        boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::reverse_iterator> reverse_iterator;
+        std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::reverse_iterator> reverse_iterator;
 
     /// Const reverse iterator, with value type ConstKeyValuePair
     typedef boost::transform_iterator<
-        boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
+        std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
 
     typedef KeyValuePair value_type;
 
@@ -321,7 +321,7 @@ namespace gtsam {
      * the original Values class.
      */
     Filtered<Value>
-    filter(const boost::function<bool(Key)>& filterFcn);
+    filter(const std::function<bool(Key)>& filterFcn);
 
     /**
      * Return a filtered view of this Values class, without copying any data.
@@ -344,7 +344,7 @@ namespace gtsam {
      */
     template<class ValueType>
     Filtered<ValueType>
-    filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
+    filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
 
     /**
      * Return a filtered view of this Values class, without copying any data.
@@ -360,7 +360,7 @@ namespace gtsam {
      * the original Values class.
      */
     ConstFiltered<Value>
-    filter(const boost::function<bool(Key)>& filterFcn) const;
+    filter(const std::function<bool(Key)>& filterFcn) const;
 
     /**
      * Return a filtered view of this Values class, without copying any data.
@@ -382,7 +382,7 @@ namespace gtsam {
      */
     template<class ValueType>
     ConstFiltered<ValueType>
-    filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
+    filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
 
     // Count values of given type \c ValueType
     template<class ValueType>
@@ -399,7 +399,7 @@ namespace gtsam {
     // Filters based on ValueType (if not Value) and also based on the user-
     // supplied \c filter function.
     template<class ValueType>
-    static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
+    static bool filterHelper(const std::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
       BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
       // Filter and check the type
       return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h
index 2a1a07fb00..f6afb287e6 100644
--- a/gtsam/nonlinear/internal/ExpressionNode.h
+++ b/gtsam/nonlinear/internal/ExpressionNode.h
@@ -57,7 +57,7 @@ T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
  * 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.
+ * are passed by value. This is the same way std::function works.
  * http://loki-lib.sourceforge.net/html/a00652.html
  */
 template<class T>
diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h
index 96424324bf..9d854a169f 100644
--- a/gtsam_unstable/base/BTree.h
+++ b/gtsam_unstable/base/BTree.h
@@ -20,7 +20,7 @@
 #include <stack>
 #include <sstream>
 #include <boost/shared_ptr.hpp>
-#include <boost/function.hpp>
+#include <functional>
 
 namespace gtsam {
 
@@ -260,7 +260,7 @@ namespace gtsam {
     }
 
     /** iterate over tree */
-    void iter(boost::function<void(const KEY&, const VALUE&)> f) const {
+    void iter(std::function<void(const KEY&, const VALUE&)> f) const {
       if (!root_) return;
       left().iter(f);
       f(key(), value());
@@ -269,7 +269,7 @@ namespace gtsam {
 
     /** map key-values in tree over function f that computes a new value */
     template<class TO>
-    BTree<KEY, TO> map(boost::function<TO(const KEY&, const VALUE&)> f) const {
+    BTree<KEY, TO> map(std::function<TO(const KEY&, const VALUE&)> f) const {
       if (empty()) return BTree<KEY, TO> ();
       std::pair<KEY, TO> xd(key(), f(key(), value()));
       return BTree<KEY, TO> (left().map(f), xd, right().map(f));
@@ -282,7 +282,7 @@ namespace gtsam {
      * The associated values are passed to [f] in reverse sort order
      */
     template<class ACC>
-    ACC fold(boost::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
+    ACC fold(std::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
         const ACC& a) const {
       if (!root_) return a;
       ACC ar = right().fold(f, a); // fold over right subtree
diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h
index c7b8cd4177..4ad7d3ea87 100644
--- a/gtsam_unstable/base/DSF.h
+++ b/gtsam_unstable/base/DSF.h
@@ -122,7 +122,7 @@ class DSF: protected BTree<KEY, KEY> {
   }
 
   // maps f over all keys, must be invertible
-  DSF map(boost::function<KEY(const KEY&)> func) const {
+  DSF map(std::function<KEY(const KEY&)> func) const {
     DSF t;
     for(const KeyLabel& pair: (Tree)*this)
       t = t.add(func(pair.first), func(pair.second));
diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h
index 337f2bc430..9f00f81d66 100644
--- a/gtsam_unstable/dynamics/FullIMUFactor.h
+++ b/gtsam_unstable/dynamics/FullIMUFactor.h
@@ -91,9 +91,9 @@ class FullIMUFactor : public NoiseModelFactor2<POSE, POSE> {
     z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
     z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang
     if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
-        boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5);
+        std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5);
     if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
-        boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5);
+        std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5);
     return z - predict_proxy(x1, x2, dt_);
   }
 
diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h
index e082dee82b..9a742b4f0e 100644
--- a/gtsam_unstable/dynamics/IMUFactor.h
+++ b/gtsam_unstable/dynamics/IMUFactor.h
@@ -81,9 +81,9 @@ class IMUFactor : public NoiseModelFactor2<POSE, POSE> {
       boost::optional<Matrix&> H2 = boost::none) const override {
     const Vector6 meas = z();
     if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
-        boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5);
+        std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5);
     if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
-        boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5);
+        std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5);
     return predict_proxy(x1, x2, dt_, meas);
   }
 
diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h
index 976f448c5e..bf3b95c0f2 100644
--- a/gtsam_unstable/dynamics/SimpleHelicopter.h
+++ b/gtsam_unstable/dynamics/SimpleHelicopter.h
@@ -166,24 +166,24 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector
       boost::optional<Matrix&> H3 = boost::none) const {
     if (H1) {
       (*H1) = numericalDerivative31(
-          boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
-              boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
+          std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
+              std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
           ),
           xik, xik_1, gk, 1e-5
       );
     }
     if (H2) {
       (*H2) = numericalDerivative32(
-          boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
-              boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
+          std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
+              std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
           ),
           xik, xik_1, gk, 1e-5
       );
     }
     if (H3) {
       (*H3) = numericalDerivative33(
-          boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
-              boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
+          std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
+              std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
           ),
           xik, xik_1, gk, 1e-5
       );
diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h
index 840b7bba72..d24d06e798 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint.h
@@ -86,11 +86,11 @@ class VelocityConstraint : public gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> {
       boost::optional<gtsam::Matrix&> H1=boost::none,
       boost::optional<gtsam::Matrix&> H2=boost::none) const override {
     if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
-        boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1,
-            boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);
+        std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1,
+            std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);
     if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
-        boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1,
-            boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);
+        std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1,
+            std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);
     return evaluateError_(x1, x2, dt_, integration_mode_);
   }
 
diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp
index 88697e075d..c755f2451f 100644
--- a/gtsam_unstable/linear/QPSParser.cpp
+++ b/gtsam_unstable/linear/QPSParser.cpp
@@ -24,7 +24,6 @@
 #include <gtsam_unstable/linear/QPSParser.h>
 #include <gtsam_unstable/linear/QPSParserException.h>
 
-#include <boost/bind/bind.hpp>
 #include <boost/fusion/include/vector.hpp>
 #include <boost/fusion/sequence.hpp>
 #include <boost/lambda/lambda.hpp>
@@ -40,7 +39,7 @@
 #include <vector>
 
 using boost::fusion::at_c;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 
 namespace bf = boost::fusion;
@@ -412,50 +411,50 @@ typedef qi::grammar<boost::spirit::basic_istream_iterator<char>> base_grammar;
 struct QPSParser::MPSGrammar : base_grammar {
   typedef std::vector<char> Chars;
   QPSVisitor *rqp_;
-  boost::function<void(bf::vector<Chars, Chars, Chars> const &)> setName;
-  boost::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)>
+  std::function<void(bf::vector<Chars, Chars, Chars> const &)> setName;
+  std::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)>
       addRow;
-  boost::function<void(
+  std::function<void(
       bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
       rhsSingle;
-  boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
+  std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
                                   Chars, Chars, Chars, double>)>
       rhsDouble;
-  boost::function<void(
+  std::function<void(
       bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
       rangeSingle;
-  boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
+  std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
                                   Chars, Chars, Chars, double>)>
       rangeDouble;
-  boost::function<void(
+  std::function<void(
       bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars>)>
       colSingle;
-  boost::function<void(bf::vector<Chars, Chars, Chars, Chars, double, Chars,
+  std::function<void(bf::vector<Chars, Chars, Chars, Chars, double, Chars,
                                   Chars, Chars, double> const &)>
       colDouble;
-  boost::function<void(
+  std::function<void(
       bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
       addQuadTerm;
-  boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars,
+  std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars,
                                   Chars, double> const &)>
       addBound;
-  boost::function<void(
+  std::function<void(
       bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)>
       addFreeBound;
   MPSGrammar(QPSVisitor *rqp)
       : base_grammar(start),
         rqp_(rqp),
-        setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)),
-        addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)),
-        rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)),
-        rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)),
-        rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)),
-        rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)),
-        colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)),
-        colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)),
-        addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)),
-        addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)),
-        addFreeBound(boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) {
+        setName(std::bind(&QPSVisitor::setName, rqp, std::placeholders::_1)),
+        addRow(std::bind(&QPSVisitor::addRow, rqp, std::placeholders::_1)),
+        rhsSingle(std::bind(&QPSVisitor::addRHS, rqp, std::placeholders::_1)),
+        rhsDouble(std::bind(&QPSVisitor::addRHSDouble, rqp, std::placeholders::_1)),
+        rangeSingle(std::bind(&QPSVisitor::addRangeSingle, rqp, std::placeholders::_1)),
+        rangeDouble(std::bind(&QPSVisitor::addRangeDouble, rqp, std::placeholders::_1)),
+        colSingle(std::bind(&QPSVisitor::addColumn, rqp, std::placeholders::_1)),
+        colDouble(std::bind(&QPSVisitor::addColumnDouble, rqp, std::placeholders::_1)),
+        addQuadTerm(std::bind(&QPSVisitor::addQuadTerm, rqp, std::placeholders::_1)),
+        addBound(std::bind(&QPSVisitor::addBound, rqp, std::placeholders::_1)),
+        addFreeBound(std::bind(&QPSVisitor::addFreeBound, rqp, std::placeholders::_1)) {
     using namespace boost::spirit;
     using namespace boost::spirit::qi;
     character = lexeme[alnum | '_' | '-' | '.'];
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
index a6638c1d71..cabbfdbe85 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -309,12 +309,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     // 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),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, std::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),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
           Pose1);
       *H1 = stack(2, &H1_Pose, &H1_Vel);
     }
@@ -323,12 +323,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     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),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, Pose1, std::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),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
           Vel1);
       *H2 = stack(2, &H2_Pose, &H2_Vel);
     }
@@ -336,12 +336,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     // 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),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
           Bias1);
       Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(
-          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
-                      this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
           Bias1);
       *H3 = stack(2, &H3_Pose, &H3_Vel);
     }
@@ -349,12 +349,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     // 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),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
           Pose2);
       Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(
-          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
-                      this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
           Pose2);
       *H4 = stack(2, &H4_Pose, &H4_Vel);
     }
@@ -363,12 +363,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     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),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
+                      this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
           Vel2);
       Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(
-          boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
-                      this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
+          std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
+                      this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
           Vel2);
       *H5 = stack(2, &H5_Pose, &H5_Vel);
     }
@@ -469,43 +469,43 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
     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),
+        std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
+                    std::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),
+        std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
+                    delta_pos_in_t0, std::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,
+        std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
+                    msr_acc_t, msr_dt, delta_angles, std::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,
+        std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
+                    msr_acc_t, msr_dt, std::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,
+        std::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),
+                    std::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,
+        std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
+                    msr_dt, std::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,
+        std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
                     msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor,
-                    boost::placeholders::_1),
+                    std::placeholders::_1),
         Bias_t0);
     Matrix H_angles_pos = Z_3x3;
     Matrix H_angles_vel = Z_3x3;
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
index 40dc81c9ab..0e2aebd7fe 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
@@ -278,29 +278,29 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4<POSE, V
     // TODO: Write analytical derivative calculations
     // Jacobian w.r.t. Pose1
     if (H1){
-      Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
-      Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
+      Matrix H1_Pose = numericalDerivative11<POSE, POSE>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
+      Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
       *H1 = stack(2, &H1_Pose, &H1_Vel);
     }
 
     // Jacobian w.r.t. Vel1
     if (H2){
-      Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
-      Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
+      Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
+      Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
       *H2 = stack(2, &H2_Pose, &H2_Vel);
     }
 
     // Jacobian w.r.t. Pose2
     if (H3){
-      Matrix H3_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
-      Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
+      Matrix H3_Pose = numericalDerivative11<POSE, POSE>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
+      Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
       *H3 = stack(2, &H3_Pose, &H3_Vel);
     }
 
     // Jacobian w.r.t. Vel2
     if (H4){
-      Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
-      Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
+      Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
+      Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
       *H4 = stack(2, &H4_Pose, &H4_Vel);
     }
 
@@ -372,15 +372,15 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4<POSE, V
     Matrix Z_3x3 = Z_3x3;
     Matrix I_3x3 = I_3x3;
 
-    Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
-    Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
+    Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
+    Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
     Matrix H_pos_angles = Z_3x3;
 
-    Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
-    Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
+    Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
+    Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
     Matrix H_vel_pos = Z_3x3;
 
-    Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
+    Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
     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 3f526e934f..0828fbd088 100644
--- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
+++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
@@ -236,12 +236,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
     // 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),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, std::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),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
           Pose1);
       *H1 = stack(2, &H1_Pose, &H1_Vel);
     }
@@ -250,12 +250,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
     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),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, Pose1, std::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),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
           Vel1);
       *H2 = stack(2, &H2_Pose, &H2_Vel);
     }
@@ -263,12 +263,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
     // 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),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, Pose1, Vel1, std::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),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
           Bias1);
       *H3 = stack(2, &H3_Pose, &H3_Vel);
     }
@@ -276,12 +276,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
     // 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),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
           Pose2);
       Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
-          boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
-                      this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
           Pose2);
       *H4 = stack(2, &H4_Pose, &H4_Vel);
     }
@@ -290,12 +290,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
     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),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
+                      this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
           Vel2);
       Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
-          boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
-                      this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
+          std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
+                      this, Pose1, Vel1, Bias1, Pose2, std::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 a9dcb8ceff..40c09416c8 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant1.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h
@@ -108,14 +108,14 @@ class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
-          boost::bind(&InvDepthFactorVariant1::inverseDepthError, this,
-                      boost::placeholders::_1, landmark),
+          std::bind(&InvDepthFactorVariant1::inverseDepthError, this,
+                      std::placeholders::_1, landmark),
           pose);
     }
     if (H2) {
       (*H2) = numericalDerivative11<Vector, Vector6>(
-          boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
-              boost::placeholders::_1), landmark);
+          std::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
+              std::placeholders::_1), landmark);
     }
 
     return inverseDepthError(pose, landmark);
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h
index d1fc92b90e..b1169580e3 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant2.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h
@@ -111,13 +111,13 @@ class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
-          boost::bind(&InvDepthFactorVariant2::inverseDepthError, this,
-              boost::placeholders::_1, landmark), pose);
+          std::bind(&InvDepthFactorVariant2::inverseDepthError, this,
+              std::placeholders::_1, landmark), pose);
     }
     if (H2) {
       (*H2) = numericalDerivative11<Vector, Vector3>(
-          boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
-              boost::placeholders::_1), landmark);
+          std::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
+              std::placeholders::_1), landmark);
     }
 
     return inverseDepthError(pose, landmark);
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h
index 4595a934b1..98f2db2f37 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant3.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h
@@ -111,14 +111,14 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
 
     if(H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
-          boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this,
-                      boost::placeholders::_1, landmark),
+          std::bind(&InvDepthFactorVariant3a::inverseDepthError, this,
+                      std::placeholders::_1, landmark),
           pose);
     }
     if(H2) {
       (*H2) = numericalDerivative11<Vector, Vector3>(
-          boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose,
-                      boost::placeholders::_1),
+          std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose,
+                      std::placeholders::_1),
           landmark);
     }
 
@@ -238,20 +238,20 @@ class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
 
     if(H1)
       (*H1) = numericalDerivative11<Vector, Pose3>(
-          boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this,
-                      boost::placeholders::_1, pose2, landmark),
+          std::bind(&InvDepthFactorVariant3b::inverseDepthError, this,
+                      std::placeholders::_1, pose2, landmark),
           pose1);
 
     if(H2)
       (*H2) = numericalDerivative11<Vector, Pose3>(
-          boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
-                      boost::placeholders::_1, landmark),
+          std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
+                      std::placeholders::_1, landmark),
           pose2);
 
     if(H3)
       (*H3) = numericalDerivative11<Vector, Vector3>(
-          boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
-                      pose2, boost::placeholders::_1),
+          std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
+                      pose2, std::placeholders::_1),
           landmark);
 
     return inverseDepthError(pose1, pose2, landmark);

From d5890a2d61a7142e745c0227e1abbe67dcc83aab Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 10 Jul 2021 21:03:15 -0400
Subject: [PATCH 42/72] update all the tests

---
 .../tests/testAlgebraicDecisionTree.cpp       |  2 +-
 gtsam/discrete/tests/testDecisionTree.cpp     |  2 +-
 gtsam/geometry/tests/testCalibratedCamera.cpp |  7 +-
 gtsam/geometry/tests/testEssentialMatrix.cpp  | 25 +++---
 gtsam/geometry/tests/testOrientedPlane3.cpp   | 12 +--
 gtsam/geometry/tests/testPinholeCamera.cpp    | 12 +--
 gtsam/geometry/tests/testPinholePose.cpp      |  4 +-
 gtsam/geometry/tests/testPoint3.cpp           | 22 ++---
 gtsam/geometry/tests/testPose3.cpp            | 11 +--
 gtsam/geometry/tests/testSO3.cpp              | 27 +++---
 gtsam/geometry/tests/testSO4.cpp              |  6 +-
 gtsam/geometry/tests/testSOn.cpp              |  4 +-
 gtsam/geometry/tests/testSimilarity3.cpp      | 29 +++----
 gtsam/geometry/tests/testUnit3.cpp            | 42 +++++----
 gtsam/linear/tests/testGaussianBayesNet.cpp   |  6 +-
 gtsam/linear/tests/testGaussianBayesTree.cpp  |  7 +-
 gtsam/navigation/tests/testAHRSFactor.cpp     | 47 +++++-----
 gtsam/navigation/tests/testAttitudeFactor.cpp |  9 +-
 gtsam/navigation/tests/testGPSFactor.cpp      |  6 +-
 gtsam/navigation/tests/testImuBias.cpp        | 13 +--
 gtsam/navigation/tests/testImuFactor.cpp      | 34 ++++----
 gtsam/navigation/tests/testMagFactor.cpp      | 18 ++--
 gtsam/navigation/tests/testMagPoseFactor.cpp  | 22 +++--
 .../tests/testManifoldPreintegration.cpp      | 19 ++--
 gtsam/navigation/tests/testNavState.cpp       | 48 +++++-----
 gtsam/navigation/tests/testScenario.cpp       |  5 +-
 .../tests/testTangentPreintegration.cpp       | 17 ++--
 gtsam/nonlinear/tests/testExpression.cpp      |  2 +-
 gtsam/nonlinear/tests/testValues.cpp          |  6 +-
 gtsam/sam/tests/testRangeFactor.cpp           | 18 ++--
 gtsam/sfm/tests/testTranslationFactor.cpp     |  7 +-
 gtsam/slam/tests/testBetweenFactor.cpp        | 15 ++--
 .../tests/testEssentialMatrixConstraint.cpp   | 17 ++--
 .../slam/tests/testEssentialMatrixFactor.cpp  | 14 ++-
 gtsam/slam/tests/testOrientedPlane3Factor.cpp | 14 +--
 gtsam/slam/tests/testReferenceFrameFactor.cpp | 16 ++--
 gtsam/slam/tests/testRotateFactor.cpp         | 11 ++-
 .../tests/testSmartProjectionPoseFactor.cpp   |  7 +-
 gtsam/slam/tests/testTriangulationFactor.cpp  | 10 ++-
 .../dynamics/tests/testSimpleHelicopter.cpp   | 42 +++++----
 gtsam_unstable/geometry/tests/testEvent.cpp   |  8 +-
 .../slam/tests/testBetweenFactorEM.cpp        |  6 +-
 .../slam/tests/testBiasedGPSFactor.cpp        | 17 ++--
 .../testEquivInertialNavFactor_GlobalVel.cpp  |  2 +-
 .../tests/testGaussMarkov1stOrderFactor.cpp   | 12 +--
 .../testInertialNavFactor_GlobalVelocity.cpp  | 45 +++++-----
 .../tests/testLocalOrientedPlane3Factor.cpp   | 10 +--
 .../slam/tests/testPartialPriorFactor.cpp     | 26 +++---
 .../slam/tests/testPoseBetweenFactor.cpp      | 23 +++--
 .../slam/tests/testPosePriorFactor.cpp        | 13 ++-
 .../slam/tests/testPoseToPointFactor.h        |  2 +-
 .../slam/tests/testProjectionFactorPPP.cpp    | 24 ++---
 .../slam/tests/testProjectionFactorPPPC.cpp   | 22 ++---
 .../tests/testRelativeElevationFactor.cpp     | 87 ++++++++++++-------
 gtsam_unstable/slam/tests/testTSAMFactors.cpp | 27 +++---
 .../testTransformBtwRobotsUnaryFactor.cpp     | 12 ++-
 .../testTransformBtwRobotsUnaryFactorEM.cpp   | 16 ++--
 tests/testExpressionFactor.cpp                | 24 +++--
 tests/testSimulated3D.cpp                     | 24 +++--
 59 files changed, 555 insertions(+), 480 deletions(-)

diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
index 0261ef833f..be720dbca4 100644
--- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
+++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
@@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) {
 }
 
 /** I can't get this to work !
- class Mul: boost::function<double(const double&, const double&)> {
+ class Mul: std::function<double(const double&, const double&)> {
  inline double operator()(const double& a, const double& b) {
  return a * b;
  }
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 71cb4abe39..96f503abc9 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -196,7 +196,7 @@ TEST(DT, conversion)
   map<string, Label> ordering;
   ordering[A] = X;
   ordering[B] = Y;
-  boost::function<bool(const int&)> op = convert;
+  std::function<bool(const int&)> op = convert;
   BDT f2(f1, ordering, op);
   //  f1.print("f1");
   //  f2.print("f2");
diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp
index 1db1926bcd..0fb0754feb 100644
--- a/gtsam/geometry/tests/testCalibratedCamera.cpp
+++ b/gtsam/geometry/tests/testCalibratedCamera.cpp
@@ -20,12 +20,11 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <iostream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -54,8 +53,8 @@ TEST(CalibratedCamera, Create) {
   EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH)));
 
   // Check derivative
-  boost::function<CalibratedCamera(Pose3)> f =  //
-      boost::bind(CalibratedCamera::Create, _1, boost::none);
+  std::function<CalibratedCamera(Pose3)> f =  //
+      std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none);
   Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp
index 7362cf7bf8..ff8c61f355 100644
--- a/gtsam/geometry/tests/testEssentialMatrix.cpp
+++ b/gtsam/geometry/tests/testEssentialMatrix.cpp
@@ -5,16 +5,15 @@
  * @date December 17, 2013
  */
 
-#include <gtsam/geometry/EssentialMatrix.h>
-#include <gtsam/geometry/CalibratedCamera.h>
-#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/EssentialMatrix.h>
 
-#include <boost/bind/bind.hpp>
-#include <CppUnitLite/TestHarness.h>
 #include <sstream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -42,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) {
       1e-8));
 
   Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
-      boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none,
-                  boost::none),
+      std::bind(EssentialMatrix::FromRotationAndDirection,
+                std::placeholders::_1, trueDirection, boost::none, boost::none),
       trueRotation);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
 
   Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(
-      boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none,
-                  boost::none),
-                  trueDirection);
+      std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation,
+                std::placeholders::_1, boost::none, boost::none),
+      trueDirection);
   EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
 }
 
@@ -176,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) {
   Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
   EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
-      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+      std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
   EXPECT(assert_equal(expectedH, actualH, 1e-7));
 }
 
@@ -189,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) {
   Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
   EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
-      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+      std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
   EXPECT(assert_equal(expectedH, actualH, 1e-5));
 }
 
diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp
index 5c7c6142e1..533041a2cf 100644
--- a/gtsam/geometry/tests/testOrientedPlane3.cpp
+++ b/gtsam/geometry/tests/testOrientedPlane3.cpp
@@ -21,10 +21,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/vector.hpp>
-#include <boost/bind/bind.hpp>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 using boost::none;
@@ -138,8 +137,9 @@ TEST(OrientedPlane3, errorVector) {
                       Vector2(actual[0], actual[1])));
   EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
 
-  boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
-      boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
+  std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
+      std::bind(&OrientedPlane3::errorVector, std::placeholders::_1,
+                std::placeholders::_2, boost::none, boost::none);
   expectedH1 = numericalDerivative21(f, plane1, plane2);
   expectedH2 = numericalDerivative22(f, plane1, plane2);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
@@ -150,8 +150,8 @@ TEST(OrientedPlane3, errorVector) {
 TEST(OrientedPlane3, jacobian_retract) {
   OrientedPlane3 plane(-1, 0.1, 0.2, 5);
   Matrix33 H_actual;
-  boost::function<OrientedPlane3(const Vector3&)> f =
-      boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
+  std::function<OrientedPlane3(const Vector3&)> f = std::bind(
+      &OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
   {
       Vector3 v(-0.1, 0.2, 0.3);
       plane.retract(v, H_actual);
diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp
index 92deda6a53..0679a46097 100644
--- a/gtsam/geometry/tests/testPinholeCamera.cpp
+++ b/gtsam/geometry/tests/testPinholeCamera.cpp
@@ -22,13 +22,12 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <cmath>
 #include <iostream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -66,8 +65,9 @@ TEST(PinholeCamera, Create) {
   EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
 
   // Check derivative
-  boost::function<Camera(Pose3,Cal3_S2)> f = //
-      boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
+  std::function<Camera(Pose3, Cal3_S2)> f =  //
+      std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none);
   Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
   EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
   Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@@ -81,8 +81,8 @@ TEST(PinholeCamera, Pose) {
   EXPECT(assert_equal(pose, camera.getPose(actualH)));
 
   // Check derivative
-  boost::function<Pose3(Camera)> f = //
-      boost::bind(&Camera::getPose,_1,boost::none);
+  std::function<Pose3(Camera)> f =  //
+      std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
   Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp
index 1cf2f4a3fd..acfcd9f396 100644
--- a/gtsam/geometry/tests/testPinholePose.cpp
+++ b/gtsam/geometry/tests/testPinholePose.cpp
@@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) {
   EXPECT(assert_equal(pose, camera.getPose(actualH)));
 
   // Check derivative
-  boost::function<Pose3(Camera)> f = //
-      boost::bind(&Camera::getPose,_1,boost::none);
+  std::function<Pose3(Camera)> f = //
+      std::bind(&Camera::getPose,_1,boost::none);
   Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index 79e44c0b35..315391ac89 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -14,14 +14,14 @@
  * @brief  Unit tests for Point3 class
  */
 
-#include <gtsam/geometry/Point3.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Point3.h>
 
-#include <boost/bind/bind.hpp>
-#include <CppUnitLite/TestHarness.h>
+#include <boost/function.hpp>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Point3)
@@ -101,7 +101,7 @@ TEST( Point3, dot) {
 
   // Use numerical derivatives to calculate the expected Jacobians
   Matrix H1, H2;
-  boost::function<double(const Point3&, const Point3&)> f =
+  std::function<double(const Point3&, const Point3&)> f =
       [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); };
   {
     gtsam::dot(p, q, H1, H2);
@@ -123,8 +123,9 @@ TEST( Point3, dot) {
 /* ************************************************************************* */
 TEST(Point3, cross) {
   Matrix aH1, aH2;
-  boost::function<Point3(const Point3&, const Point3&)> f =
-      boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
+  std::function<Point3(const Point3&, const Point3&)> f =
+      std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none);
   const Point3 omega(0, 1, 0), theta(4, 6, 8);
   cross(omega, theta, aH1, aH2);
   EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
@@ -142,8 +143,9 @@ TEST( Point3, cross2) {
 
   // Use numerical derivatives to calculate the expected Jacobians
   Matrix H1, H2;
-  boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2,  //
-                                                                      boost::none, boost::none);
+  std::function<Point3(const Point3&, const Point3&)> f =
+      std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2,  //
+                boost::none, boost::none);
   {
     gtsam::cross(p, q, H1, H2);
     EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
@@ -163,7 +165,7 @@ TEST (Point3, normalize) {
   Point3 expected(point / sqrt(14.0));
   EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<Point3, Point3>(
-      boost::bind(gtsam::normalize, _1, boost::none), point);
+      std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point);
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 9ed76d4a67..11b8791d46 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -22,8 +22,7 @@
 
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
-#include <boost/bind/bind.hpp>
-using namespace boost::placeholders;
+using namespace std::placeholders;
 
 #include <CppUnitLite/TestHarness.h>
 #include <cmath>
@@ -215,7 +214,7 @@ TEST(Pose3, translation) {
   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);
+      std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T);
   EXPECT(assert_equal(numericalH, actualH, 1e-6));
 }
 
@@ -226,7 +225,7 @@ TEST(Pose3, rotation) {
   EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
 
   Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&Pose3::rotation, _1, boost::none), T);
+      std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T);
   EXPECT(assert_equal(numericalH, actualH, 1e-6));
 }
 
@@ -1052,7 +1051,9 @@ TEST(Pose3, Create) {
   Matrix63 actualH1, actualH2;
   Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
   EXPECT(assert_equal(T, actual));
-  boost::function<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
+  std::function<Pose3(Rot3, Point3)> create =
+      std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none);
   EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
   EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
 }
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 58ad967d21..910d482b06 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -15,15 +15,12 @@
  * @author Frank Dellaert
  **/
 
-#include <gtsam/geometry/SO3.h>
-
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
+#include <gtsam/geometry/SO3.h>
 
-#include <boost/bind/bind.hpp>
-#include <CppUnitLite/TestHarness.h>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -211,7 +208,7 @@ TEST(SO3, ExpmapDerivative) {
 TEST(SO3, ExpmapDerivative2) {
   const Vector3 theta(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
-      boost::bind(&SO3::Expmap, _1, boost::none), theta);
+      std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
 
   CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
   CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@@ -222,7 +219,7 @@ TEST(SO3, ExpmapDerivative2) {
 TEST(SO3, ExpmapDerivative3) {
   const Vector3 theta(10, 20, 30);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
-      boost::bind(&SO3::Expmap, _1, boost::none), theta);
+      std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
 
   CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
   CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@@ -277,7 +274,7 @@ TEST(SO3, ExpmapDerivative5) {
 TEST(SO3, ExpmapDerivative6) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
-      boost::bind(&SO3::Expmap, _1, boost::none), thetahat);
+      std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat);
   Matrix3 Jactual;
   SO3::Expmap(thetahat, Jactual);
   EXPECT(assert_equal(Jexpected, Jactual));
@@ -288,7 +285,7 @@ TEST(SO3, LogmapDerivative) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const SO3 R = SO3::Expmap(thetahat);  // some rotation
   const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
-      boost::bind(&SO3::Logmap, _1, boost::none), R);
+      std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
   const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
   EXPECT(assert_equal(Jexpected, Jactual));
 }
@@ -298,7 +295,7 @@ TEST(SO3, JacobianLogmap) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const SO3 R = SO3::Expmap(thetahat);  // some rotation
   const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
-      boost::bind(&SO3::Logmap, _1, boost::none), R);
+      std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
   Matrix3 Jactual;
   SO3::Logmap(R, Jactual);
   EXPECT(assert_equal(Jexpected, Jactual));
@@ -308,7 +305,7 @@ TEST(SO3, JacobianLogmap) {
 TEST(SO3, ApplyDexp) {
   Matrix aH1, aH2;
   for (bool nearZeroApprox : {true, false}) {
-    boost::function<Vector3(const Vector3&, const Vector3&)> f =
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
         };
@@ -331,7 +328,7 @@ TEST(SO3, ApplyDexp) {
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
   for (bool nearZeroApprox : {true, false}) {
-    boost::function<Vector3(const Vector3&, const Vector3&)> f =
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
         };
@@ -357,7 +354,7 @@ TEST(SO3, vec) {
   Matrix actualH;
   const Vector9 actual = R2.vec(actualH);
   CHECK(assert_equal(expected, actual));
-  boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
+  std::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
   const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
   CHECK(assert_equal(numericalH, actualH));
 }
@@ -371,7 +368,7 @@ TEST(Matrix, compose) {
   Matrix actualH;
   const Matrix3 actual = so3::compose(M, R, actualH);
   CHECK(assert_equal(expected, actual));
-  boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
+  std::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
     return so3::compose(M, R);
   };
   Matrix numericalH = numericalDerivative11(f, M, 1e-2);
diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp
index f771eea5f3..5486755f77 100644
--- a/gtsam/geometry/tests/testSO4.cpp
+++ b/gtsam/geometry/tests/testSO4.cpp
@@ -166,7 +166,7 @@ TEST(SO4, vec) {
   Matrix actualH;
   const Vector16 actual = Q2.vec(actualH);
   EXPECT(assert_equal(expected, actual));
-  boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
+  std::function<Vector16(const SO4&)> f = [](const SO4& Q) {
     return Q.vec();
   };
   const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
@@ -179,7 +179,7 @@ TEST(SO4, topLeft) {
   Matrix actualH;
   const Matrix3 actual = topLeft(Q3, actualH);
   EXPECT(assert_equal(expected, actual));
-  boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
+  std::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
     return topLeft(Q3);
   };
   const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
@@ -192,7 +192,7 @@ TEST(SO4, stiefel) {
   Matrix actualH;
   const Matrix43 actual = stiefel(Q3, actualH);
   EXPECT(assert_equal(expected, actual));
-  boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
+  std::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
     return stiefel(Q3);
   };
   const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp
index 4d0ed98b36..d9d4da34c1 100644
--- a/gtsam/geometry/tests/testSOn.cpp
+++ b/gtsam/geometry/tests/testSOn.cpp
@@ -189,7 +189,7 @@ Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
 /// Test Jacobian of Retract at origin
 TEST(SOn, RetractJacobian) {
   Matrix actualH = RetractJacobian(3);
-  boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
+  std::function<Matrix(const Vector &)> h = [](const Vector &v) {
     return SOn::ChartAtOrigin::Retract(v).matrix();
   };
   Vector3 v;
@@ -205,7 +205,7 @@ TEST(SOn, vec) {
   SOn Q = SOn::ChartAtOrigin::Retract(v);
   Matrix actualH;
   const Vector actual = Q.vec(actualH);
-  boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
+  std::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
   const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
   CHECK(assert_equal(H, actualH));
 }
diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp
index 10a9b2ac49..428422072f 100644
--- a/gtsam/geometry/tests/testSimilarity3.cpp
+++ b/gtsam/geometry/tests/testSimilarity3.cpp
@@ -16,24 +16,22 @@
  * @author Zhaoyang Lv
  */
 
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Similarity3.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/testLie.h>
-#include <gtsam/base/Testable.h>
-
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/BetweenFactor.h>
 
-#include <boost/bind/bind.hpp>
-#include <boost/function.hpp>
+#include <functional>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 using symbol_shorthand::X;
@@ -243,8 +241,9 @@ TEST(Similarity3, GroupAction) {
   EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa)));
 
   // Test derivative
-  boost::function<Point3(Similarity3, Point3)> f = boost::bind(
-      &Similarity3::transformFrom, _1, _2, boost::none, boost::none);
+  // Use lambda to resolve overloaded method
+  std::function<Point3(const Similarity3&, const Point3&)>
+      f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); };
 
   Point3 q(1, 2, 3);
   for (const auto& T : { T1, T2, T3, T4, T5, T6 }) {
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index 4d609380c8..b548b93153 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -32,13 +32,12 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/std/vector.hpp>
-#include <boost/bind/bind.hpp>
 
 #include <cmath>
 #include <random>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 using gtsam::symbol_shorthand::U;
@@ -127,8 +126,9 @@ TEST(Unit3, dot) {
 
   // Use numerical derivatives to calculate the expected Jacobians
   Matrix H1, H2;
-  boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2,  //
-                                                                      boost::none, boost::none);
+  std::function<double(const Unit3&, const Unit3&)> f =
+      std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2,  //
+                boost::none, boost::none);
   {
     p.dot(q, H1, H2);
     EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
@@ -158,13 +158,13 @@ TEST(Unit3, error) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative11<Vector2,Unit3>(
-        boost::bind(&Unit3::error, &p, _1, boost::none), q);
+        std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q);
     p.error(q, actual);
     EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
   {
     expected = numericalDerivative11<Vector2,Unit3>(
-        boost::bind(&Unit3::error, &p, _1, boost::none), r);
+        std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r);
     p.error(r, actual);
     EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
@@ -185,25 +185,33 @@ TEST(Unit3, error2) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative21<Vector2, Unit3, Unit3>(
-        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
+        std::bind(&Unit3::errorVector, std::placeholders::_1,
+                  std::placeholders::_2, boost::none, boost::none),
+        p, q);
     p.errorVector(q, actual, boost::none);
     EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative21<Vector2, Unit3, Unit3>(
-        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
+        std::bind(&Unit3::errorVector, std::placeholders::_1,
+                  std::placeholders::_2, boost::none, boost::none),
+        p, r);
     p.errorVector(r, actual, boost::none);
     EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative22<Vector2, Unit3, Unit3>(
-        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
+        std::bind(&Unit3::errorVector, std::placeholders::_1,
+                  std::placeholders::_2, boost::none, boost::none),
+        p, q);
     p.errorVector(q, boost::none, actual);
     EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative22<Vector2, Unit3, Unit3>(
-        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
+        std::bind(&Unit3::errorVector, std::placeholders::_1,
+                  std::placeholders::_2, boost::none, boost::none),
+        p, r);
     p.errorVector(r, boost::none, actual);
     EXPECT(assert_equal(expected, actual, 1e-5));
   }
@@ -221,13 +229,13 @@ TEST(Unit3, distance) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalGradient<Unit3>(
-        boost::bind(&Unit3::distance, &p, _1, boost::none), q);
+        std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q);
     p.distance(q, actual);
     EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
   {
     expected = numericalGradient<Unit3>(
-        boost::bind(&Unit3::distance, &p, _1, boost::none), r);
+        std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r);
     p.distance(r, actual);
     EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
@@ -319,7 +327,7 @@ TEST(Unit3, basis) {
 
   Matrix62 actualH;
   Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
-      boost::bind(BasisTest, _1, boost::none), p);
+      std::bind(BasisTest, std::placeholders::_1, boost::none), p);
 
   // without H, first time
   EXPECT(assert_equal(expected, p.basis(), 1e-6));
@@ -348,7 +356,7 @@ TEST(Unit3, basis_derivatives) {
     p.basis(actualH);
 
     Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
-                           boost::bind(BasisTest, _1, boost::none), p);
+        std::bind(BasisTest, std::placeholders::_1, boost::none), p);
     EXPECT(assert_equal(expectedH, actualH, 1e-5));
   }
 }
@@ -376,8 +384,8 @@ TEST(Unit3, retract) {
 TEST (Unit3, jacobian_retract) {
   Matrix22 H;
   Unit3 p;
-  boost::function<Unit3(const Vector2&)> f =
-      boost::bind(&Unit3::retract, p, _1, boost::none);
+  std::function<Unit3(const Vector2&)> f =
+      std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none);
   {
       Vector2 v (-0.2, 0.1);
       p.retract(v, H);
@@ -440,7 +448,7 @@ TEST (Unit3, FromPoint3) {
   Unit3 expected(point);
   EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
   Matrix expectedH = numericalDerivative11<Unit3, Point3>(
-      boost::bind(Unit3::FromPoint3, _1, boost::none), point);
+      std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point);
   EXPECT(assert_equal(expectedH, actualH, 1e-5));
 }
 
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index c88bf87314..00a338e547 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -32,7 +32,7 @@
 #include <sstream>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -270,11 +270,11 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
 
   // Compute the Hessian numerically
   Matrix hessian = numericalHessian<Vector10>(
-      boost::bind(&computeError, gbn, _1), Vector10::Zero());
+      std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
 
   // Compute the gradient numerically
   Vector gradient = numericalGradient<Vector10>(
-      boost::bind(&computeError, gbn, _1), Vector10::Zero());
+      std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
 
   // Compute the gradient using dense matrices
   Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp
index a6a60c19ce..c5601af271 100644
--- a/gtsam/linear/tests/testGaussianBayesTree.cpp
+++ b/gtsam/linear/tests/testGaussianBayesTree.cpp
@@ -21,7 +21,6 @@
 #include <boost/assign/list_of.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/assign/std/set.hpp> // for operator +=
-#include <boost/bind/bind.hpp>
 
 #include <gtsam/base/debug.h>
 #include <gtsam/base/numericalDerivative.h>
@@ -30,7 +29,7 @@
 #include <gtsam/linear/GaussianConditional.h>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -260,11 +259,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
 
   // Compute the Hessian numerically
   Matrix hessian = numericalHessian<Vector10>(
-      boost::bind(&computeError, bt, _1), Vector10::Zero());
+      std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
 
   // Compute the gradient numerically
   Vector gradient = numericalGradient<Vector10>(
-      boost::bind(&computeError, bt, _1), Vector10::Zero());
+      std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
 
   // Compute the gradient using dense matrices
   Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp
index 828e264f46..a4d06d01a0 100644
--- a/gtsam/navigation/tests/testAHRSFactor.cpp
+++ b/gtsam/navigation/tests/testAHRSFactor.cpp
@@ -25,10 +25,9 @@
 #include <gtsam/base/debug.h>
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind/bind.hpp>
 #include <list>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -175,17 +174,17 @@ TEST(AHRSFactor, Error) {
 
   // Expected Jacobians
   Matrix H1e = numericalDerivative11<Vector3, Rot3>(
-      boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
+      std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
   Matrix H2e = numericalDerivative11<Vector3, Rot3>(
-      boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
+      std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
   Matrix H3e = numericalDerivative11<Vector3, Vector3>(
-      boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
+      std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
 
   // Check rotation Jacobians
   Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
-      boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
+      std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
   Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
-      boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
+      std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
 
   // Actual Jacobians
   Matrix H1a, H2a, H3a;
@@ -234,19 +233,19 @@ TEST(AHRSFactor, ErrorWithBiases) {
 
   // Expected Jacobians
   Matrix H1e = numericalDerivative11<Vector, Rot3>(
-      boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
+      std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
   Matrix H2e = numericalDerivative11<Vector, Rot3>(
-      boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
+      std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
   Matrix H3e = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
+      std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
 
   // Check rotation Jacobians
   Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
-      boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
+      std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
   Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
-      boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
+      std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
   Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
+      std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
 
   // Actual Jacobians
   Matrix H1a, H2a, H3a;
@@ -269,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) {
 
   // Compute numerical derivatives
   Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);
+      std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega);
 
   const Matrix3 Jr = Rot3::ExpmapDerivative(
       (measuredOmega - biasOmega) * deltaT);
@@ -294,7 +293,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
 
   // Compute numerical derivatives
   Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
-      boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
+      std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta);
 
   const Vector3 x = thetahat; // parametrization of so(3)
   const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
@@ -368,7 +367,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
   // Compute numerical derivatives
   Matrix expectedDelRdelBias =
       numericalDerivative11<Rot3, Vector3>(
-          boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
+          std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1,
               measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
   Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
 
@@ -410,19 +409,19 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
 
   // Expected Jacobians
   Matrix H1e = numericalDerivative11<Vector, Rot3>(
-      boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
+      std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
   Matrix H2e = numericalDerivative11<Vector, Rot3>(
-      boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
+      std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
   Matrix H3e = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
+      std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
 
   // Check rotation Jacobians
   Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
-      boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
+      std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
   Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
-      boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
+      std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
   Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
+      std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
 
   // Actual Jacobians
   Matrix H1a, H2a, H3a;
@@ -459,8 +458,8 @@ TEST (AHRSFactor, predictTest) {
 
   // AHRSFactor::PreintegratedMeasurements::predict
   Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
-      boost::bind(&AHRSFactor::PreintegratedMeasurements::predict,
-          &pim, _1, boost::none), bias);
+      std::bind(&AHRSFactor::PreintegratedMeasurements::predict,
+          &pim, std::placeholders::_1, boost::none), bias);
 
   // Actual Jacobians
   Matrix H;
diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index 2ab60fe6ae..d49907cbfd 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -25,7 +25,7 @@
 #include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -50,8 +50,9 @@ TEST( Rot3AttitudeFactor, Constructor ) {
   EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,Rot3>(
-      boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
+  Matrix expectedH = numericalDerivative11<Vector, Rot3>(
+      std::bind(&Rot3AttitudeFactor::evaluateError, &factor,
+                std::placeholders::_1, boost::none),
       nRb);
 
   // Use the factor to calculate the derivative
@@ -117,7 +118,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,Pose3>(
-      boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
+      std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1,
           boost::none), T);
 
   // Use the factor to calculate the derivative
diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index b486272abf..b784c0c94c 100644
--- a/gtsam/navigation/tests/testGPSFactor.cpp
+++ b/gtsam/navigation/tests/testGPSFactor.cpp
@@ -27,7 +27,7 @@
 #include <GeographicLib/Config.h>
 #include <GeographicLib/LocalCartesian.hpp>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
@@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) {
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,Pose3>(
-      boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
+      std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
 
   // Use the factor to calculate the derivative
   Matrix actualH;
@@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) {
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,NavState>(
-      boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
+      std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
 
   // Use the factor to calculate the derivative
   Matrix actualH;
diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp
index e7da2c81c6..b486a4a98b 100644
--- a/gtsam/navigation/tests/testImuBias.cpp
+++ b/gtsam/navigation/tests/testImuBias.cpp
@@ -19,9 +19,8 @@
 #include <gtsam/base/numericalDerivative.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind/bind.hpp>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -128,8 +127,9 @@ TEST(ImuBias, operatorSubB) {
 TEST(ImuBias, Correct1) {
   Matrix aH1, aH2;
   const Vector3 measurement(1, 2, 3);
-  boost::function<Vector3(const Bias&, const Vector3&)> f = boost::bind(
-      &Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
+  std::function<Vector3(const Bias&, const Vector3&)> f =
+      std::bind(&Bias::correctAccelerometer, std::placeholders::_1,
+                std::placeholders::_2, boost::none, boost::none);
   bias1.correctAccelerometer(measurement, aH1, aH2);
   EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
   EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
@@ -139,8 +139,9 @@ TEST(ImuBias, Correct1) {
 TEST(ImuBias, Correct2) {
   Matrix aH1, aH2;
   const Vector3 measurement(1, 2, 3);
-  boost::function<Vector3(const Bias&, const Vector3&)> f =
-      boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
+  std::function<Vector3(const Bias&, const Vector3&)> f =
+      std::bind(&Bias::correctGyroscope, std::placeholders::_1,
+                std::placeholders::_2, boost::none, boost::none);
   bias1.correctGyroscope(measurement, aH1, aH2);
   EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
   EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp
index f19862772c..801d87895d 100644
--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/gtsam/navigation/tests/testImuFactor.cpp
@@ -146,9 +146,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
   Matrix9 aH1, aH2;
   Matrix96 aH3;
   actual.computeError(x1, x2, bias, aH1, aH2, aH3);
-  boost::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
-      boost::bind(&PreintegrationBase::computeError, actual,
-                  boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3,
+  std::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
+      std::bind(&PreintegrationBase::computeError, actual,
+                  std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
                   boost::none, boost::none, boost::none);
   EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
   EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
@@ -204,20 +204,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
   Matrix96 actualH;
   pim.biasCorrectedDelta(kZeroBias, actualH);
   Matrix expectedH = numericalDerivative11<Vector9, Bias>(
-      boost::bind(&PreintegrationBase::biasCorrectedDelta, pim,
-          boost::placeholders::_1, boost::none), kZeroBias);
+      std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
+          std::placeholders::_1, boost::none), kZeroBias);
   EXPECT(assert_equal(expectedH, actualH));
 
   Matrix9 aH1;
   Matrix96 aH2;
   NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
   Matrix eH1 = numericalDerivative11<NavState, NavState>(
-      boost::bind(&PreintegrationBase::predict, pim, boost::placeholders::_1,
+      std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1,
           kZeroBias, boost::none, boost::none), state1);
   EXPECT(assert_equal(eH1, aH1));
   Matrix eH2 = numericalDerivative11<NavState, Bias>(
-      boost::bind(&PreintegrationBase::predict, pim, state1,
-          boost::placeholders::_1, boost::none, boost::none), kZeroBias);
+      std::bind(&PreintegrationBase::predict, pim, state1,
+          std::placeholders::_1, boost::none, boost::none), kZeroBias);
   EXPECT(assert_equal(eH2, aH2));
 }
 
@@ -278,12 +278,12 @@ TEST(ImuFactor, ErrorAndJacobians) {
   // Make sure rotation part is correct when error is interpreted as axis-angle
   // Jacobians are around zero, so the rotation part is the same as:
   Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&evaluateRotationError, factor, boost::placeholders::_1, v1, x2, v2, kZeroBias),
+      std::bind(&evaluateRotationError, factor, std::placeholders::_1, v1, x2, v2, kZeroBias),
       x1);
   EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
 
   Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&evaluateRotationError, factor, x1, v1, boost::placeholders::_1, v2, kZeroBias),
+      std::bind(&evaluateRotationError, factor, x1, v1, std::placeholders::_1, v2, kZeroBias),
       x2);
   EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
 
@@ -333,8 +333,8 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
   Matrix96 actualH;
   pim.biasCorrectedDelta(bias, actualH);
   Matrix expectedH = numericalDerivative11<Vector9, Bias>(
-      boost::bind(&PreintegrationBase::biasCorrectedDelta, pim,
-          boost::placeholders::_1, boost::none), bias);
+      std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
+          std::placeholders::_1, boost::none), bias);
   EXPECT(assert_equal(expectedH, actualH));
 
   // Create factor
@@ -522,7 +522,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega,
       boost::none, D_correctedAcc_measuredOmega, boost::none);
   Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
-      boost::bind(correctedAcc, pim, measuredAcc, boost::placeholders::_1),
+      std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1),
       measuredOmega, 1e-6);
   EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
 
@@ -534,15 +534,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
 //      pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
 //
 //  Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
-//      boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
-//          boost::placeholders::_1, boost::placeholders::_2,
+//      std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
+//          std::placeholders::_1, std::placeholders::_2,
 //          dt, boost::none, boost::none, boost::none), measuredAcc,
 //      measuredOmega, 1e-6);
 //  EXPECT(assert_equal(expectedG1, G1, 1e-5));
 //
 //  Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
-//      boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
-//          boost::placeholders::_1, boost::placeholders::_2,
+//      std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
+//          std::placeholders::_1, std::placeholders::_2,
 //          dt, boost::none, boost::none, boost::none), measuredAcc,
 //      measuredOmega, 1e-6);
 //  EXPECT(assert_equal(expectedG2, G2, 1e-5));
diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp
index ad193b5036..5107b3b6b3 100644
--- a/gtsam/navigation/tests/testMagFactor.cpp
+++ b/gtsam/navigation/tests/testMagFactor.cpp
@@ -26,7 +26,7 @@
 
 #include <GeographicLib/LocalCartesian.hpp>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
@@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) {
   Point3 expected(22735.5, 314.502, 44202.5);
   EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
   EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
-      (boost::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
+      (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
 }
 
 // *************************************************************************
@@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) {
   MagFactor f(1, measured, s, dir, bias, model);
   EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
   EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
-      (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
+      (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
 
 // MagFactor1
   MagFactor1 f1(1, measured, s, dir, bias, model);
   EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
   EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
-      (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
+      (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
 
 // MagFactor2
   MagFactor2 f2(1, 2, measured, nRb, model);
   EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
   EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
-      (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
+      (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
       H1, 1e-7));
   EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
-      (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
+      (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
       H2, 1e-7));
 
 // MagFactor2
   MagFactor3 f3(1, 2, 3, measured, nRb, model);
   EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
-      (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
+      (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
       H1, 1e-7));
   EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
-      (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
+      (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
       H2, 1e-7));
   EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
-      (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
+      (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
       H3, 1e-7));
 }
 
diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp
index 1a3c5b2a97..204c1d38f9 100644
--- a/gtsam/navigation/tests/testMagPoseFactor.cpp
+++ b/gtsam/navigation/tests/testMagPoseFactor.cpp
@@ -17,9 +17,7 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/MagPoseFactor.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 
 // *****************************************************************************
@@ -78,8 +76,11 @@ TEST(MagPoseFactor, JacobianPose2) {
   // Error should be zero at the groundtruth pose.
   MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
   CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
-  CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
-      (boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
+  CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2>  //
+                            (std::bind(&MagPoseFactor<Pose2>::evaluateError, &f,
+                                       std::placeholders::_1, boost::none),
+                             n_P2_b),
+                            H2, 1e-7));
 }
 
 // *****************************************************************************
@@ -89,8 +90,11 @@ TEST(MagPoseFactor, JacobianPose3) {
   // Error should be zero at the groundtruth pose.
   MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
   CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
-  CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
-      (boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
+  CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3>  //
+                            (std::bind(&MagPoseFactor<Pose3>::evaluateError, &f,
+                                       std::placeholders::_1, boost::none),
+                             n_P3_b),
+                            H3, 1e-7));
 }
 
 // *****************************************************************************
@@ -104,7 +108,7 @@ TEST(MagPoseFactor, body_P_sensor2) {
   MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor);
   CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
-      (boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
+      (std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7));
 }
 
 // *****************************************************************************
@@ -118,7 +122,7 @@ TEST(MagPoseFactor, body_P_sensor3) {
   MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor);
   CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
-      (boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
+      (std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7));
 }
 
 // *****************************************************************************
diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp
index 625689ed79..7796ccbda5 100644
--- a/gtsam/navigation/tests/testManifoldPreintegration.cpp
+++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp
@@ -22,11 +22,10 @@
 #include <gtsam/nonlinear/expressionTesting.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind/bind.hpp>
 
 #include "imuFactorTesting.h"
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 
 namespace testing {
 // Create default parameters with Z-down and above noise parameters
@@ -43,21 +42,21 @@ static boost::shared_ptr<PreintegrationParams> Params() {
 TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
   testing::SomeMeasurements measurements;
 
-  boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
+  std::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
       [=](const Vector3& a, const Vector3& w) {
         ManifoldPreintegration pim(testing::Params(), Bias(a, w));
         testing::integrateMeasurements(measurements, &pim);
         return pim.deltaRij();
       };
 
-  boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
+  std::function<Point3(const Vector3&, const Vector3&)> deltaPij =
       [=](const Vector3& a, const Vector3& w) {
         ManifoldPreintegration pim(testing::Params(), Bias(a, w));
         testing::integrateMeasurements(measurements, &pim);
         return pim.deltaPij();
       };
 
-  boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
+  std::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
       [=](const Vector3& a, const Vector3& w) {
         ManifoldPreintegration pim(testing::Params(), Bias(a, w));
         testing::integrateMeasurements(measurements, &pim);
@@ -98,10 +97,12 @@ TEST(ManifoldPreintegration, computeError) {
   Matrix9 aH1, aH2;
   Matrix96 aH3;
   pim.computeError(x1, x2, bias, aH1, aH2, aH3);
-  boost::function<Vector9(const NavState&, const NavState&,
-                          const imuBias::ConstantBias&)> f =
-      boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
-                  boost::none, boost::none, boost::none);
+  std::function<Vector9(const NavState&, const NavState&,
+                        const imuBias::ConstantBias&)>
+      f = std::bind(&ManifoldPreintegration::computeError, pim,
+                    std::placeholders::_1, std::placeholders::_2,
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
   EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
   EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index 86c708f5e8..e7adb923d7 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/gtsam/navigation/tests/testNavState.cpp
@@ -23,7 +23,7 @@
 #include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -39,9 +39,9 @@ static const Vector9 kZeroXi = Vector9::Zero();
 
 /* ************************************************************************* */
 TEST(NavState, Constructor) {
-  boost::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
-      boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
-          boost::none);
+  std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
+      std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2,
+                std::placeholders::_3, boost::none, boost::none, boost::none);
   Matrix aH1, aH2, aH3;
   EXPECT(
       assert_equal(kState1,
@@ -59,9 +59,9 @@ TEST(NavState, Constructor) {
 
 /* ************************************************************************* */
 TEST(NavState, Constructor2) {
-  boost::function<NavState(const Pose3&, const Vector3&)> construct =
-      boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
-          boost::none);
+  std::function<NavState(const Pose3&, const Vector3&)> construct =
+      std::bind(&NavState::FromPoseVelocity, std::placeholders::_1,
+                std::placeholders::_2, boost::none, boost::none);
   Matrix aH1, aH2;
   EXPECT(
       assert_equal(kState1,
@@ -76,7 +76,7 @@ TEST( NavState, Attitude) {
   Rot3 actual = kState1.attitude(aH);
   EXPECT(assert_equal(actual, kAttitude));
   eH = numericalDerivative11<Rot3, NavState>(
-      boost::bind(&NavState::attitude, _1, boost::none), kState1);
+      std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1);
   EXPECT(assert_equal((Matrix )eH, aH));
 }
 
@@ -86,7 +86,8 @@ TEST( NavState, Position) {
   Point3 actual = kState1.position(aH);
   EXPECT(assert_equal(actual, kPosition));
   eH = numericalDerivative11<Point3, NavState>(
-      boost::bind(&NavState::position, _1, boost::none), kState1);
+      std::bind(&NavState::position, std::placeholders::_1, boost::none),
+      kState1);
   EXPECT(assert_equal((Matrix )eH, aH));
 }
 
@@ -96,7 +97,8 @@ TEST( NavState, Velocity) {
   Velocity3 actual = kState1.velocity(aH);
   EXPECT(assert_equal(actual, kVelocity));
   eH = numericalDerivative11<Velocity3, NavState>(
-      boost::bind(&NavState::velocity, _1, boost::none), kState1);
+      std::bind(&NavState::velocity, std::placeholders::_1, boost::none),
+      kState1);
   EXPECT(assert_equal((Matrix )eH, aH));
 }
 
@@ -106,7 +108,8 @@ TEST( NavState, BodyVelocity) {
   Velocity3 actual = kState1.bodyVelocity(aH);
   EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
   eH = numericalDerivative11<Velocity3, NavState>(
-      boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1);
+      std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none),
+      kState1);
   EXPECT(assert_equal((Matrix )eH, aH));
 }
 
@@ -137,8 +140,9 @@ TEST( NavState, Manifold ) {
   // Check retract derivatives
   Matrix9 aH1, aH2;
   kState1.retract(xi, aH1, aH2);
-  boost::function<NavState(const NavState&, const Vector9&)> retract =
-      boost::bind(&NavState::retract, _1, _2, boost::none, boost::none);
+  std::function<NavState(const NavState&, const Vector9&)> retract =
+      std::bind(&NavState::retract, std::placeholders::_1,
+                std::placeholders::_2, boost::none, boost::none);
   EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
   EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
 
@@ -149,9 +153,9 @@ TEST( NavState, Manifold ) {
   EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
 
   // Check localCoordinates derivatives
-  boost::function<Vector9(const NavState&, const NavState&)> local =
-      boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
-          boost::none);
+  std::function<Vector9(const NavState&, const NavState&)> local =
+      std::bind(&NavState::localCoordinates, std::placeholders::_1,
+                std::placeholders::_2, boost::none, boost::none);
   // from state1 to state2
   kState1.localCoordinates(state2, aH1, aH2);
   EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
@@ -168,8 +172,9 @@ TEST( NavState, Manifold ) {
 
 /* ************************************************************************* */
 static const double dt = 2.0;
-boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
-    &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
+std::function<Vector9(const NavState&, const bool&)> coriolis =
+    std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis,
+              std::placeholders::_2, boost::none);
 
 TEST(NavState, Coriolis) {
   Matrix9 aH;
@@ -244,9 +249,10 @@ TEST(NavState, CorrectPIM) {
   xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
   double dt = 0.5;
   Matrix9 aH1, aH2;
-  boost::function<Vector9(const NavState&, const Vector9&)> correctPIM =
-      boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
-          false, boost::none, boost::none);
+  std::function<Vector9(const NavState&, const Vector9&)> correctPIM =
+      std::bind(&NavState::correctPIM, std::placeholders::_1,
+                std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false,
+                boost::none, boost::none);
   kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
   EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
   EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp
index d0bae36900..0df51956bd 100644
--- a/gtsam/navigation/tests/testScenario.cpp
+++ b/gtsam/navigation/tests/testScenario.cpp
@@ -19,10 +19,9 @@
 #include <gtsam/navigation/Scenario.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind/bind.hpp>
 #include <cmath>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -148,7 +147,7 @@ TEST(Scenario, Accelerating) {
   {
     // Check acceleration in nav
     Matrix expected = numericalDerivative11<Vector3, double>(
-        boost::bind(&Scenario::velocity_n, scenario, _1), T);
+        std::bind(&Scenario::velocity_n, scenario, std::placeholders::_1), T);
     EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
   }
 
diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp
index 9bb988b429..ada0590945 100644
--- a/gtsam/navigation/tests/testTangentPreintegration.cpp
+++ b/gtsam/navigation/tests/testTangentPreintegration.cpp
@@ -22,11 +22,10 @@
 #include <gtsam/nonlinear/expressionTesting.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/bind/bind.hpp>
 
 #include "imuFactorTesting.h"
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 
 static const double kDt = 0.1;
 
@@ -78,7 +77,7 @@ TEST(TangentPreintegration, UpdateEstimate2) {
 TEST(ImuFactor, BiasCorrectionJacobians) {
   testing::SomeMeasurements measurements;
 
-  boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
+  std::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
       [=](const Vector3& a, const Vector3& w) {
         TangentPreintegration pim(testing::Params(), Bias(a, w));
         testing::integrateMeasurements(measurements, &pim);
@@ -105,10 +104,12 @@ TEST(TangentPreintegration, computeError) {
   Matrix9 aH1, aH2;
   Matrix96 aH3;
   pim.computeError(x1, x2, bias, aH1, aH2, aH3);
-  boost::function<Vector9(const NavState&, const NavState&,
-                          const imuBias::ConstantBias&)> f =
-      boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
-                  boost::none, boost::none, boost::none);
+  std::function<Vector9(const NavState&, const NavState&,
+                        const imuBias::ConstantBias&)>
+      f = std::bind(&TangentPreintegration::computeError, pim,
+                    std::placeholders::_1, std::placeholders::_2,
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
   EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
   EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
@@ -121,7 +122,7 @@ TEST(TangentPreintegration, Compose) {
   TangentPreintegration pim(testing::Params());
   testing::integrateMeasurements(measurements, &pim);
 
-  boost::function<Vector9(const Vector9&, const Vector9&)> f =
+  std::function<Vector9(const Vector9&, const Vector9&)> f =
       [pim](const Vector9& zeta01, const Vector9& zeta12) {
         return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
       };
diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp
index 8fcf84a113..7bb8021866 100644
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@@ -495,7 +495,7 @@ TEST(Expression, Subtract) {
 /* ************************************************************************* */
 TEST(Expression, LinearExpression) {
   const Key key(67);
-  const boost::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
+  const std::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
   const Matrix3 kIdentity = I_3x3;
   const Expression<Vector3> linear_ = linearExpression(f, Point3_(key), kIdentity);
 
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index e9076a7d7a..b894f48169 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -34,7 +34,7 @@
 #include <type_traits>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
@@ -336,7 +336,7 @@ TEST(Values, filter) {
 
   // Filter by key
   int i = 0;
-  Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
+  Values::Filtered<Value> filtered = values.filter(std::bind(std::greater_equal<Key>(), std::placeholders::_1, 2));
   EXPECT_LONGS_EQUAL(2, (long)filtered.size());
   for(const auto key_value: filtered) {
     if(i == 0) {
@@ -364,7 +364,7 @@ TEST(Values, filter) {
   EXPECT(assert_equal(expectedSubValues1, actualSubValues1));
 
   // ConstFilter by Key
-  Values::ConstFiltered<Value> constfiltered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
+  Values::ConstFiltered<Value> constfiltered = values.filter(std::bind(std::greater_equal<Key>(), std::placeholders::_1, 2));
   EXPECT_LONGS_EQUAL(2, (long)constfiltered.size());
   Values fromconstfiltered(constfiltered);
   EXPECT(assert_equal(expectedSubValues1, fromconstfiltered));
diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp
index 2af5825db8..5f5d4f4dd0 100644
--- a/gtsam/sam/tests/testRangeFactor.cpp
+++ b/gtsam/sam/tests/testRangeFactor.cpp
@@ -28,7 +28,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include <boost/bind/bind.hpp>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -265,9 +265,9 @@ TEST( RangeFactor, Jacobian2D ) {
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
   H1Expected = numericalDerivative11<Vector, Pose2>(
-      boost::bind(&factorError2D, _1, point, factor), pose);
+      std::bind(&factorError2D, std::placeholders::_1, point, factor), pose);
   H2Expected = numericalDerivative11<Vector, Point2>(
-      boost::bind(&factorError2D, pose, _1, factor), point);
+      std::bind(&factorError2D, pose, std::placeholders::_1, factor), point);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@@ -296,9 +296,9 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
   H1Expected = numericalDerivative11<Vector, Pose2>(
-      boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
+      std::bind(&factorErrorWithTransform2D, std::placeholders::_1, point, factor), pose);
   H2Expected = numericalDerivative11<Vector, Point2>(
-      boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point);
+      std::bind(&factorErrorWithTransform2D, pose, std::placeholders::_1, factor), point);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@@ -323,9 +323,9 @@ TEST( RangeFactor, Jacobian3D ) {
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
   H1Expected = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&factorError3D, _1, point, factor), pose);
+      std::bind(&factorError3D, std::placeholders::_1, point, factor), pose);
   H2Expected = numericalDerivative11<Vector, Point3>(
-      boost::bind(&factorError3D, pose, _1, factor), point);
+      std::bind(&factorError3D, pose, std::placeholders::_1, factor), point);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@@ -355,9 +355,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
   H1Expected = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
+      std::bind(&factorErrorWithTransform3D, std::placeholders::_1, point, factor), pose);
   H2Expected = numericalDerivative11<Vector, Point3>(
-      boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point);
+      std::bind(&factorErrorWithTransform3D, pose, std::placeholders::_1, factor), point);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp
index 05ae76faa3..818f2bce51 100644
--- a/gtsam/sfm/tests/testTranslationFactor.cpp
+++ b/gtsam/sfm/tests/testTranslationFactor.cpp
@@ -20,10 +20,9 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/sfm/TranslationFactor.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -91,9 +90,9 @@ TEST(TranslationFactor, Jacobian) {
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
   H1Expected = numericalDerivative11<Vector, Point3>(
-      boost::bind(&factorError, _1, T2, factor), T1);
+      std::bind(&factorError, std::placeholders::_1, T2, factor), T1);
   H2Expected = numericalDerivative11<Vector, Point3>(
-      boost::bind(&factorError, T1, _1, factor), T2);
+      std::bind(&factorError, T1, std::placeholders::_1, factor), T2);
 
   // Verify the Jacobians are correct
   EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp
index a1f8774e5a..6897943ec1 100644
--- a/gtsam/slam/tests/testBetweenFactor.cpp
+++ b/gtsam/slam/tests/testBetweenFactor.cpp
@@ -14,7 +14,7 @@
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/slam/BetweenFactor.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 using namespace gtsam::noiseModel;
@@ -35,15 +35,16 @@ TEST(BetweenFactor, Rot3) {
   Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2));
   EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail
 
-  Matrix numericalH1 = numericalDerivative21<Vector3,Rot3,Rot3>(
-      boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
-          &BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
-          boost::none)), R1, R2, 1e-5);
+  Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>(
+      std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
+          &BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1,
+          std::placeholders::_2, boost::none, boost::none)),
+      R1, R2, 1e-5);
   EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
 
   Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>(
-      boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
-          &BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
+      std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
+          &BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none,
           boost::none)), R1, R2, 1e-5);
   EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
 }
diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
index fb2172107c..080239b350 100644
--- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
@@ -23,10 +23,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -53,12 +52,14 @@ TEST( EssentialMatrixConstraint, test ) {
   CHECK(assert_equal(expected, actual, 1e-8));
 
   // Calculate numerical derivatives
-  Matrix expectedH1 = numericalDerivative11<Vector5,Pose3>(
-      boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
-          boost::none, boost::none), pose1);
-  Matrix expectedH2 = numericalDerivative11<Vector5,Pose3>(
-      boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
-          boost::none, boost::none), pose2);
+  Matrix expectedH1 = numericalDerivative11<Vector5, Pose3>(
+      std::bind(&EssentialMatrixConstraint::evaluateError, &factor,
+                std::placeholders::_1, pose2, boost::none, boost::none),
+      pose1);
+  Matrix expectedH2 = numericalDerivative11<Vector5, Pose3>(
+      std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1,
+                std::placeholders::_1, boost::none, boost::none),
+      pose2);
 
   // Use the factor to calculate the derivative
   Matrix actualH1;
diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
index 79a86865d1..03775a70f0 100644
--- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
@@ -17,9 +17,7 @@
 #include <gtsam/slam/EssentialMatrixFactor.h>
 #include <gtsam/slam/dataset.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -104,8 +102,8 @@ TEST(EssentialMatrixFactor, factor) {
 TEST(EssentialMatrixFactor, ExpressionFactor) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
-    boost::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
-        boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
+    std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
+        std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
     Expression<EssentialMatrix> E_(key);  // leaf expression
     Expression<double> expr(f, E_);       // unary expression
 
@@ -130,9 +128,9 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
 TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
-    boost::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
-        boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
-    boost::function<EssentialMatrix(const Rot3 &, const Unit3 &,
+    std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
+        std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
+    std::function<EssentialMatrix(const Rot3 &, const Unit3 &,
                                     OptionalJacobian<5, 3>,
                                     OptionalJacobian<5, 2>)>
         g;
diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
index 022dc859ea..35c7504081 100644
--- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp
+++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
@@ -27,10 +27,9 @@
 
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std.hpp>
-#include <boost/bind/bind.hpp>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 
@@ -145,8 +144,9 @@ TEST( OrientedPlane3Factor, Derivatives ) {
   OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
 
   // Calculate numerical derivatives
-  boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
-      &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
+  std::function<Vector(const Pose3 &, const OrientedPlane3 &)> f = std::bind(
+      &OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1,
+      std::placeholders::_2, boost::none, boost::none);
   Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
   Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
 
@@ -184,15 +184,15 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
 
   // Calculate numerical derivatives
   Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
-      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
+      std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
           boost::none), T1);
 
   Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
-      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
+      std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
           boost::none), T2);
 
   Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
-      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
+      std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
           boost::none), T3);
 
   // Use the factor to calculate the derivative
diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp
index 075710ae73..b5800a414b 100644
--- a/gtsam/slam/tests/testReferenceFrameFactor.cpp
+++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp
@@ -16,8 +16,6 @@
 
 #include <iostream>
 
-#include <boost/bind/bind.hpp>
-
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam/base/Testable.h>
@@ -32,7 +30,7 @@
 
 using namespace std;
 using namespace boost;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 
 typedef gtsam::ReferenceFrameFactor<gtsam::Point2, gtsam::Pose2> PointReferenceFrameFactor;
@@ -70,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) {
 
   Matrix numericalDT, numericalDL, numericalDF;
   numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
-      boost::bind(evaluateError_, tc, _1, _2, _3),
+      std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
       global, trans, local, 1e-5);
   numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
-      boost::bind(evaluateError_, tc, _1, _2, _3),
+      std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
       global, trans, local, 1e-5);
   numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
-      boost::bind(evaluateError_, tc, _1, _2, _3),
+      std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
       global, trans, local, 1e-5);
 
   EXPECT(assert_equal(numericalDF, actualDF));
@@ -102,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) {
 
   Matrix numericalDT, numericalDL, numericalDF;
   numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
-      boost::bind(evaluateError_, tc, _1, _2, _3),
+      std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
       global, trans, local, 1e-5);
   numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
-      boost::bind(evaluateError_, tc, _1, _2, _3),
+      std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
       global, trans, local, 1e-5);
   numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
-      boost::bind(evaluateError_, tc, _1, _2, _3),
+      std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
       global, trans, local, 1e-5);
 
   EXPECT(assert_equal(numericalDF, actualDF));
diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp
index e4ecafd425..b8fd01730e 100644
--- a/gtsam/slam/tests/testRotateFactor.cpp
+++ b/gtsam/slam/tests/testRotateFactor.cpp
@@ -13,12 +13,11 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/std/vector.hpp>
-#include <boost/bind/bind.hpp>
 #include <vector>
 
 using namespace std;
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 
 static const double kDegree = M_PI / 180;
@@ -73,13 +72,13 @@ TEST (RotateFactor, test) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative11<Vector3,Rot3>(
-        boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
+        std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc);
     f.evaluateError(iRc, actual);
     EXPECT(assert_equal(expected, actual, 1e-9));
   }
   {
     expected = numericalDerivative11<Vector3,Rot3>(
-        boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
+        std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), R);
     f.evaluateError(R, actual);
     EXPECT(assert_equal(expected, actual, 1e-9));
   }
@@ -144,14 +143,14 @@ TEST (RotateDirectionsFactor, test) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative11<Vector,Rot3>(
-        boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
+        std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1,
             boost::none), iRc);
     f.evaluateError(iRc, actual);
     EXPECT(assert_equal(expected, actual, 1e-9));
   }
   {
     expected = numericalDerivative11<Vector,Rot3>(
-        boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
+        std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1,
             boost::none), R);
     f.evaluateError(R, actual);
     EXPECT(assert_equal(expected, actual, 1e-9));
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index 26caa6b75d..c7f220c102 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -27,11 +27,10 @@
 #include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/map.hpp>
-#include <boost/bind/bind.hpp>
 #include <iostream>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 
 static const double rankTol = 1.0;
 // Create a noise model for the pixel error
@@ -132,8 +131,8 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
   EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
 
   // Calculate expected derivative for point (easiest to check)
-  boost::function<Vector(Point3)> f = //
-      boost::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, _1);
+  std::function<Vector(Point3)> f = //
+      std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
 
   // Calculate using computeEP
   Matrix actualE;
diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp
index 9fe087c2ff..ad88c88fcc 100644
--- a/gtsam/slam/tests/testTriangulationFactor.cpp
+++ b/gtsam/slam/tests/testTriangulationFactor.cpp
@@ -32,7 +32,7 @@
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 
 // Some common constants
 static const boost::shared_ptr<Cal3_S2> sharedCal = //
@@ -62,7 +62,7 @@ TEST( triangulation, TriangulationFactor ) {
   factor.evaluateError(landmark, HActual);
 
   Matrix HExpected = numericalDerivative11<Vector,Point3>(
-      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
+      std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(HExpected, HActual, 1e-3));
@@ -86,13 +86,15 @@ TEST( triangulation, TriangulationFactorStereo ) {
   factor.evaluateError(landmark, HActual);
 
   Matrix HExpected = numericalDerivative11<Vector,Point3>(
-      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
+      std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(HExpected, HActual, 1e-3));
 
   // compare same problem against expression factor
-  Expression<StereoPoint2>::UnaryFunction<Point3>::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2);
+  Expression<StereoPoint2>::UnaryFunction<Point3>::type f =
+      std::bind(&StereoCamera::project2, camera2, std::placeholders::_1,
+                boost::none, std::placeholders::_2);
   Expression<Point3> point_(pointKey);
   Expression<StereoPoint2> project2_(f, point_);
 
diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
index ede11c7fb5..882d5423ad 100644
--- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
+++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
@@ -3,14 +3,13 @@
  * @author Duy-Nguyen Ta
  */
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam_unstable/dynamics/SimpleHelicopter.h>
 
 /* ************************************************************************* */
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 
@@ -58,19 +57,28 @@ TEST( Reconstruction, evaluateError) {
       assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol));
 
   Matrix numericalH1 = numericalDerivative31(
-      boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
-          boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
-              boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
+      std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
+          std::bind(&Reconstruction::evaluateError, constraint,
+                    std::placeholders::_1, std::placeholders::_2,
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      g2, g1, V1_g1, 1e-5);
 
   Matrix numericalH2 = numericalDerivative32(
-      boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
-          boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
-              boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
+      std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
+          std::bind(&Reconstruction::evaluateError, constraint,
+                    std::placeholders::_1, std::placeholders::_2,
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      g2, g1, V1_g1, 1e-5);
 
   Matrix numericalH3 = numericalDerivative33(
-      boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
-          boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
-              boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
+      std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
+          std::bind(&Reconstruction::evaluateError, constraint,
+                    std::placeholders::_1, std::placeholders::_2,
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      g2, g1, V1_g1, 1e-5);
 
   EXPECT(assert_equal(numericalH1,H1,1e-5));
   EXPECT(assert_equal(numericalH2,H2,1e-5));
@@ -111,22 +119,22 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
   EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
 
   Matrix numericalH1 = numericalDerivative31(
-      boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
-          boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
+      std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
+          std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none)
           ),
           expectedv2, V1_g1, g2, 1e-5
       );
 
   Matrix numericalH2 = numericalDerivative32(
-      boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
-          boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
+      std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
+          std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none)
           ),
           expectedv2, V1_g1, g2, 1e-5
       );
 
   Matrix numericalH3 = numericalDerivative33(
-      boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
-          boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
+      std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
+          std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none)
           ),
           expectedv2, V1_g1, g2, 1e-5
       );
diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp
index 3a599e6c5f..f1bbc39704 100644
--- a/gtsam_unstable/geometry/tests/testEvent.cpp
+++ b/gtsam_unstable/geometry/tests/testEvent.cpp
@@ -23,9 +23,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -67,11 +65,11 @@ TEST(Event, Derivatives) {
   Matrix13 actualH2;
   kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
   Matrix expectedH1 = numericalDerivative11<double, Event>(
-      boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
+      std::bind(kToa, std::placeholders::_1, microphoneAt0, boost::none, boost::none),
       exampleEvent);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
   Matrix expectedH2 = numericalDerivative11<double, Point3>(
-      boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
+      std::bind(kToa, exampleEvent, std::placeholders::_1, boost::none, boost::none),
       microphoneAt0);
   EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
 }
diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
index 70368cc0e1..4d6e1912a1 100644
--- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
+++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
@@ -190,12 +190,12 @@ TEST (BetweenFactorEM, jacobian ) {
 //  CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
 
   double stepsize = 1.0e-9;
-  Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
-  Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
+  Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
+  Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
 
 
   // try to check numerical derivatives of a standard between factor
-  Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
+  Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
 //  CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
 //
 //
diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
index 0eb8b274b3..59c4fdf536 100644
--- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
+++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
@@ -10,10 +10,9 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam_unstable/slam/BiasedGPSFactor.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 using namespace gtsam::noiseModel;
@@ -68,15 +67,17 @@ TEST(BiasedGPSFactor, jacobian) {
   factor.evaluateError(pose,bias, actualH1, actualH2);
 
   Matrix numericalH1 = numericalDerivative21(
-      boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
-          &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
-          boost::none)), pose, bias, 1e-5);
+      std::function<Vector(const Pose3&, const Point3&)>(std::bind(
+          &BiasedGPSFactor::evaluateError, factor, std::placeholders::_1,
+          std::placeholders::_2, boost::none, boost::none)),
+      pose, bias, 1e-5);
   EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
 
   Matrix numericalH2 = numericalDerivative22(
-      boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
-          &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
-          boost::none)), pose, bias, 1e-5);
+      std::function<Vector(const Pose3&, const Point3&)>(std::bind(
+          &BiasedGPSFactor::evaluateError, factor, std::placeholders::_1,
+          std::placeholders::_2, boost::none, boost::none)),
+      pose, bias, 1e-5);
   EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
 }
 
diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
index 95b9b2f88f..644283512f 100644
--- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
+++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
@@ -26,7 +26,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include <iostream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
index 74134612d8..8692cf5841 100644
--- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
+++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
@@ -23,9 +23,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/deprecated/LieVector.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -108,9 +106,13 @@ TEST (GaussMarkovFactor, jacobian ) {
   // Calculate the Jacobian matrices H1 and H2 using the numerical derivative function
   Matrix numerical_H1, numerical_H2;
   numerical_H1 = numericalDerivative21<Vector3, Vector3, Vector3>(
-      boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
+      std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2,
+                factor),
+      v1_upd, v2_upd);
   numerical_H2 = numericalDerivative22<Vector3, Vector3, Vector3>(
-      boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
+      std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2,
+                factor),
+      v1_upd, v2_upd);
 
   // Verify they are equal for this choice of state
   CHECK( assert_equal(numerical_H1, computed_H1, 1e-9));
diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
index aae59035be..e68b2fe5f4 100644
--- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
+++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
@@ -16,7 +16,6 @@
  */
 
 #include <iostream>
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
@@ -26,7 +25,7 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -251,7 +250,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
 //
 //  Vector3 v(predictionRq(angles, q));
 //
-//  J_expected = numericalDerivative11<Vector3, Vector3>(boost::bind(&predictionRq, _1, q), angles);
+//  J_expected = numericalDerivative11<Vector3, Vector3>(std::bind(&predictionRq, std::placeholders::_1, q), angles);
 //
 //  cout<<"J_hyp"<<J_hyp<<endl;
 //  cout<<"J_expected"<<J_expected<<endl;
@@ -312,19 +311,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
       H5_expectedPose;
   H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
-      boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
       Pose1);
   H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
-      boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
       Vel1);
   H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
-      boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
       Bias1);
   H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
-      boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor),
+      std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
       Pose2);
   H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
-      boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor),
+      std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
       Vel2);
 
   // Verify they are equal for this choice of state
@@ -346,19 +345,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
       H5_expectedVel;
   H1_expectedVel = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
       Pose1);
   H2_expectedVel = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
       Vel1);
   H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
-      boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
       Bias1);
   H4_expectedVel = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor),
+      std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
       Pose2);
   H5_expectedVel = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor),
+      std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
       Vel2);
 
   // Verify they are equal for this choice of state
@@ -644,19 +643,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
       H5_expectedPose;
   H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
-      boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
       Pose1);
   H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
-      boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
       Vel1);
   H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
-      boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
       Bias1);
   H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
-      boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor),
+      std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
       Pose2);
   H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
-      boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor),
+      std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
       Vel2);
 
   // Verify they are equal for this choice of state
@@ -678,19 +677,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
       H5_expectedVel;
   H1_expectedVel = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
       Pose1);
   H2_expectedVel = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
       Vel1);
   H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
-      boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor),
+      std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
       Bias1);
   H4_expectedVel = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor),
+      std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
       Pose2);
   H5_expectedVel = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor),
+      std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
       Vel2);
 
   // Verify they are equal for this choice of state
diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
index 6bbfb55ae1..b97d56c7ea 100644
--- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
+++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
@@ -24,9 +24,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 
@@ -143,8 +141,10 @@ TEST(LocalOrientedPlane3Factor, Derivatives) {
   LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey);
 
   // Calculate numerical derivatives
-  auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor,
-    _1, _2, _3, boost::none, boost::none, boost::none);
+  auto f =
+      std::bind(&LocalOrientedPlane3Factor::evaluateError, factor,
+                std::placeholders::_1, std::placeholders::_2,
+                std::placeholders::_3, boost::none, boost::none, boost::none);
   Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3,
     OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
   Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3,
diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
index 2b99b8779a..7ba8a0d04b 100644
--- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
@@ -16,10 +16,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -83,7 +82,9 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) {
 
   // Calculate numerical derivatives.
   Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
-      boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose);
+      std::bind(&TestPartialPriorFactor2::evaluateError, &factor,
+                std::placeholders::_1, boost::none),
+      pose);
 
   // Use the factor to calculate the derivative.
   Matrix actualH1;
@@ -99,13 +100,16 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) {
   Pose2 measurement(-6.0, 3.5, 0.123);
 
   // Prior on x component of translation.
-  TestPartialPriorFactor2 factor(poseKey, { 0, 1 }, measurement.translation(), NM::Isotropic::Sigma(2, 0.25));
+  TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(),
+                                 NM::Isotropic::Sigma(2, 0.25));
 
   Pose2 pose = measurement; // Zero-error linearization point.
 
   // Calculate numerical derivatives.
   Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
-      boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose);
+      std::bind(&TestPartialPriorFactor2::evaluateError, &factor,
+                std::placeholders::_1, boost::none),
+      pose);
 
   // Use the factor to calculate the derivative.
   Matrix actualH1;
@@ -127,7 +131,7 @@ TEST(PartialPriorFactor, JacobianTheta) {
 
   // Calculate numerical derivatives.
   Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
-      boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose);
+      std::bind(&TestPartialPriorFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
 
   // Use the factor to calculate the derivative.
   Matrix actualH1;
@@ -178,7 +182,7 @@ TEST(PartialPriorFactor, JacobianAtIdentity3) {
 
   // Calculate numerical derivatives.
   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
+      std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
 
   // Use the factor to calculate the derivative.
   Matrix actualH1;
@@ -200,7 +204,7 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) {
 
   // Calculate numerical derivatives.
   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
+      std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
 
   // Use the factor to calculate the derivative.
   Matrix actualH1;
@@ -224,7 +228,7 @@ TEST(PartialPriorFactor, JacobianFullTranslation3) {
 
   // Calculate numerical derivatives.
   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
+      std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
 
   // Use the factor to calculate the derivative.
   Matrix actualH1;
@@ -248,7 +252,7 @@ TEST(PartialPriorFactor, JacobianTxTz3) {
 
   // Calculate numerical derivatives.
   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
+      std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
 
   // Use the factor to calculate the derivative.
   Matrix actualH1;
@@ -271,7 +275,7 @@ TEST(PartialPriorFactor, JacobianFullRotation3) {
 
   // Calculate numerical derivatives.
   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
+      std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
 
   // Use the factor to calculate the derivative.
   Matrix actualH1;
diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
index 6f7725eed1..cd5bf1d9e9 100644
--- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
@@ -22,10 +22,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -199,8 +198,14 @@ TEST( PoseBetweenFactor, Jacobian ) {
               Point3(-3.37493895, 6.14660244, -8.93650986));
 
   // Calculate numerical derivatives
-  Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
-  Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
+  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
+      std::bind(&TestPoseBetweenFactor::evaluateError, &factor,
+                std::placeholders::_1, pose2, boost::none, boost::none),
+      pose1);
+  Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
+      std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1,
+                std::placeholders::_1, boost::none, boost::none),
+      pose2);
 
   // Use the factor to calculate the derivative
   Matrix actualH1;
@@ -228,8 +233,14 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) {
               Point3(-3.5257579, 6.02637531, -8.98382384));
 
   // Calculate numerical derivatives
-  Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
-  Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
+  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
+      std::bind(&TestPoseBetweenFactor::evaluateError, &factor,
+                std::placeholders::_1, pose2, boost::none, boost::none),
+      pose1);
+  Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
+      std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1,
+                std::placeholders::_1, boost::none, boost::none),
+      pose2);
 
   // Use the factor to calculate the derivative
   Matrix actualH1;
diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
index cc26155172..dbbc02a8ba 100644
--- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
@@ -22,10 +22,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -188,7 +187,10 @@ TEST( PosePriorFactor, Jacobian ) {
   Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
 
   // Calculate numerical derivatives
-  Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
+  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
+      std::bind(&TestPosePriorFactor::evaluateError, &factor,
+                std::placeholders::_1, boost::none),
+      pose);
 
   // Use the factor to calculate the derivative
   Matrix actualH1;
@@ -212,7 +214,10 @@ TEST( PosePriorFactor, JacobianWithTransform ) {
              Point3(-4.74767676, 7.67044942, -11.00985));
 
   // Calculate numerical derivatives
-  Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
+  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
+      std::bind(&TestPosePriorFactor::evaluateError, &factor,
+                std::placeholders::_1, boost::none),
+      pose);
 
   // Use the factor to calculate the derivative
   Matrix actualH1;
diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h
index 8f8563e9db..e0e5c45817 100644
--- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h
+++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h
@@ -63,7 +63,7 @@ TEST(PoseToPointFactor, jacobian) {
   PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
 
   // Calculate numerical derivatives
-  auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
+  auto f = std::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
                        boost::none, boost::none);
   Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
   Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);
diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
index 8a65e5e571..c05f83a23b 100644
--- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
+++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
@@ -28,9 +28,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -178,10 +176,13 @@ TEST( ProjectionFactorPPP, Jacobian ) {
   CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
 
   // Verify H2 with numerical derivative
-  Matrix H2Expected = numericalDerivative32<Vector,Pose3, Pose3, Point3>(
-      boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
-      boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
-          boost::none, boost::none, boost::none)), pose, Pose3(), point);
+  Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
+      std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
+          std::bind(&TestProjectionFactor::evaluateError, &factor,
+                    std::placeholders::_1, std::placeholders::_2,
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      pose, Pose3(), point);
 
   CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
 }
@@ -214,9 +215,12 @@ TEST( ProjectionFactorPPP, JacobianWithTransform ) {
 
   // Verify H2 with numerical derivative
   Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
-      boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
-      boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
-          boost::none, boost::none, boost::none)), pose, body_P_sensor, point);
+      std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
+          std::bind(&TestProjectionFactor::evaluateError, &factor,
+                    std::placeholders::_1, std::placeholders::_2,
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      pose, body_P_sensor, point);
 
   CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
 
diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
index 232f8de17e..6490dfc753 100644
--- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
+++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
@@ -28,9 +28,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -138,12 +136,16 @@ TEST( ProjectionFactorPPPC, Jacobian ) {
 
   // Verify H2 and H4 with numerical derivatives
   Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
-          *K1, boost::none, boost::none, boost::none, boost::none), Pose3());
+      std::bind(&TestProjectionFactor::evaluateError, &factor, pose,
+                std::placeholders::_1, point, *K1, boost::none, boost::none,
+                boost::none, boost::none),
+      Pose3());
 
   Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
-      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point,
-          _1, boost::none, boost::none, boost::none, boost::none), *K1);
+      std::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(),
+                point, std::placeholders::_1, boost::none, boost::none,
+                boost::none, boost::none),
+      *K1);
 
   CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
   CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
@@ -174,12 +176,12 @@ TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
 
   // Verify H2 and H4 with numerical derivatives
   Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
-      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
+      std::bind(&TestProjectionFactor::evaluateError, &factor, pose, std::placeholders::_1, point,
           *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);
 
   Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
-      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
-          _1, boost::none, boost::none, boost::none, boost::none), *K1);
+      std::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
+          std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), *K1);
 
   CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
   CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
index 2fda9debb8..25ca3339b7 100644
--- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
+++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
@@ -5,14 +5,13 @@
  * @author Alex Cunningham
  */
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam_unstable/slam/RelativeElevationFactor.h>
 
 #include <gtsam/base/numericalDerivative.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 
 SharedNoiseModel model1 = noiseModel::Unit::Create(1);
@@ -37,10 +36,14 @@ TEST( testRelativeElevationFactor, level_zero_error ) {
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
   EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2)));
-  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
-  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose1, point1, 1e-5);
+  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose1, point1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
   EXPECT(assert_equal(expH2, actH2, tol));
 }
@@ -52,10 +55,14 @@ TEST( testRelativeElevationFactor, level_positive ) {
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
   EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
-  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
-  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose1, point1, 1e-5);
+  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose1, point1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
   EXPECT(assert_equal(expH2, actH2, tol));
 }
@@ -67,10 +74,14 @@ TEST( testRelativeElevationFactor, level_negative ) {
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
   EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
-  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
-  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose1, point1, 1e-5);
+  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose1, point1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
   EXPECT(assert_equal(expH2, actH2, tol));
 }
@@ -82,10 +93,14 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) {
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
   EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2)));
-  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
-  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose2, point1, 1e-5);
+  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose2, point1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
   EXPECT(assert_equal(expH2, actH2, tol));
 }
@@ -97,10 +112,14 @@ TEST( testRelativeElevationFactor, rotated_positive ) {
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
   EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
-  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
-  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose2, point1, 1e-5);
+  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose2, point1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
   EXPECT(assert_equal(expH2, actH2, tol));
 }
@@ -112,10 +131,14 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) {
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
   EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
-  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
-  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose2, point1, 1e-5);
+  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose2, point1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
   EXPECT(assert_equal(expH2, actH2, tol));
 }
@@ -127,10 +150,14 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) {
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
   EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2)));
-  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
-  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
-      boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
+  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose3, point1, 1e-5);
+  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
+      std::bind(evalFactorError, factor, std::placeholders::_1,
+                std::placeholders::_2),
+      pose3, point1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
   EXPECT(assert_equal(expH2, actH2, tol));
 }
diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp
index 77f82bca49..fbb21e191c 100644
--- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp
+++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp
@@ -19,10 +19,9 @@
 #include <gtsam_unstable/slam/TSAMFactors.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -48,10 +47,10 @@ TEST( DeltaFactor, all ) {
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
   H1Expected = numericalDerivative11<Vector2, Pose2>(
-      boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none,
+      std::bind(&DeltaFactor::evaluateError, &factor, std::placeholders::_1, point, boost::none,
           boost::none), pose);
   H2Expected = numericalDerivative11<Vector2, Point2>(
-      boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none,
+      std::bind(&DeltaFactor::evaluateError, &factor, pose, std::placeholders::_1, boost::none,
           boost::none), point);
 
   // Verify the Jacobians are correct
@@ -82,17 +81,17 @@ TEST( DeltaFactorBase, all ) {
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected, H3Expected, H4Expected;
   H1Expected = numericalDerivative11<Vector2, Pose2>(
-      boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2,
+      std::bind(&DeltaFactorBase::evaluateError, &factor, std::placeholders::_1, pose, base2,
           point, boost::none, boost::none, boost::none, boost::none), base1);
   H2Expected = numericalDerivative11<Vector2, Pose2>(
-      boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2,
+      std::bind(&DeltaFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2,
           point, boost::none, boost::none, boost::none, boost::none), pose);
   H3Expected = numericalDerivative11<Vector2, Pose2>(
-      boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1,
+      std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, std::placeholders::_1,
           point, boost::none, boost::none, boost::none, boost::none), base2);
   H4Expected = numericalDerivative11<Vector2, Point2>(
-      boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2,
-          _1, boost::none, boost::none, boost::none, boost::none), point);
+      std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2,
+          std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), point);
 
   // Verify the Jacobians are correct
   EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
@@ -123,17 +122,17 @@ TEST( OdometryFactorBase, all ) {
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected, H3Expected, H4Expected;
   H1Expected = numericalDerivative11<Vector3, Pose2>(
-      boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2,
+      std::bind(&OdometryFactorBase::evaluateError, &factor, std::placeholders::_1, pose1, base2,
           pose2, boost::none, boost::none, boost::none, boost::none), base1);
   H2Expected = numericalDerivative11<Vector3, Pose2>(
-      boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2,
+      std::bind(&OdometryFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2,
           pose2, boost::none, boost::none, boost::none, boost::none), pose1);
   H3Expected = numericalDerivative11<Vector3, Pose2>(
-      boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1,
+      std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, std::placeholders::_1,
           pose2, boost::none, boost::none, boost::none, boost::none), base2);
   H4Expected = numericalDerivative11<Vector3, Pose2>(
-      boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1,
-          base2, _1, boost::none, boost::none, boost::none, boost::none),
+      std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1,
+          base2, std::placeholders::_1, boost::none, boost::none, boost::none, boost::none),
       pose2);
 
   // Verify the Jacobians are correct
diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
index d9e945b78c..36914f88f2 100644
--- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
+++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
@@ -18,9 +18,7 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -233,7 +231,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
   Matrix H1_actual = H_actual[0];
 
   double stepsize = 1.0e-9;
-  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1,  key, g), orgA_T_orgB, stepsize);
+  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1,  key, g), orgA_T_orgB, stepsize);
 //  CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
 }
 
@@ -287,12 +285,12 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
 ////  CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
 //
 //  double stepsize = 1.0e-9;
-//  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
-//  Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
+//  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
+//  Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize);
 //
 //
 //  // try to check numerical derivatives of a standard between factor
-//  Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
+//  Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
 //  CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
 //
 //
diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
index 2fd282091a..657a9fb348 100644
--- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
+++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
@@ -18,9 +18,7 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 
-#include <boost/bind/bind.hpp>
-
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -262,8 +260,10 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
   Matrix H1_actual = H_actual[0];
 
   double stepsize = 1.0e-9;
-  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1,  key, g), orgA_T_orgB, stepsize);
-//  CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
+  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(
+      std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB,
+      stepsize);
+  //  CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
 }
 /////* ************************************************************************** */
 //TEST (TransformBtwRobotsUnaryFactorEM, jacobian ) {
@@ -312,12 +312,12 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
 ////  CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
 //
 //  double stepsize = 1.0e-9;
-//  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
-//  Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
+//  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
+//  Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize);
 //
 //
 //  // try to check numerical derivatives of a standard between factor
-//  Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
+//  Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
 //  CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
 //
 //
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index 6bb5751bf6..e9eac22ebb 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -17,22 +17,19 @@
  * @brief unit tests for Block Automatic Differentiation
  */
 
-#include <gtsam/slam/expressions.h>
-#include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/nonlinear/PriorFactor.h>
-#include <gtsam/nonlinear/expressionTesting.h>
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/nonlinear/expressionTesting.h>
-#include <gtsam/base/Testable.h>
-
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+#include <gtsam/slam/expressions.h>
 
 #include <boost/assign/list_of.hpp>
 using boost::assign::list_of;
-#include <boost/bind/bind.hpp>
-using namespace boost::placeholders;
+using namespace std::placeholders;
 
 using namespace std;
 using namespace gtsam;
@@ -621,9 +618,10 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
   Matrix3 A;
   const Vector Ab = f(a, b, H1, A);
   CHECK(assert_equal(A * b, Ab));
-  CHECK(assert_equal(numericalDerivative11<Vector3, Point2>(
-                         boost::bind(f, _1, b, boost::none, boost::none), a),
-                     H1));
+  CHECK(assert_equal(
+      numericalDerivative11<Vector3, Point2>(
+          std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a),
+      H1));
 
   Values values;
   values.insert<Point2>(0, a);
diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp
index 342c353bca..2bc381f7a4 100644
--- a/tests/testSimulated3D.cpp
+++ b/tests/testSimulated3D.cpp
@@ -26,7 +26,7 @@
 
 #include <iostream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 
 // Convenience for named keys
@@ -46,7 +46,7 @@ TEST( simulated3D, Values )
 TEST( simulated3D, Dprior )
 {
   Point3 x(1,-9, 7);
-  Matrix numerical = numericalDerivative11<Point3, Point3>(boost::bind(simulated3D::prior, _1, boost::none),x);
+  Matrix numerical = numericalDerivative11<Point3, Point3>(std::bind(simulated3D::prior, std::placeholders::_1, boost::none),x);
   Matrix computed;
   simulated3D::prior(x,computed);
   EXPECT(assert_equal(numerical,computed,1e-9));
@@ -55,13 +55,19 @@ TEST( simulated3D, Dprior )
 /* ************************************************************************* */
 TEST( simulated3D, DOdo )
 {
-  Point3 x1(1,-9,7),x2(-5,6,7);
-  Matrix H1,H2;
-  simulated3D::odo(x1,x2,H1,H2);
-  Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
-  EXPECT(assert_equal(A1,H1,1e-9));
-  Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
-  EXPECT(assert_equal(A2,H2,1e-9));
+  Point3 x1(1, -9, 7), x2(-5, 6, 7);
+  Matrix H1, H2;
+  simulated3D::odo(x1, x2, H1, H2);
+  Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(
+      std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none),
+      x1, x2);
+  EXPECT(assert_equal(A1, H1, 1e-9));
+  Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(
+      std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none),
+      x1, x2);
+  EXPECT(assert_equal(A2, H2, 1e-9));
 }
 
 

From ddfb45efb03544636f69268f85a703816a730c57 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sun, 11 Jul 2021 01:02:36 -0600
Subject: [PATCH 43/72] fix typo in block indexing, 3x3 covariance for Pose2
 should have just 1x1 block for theta

---
 gtsam/sfm/ShonanAveraging.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index 88c393f16d..76fd1bfc70 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -955,7 +955,7 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
         "parseMeasurements<Rot2> can only convert Pose2 measurements "
         "with Gaussian noise models.");
   const Matrix3 M = gaussian->covariance();
-  auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2));
+  auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2));
   return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
                                  model);
 }

From 941594c94be1d1223d7fbcf9d1181f2c73411574 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Sun, 11 Jul 2021 14:11:40 +0200
Subject: [PATCH 44/72] Testing CameraSet and triangulatePoint3

Currently triangulatePoint3 returns wrong results for fisheye models. The template for PinholePose may be implemented for a fixed size of variable dimensions.
---
 python/gtsam/tests/test_Cal3Fisheye.py | 40 +++++++++++++++++++++++++-
 1 file changed, 39 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py
index 23f7a9b8c3..d731204ef8 100644
--- a/python/gtsam/tests/test_Cal3Fisheye.py
+++ b/python/gtsam/tests/test_Cal3Fisheye.py
@@ -29,7 +29,7 @@ def setUpClass(cls):
         image plane and theta the incident angle of the object point.
         """
         x, y, z = 1.0, 0.0, 1.0
-        # x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails!
+        # x, y, z = 0.5, 0.0, 2.0
         u, v = np.arctan2(x, z), 0.0
         cls.obj_point = np.array([x, y, z])
         cls.img_point = np.array([u, v])
@@ -93,6 +93,44 @@ def test_sfm_factor2(self):
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)
 
+    @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
+    def test_triangulation_skipped(self):
+        """Estimate spatial point from image measurements"""
+        p1 = [-1.0, 0.0, -1.0]
+        p2 = [ 1.0, 0.0, -1.0]
+        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        obj_point = np.array([0.0, 0.0, 0.0])
+        pose1 = gtsam.Pose3(q1, p1)
+        pose2 = gtsam.Pose3(q2, p2)
+        camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
+        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
+        cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
+        measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
+        triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True)
+        self.gtsamAssertEquals(measurements[0], self.img_point)
+        self.gtsamAssertEquals(triangulated, obj_point)
+
+    def test_triangulation_rectify(self):
+        """Estimate spatial point from image measurements using rectification"""
+        p1 = [-1.0, 0.0, -1.0]
+        p2 = [ 1.0, 0.0, -1.0]
+        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        obj_point = np.array([0.0, 0.0, 0.0])
+        pose1 = gtsam.Pose3(q1, p1)
+        pose2 = gtsam.Pose3(q2, p2)
+        camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
+        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
+        cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
+        measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
+        rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)])
+        shared_cal = gtsam.Cal3_S2()
+        poses = gtsam.Pose3Vector([pose1, pose2])
+        triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
+        self.gtsamAssertEquals(measurements[0], self.img_point)
+        self.gtsamAssertEquals(triangulated, obj_point)
+
     def test_retract(self):
         expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
                                      1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10)

From c595767caeba831701298ab410a693048b2fe1db Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Sun, 11 Jul 2021 14:14:08 +0200
Subject: [PATCH 45/72] Unittest, triangulation for Cal3Unified

---
 python/gtsam/tests/test_Cal3Unified.py | 38 ++++++++++++++++++++++++++
 1 file changed, 38 insertions(+)

diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index f2c1ada488..0b09fc3ae7 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -105,6 +105,44 @@ def test_sfm_factor2(self):
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)
 
+    @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
+    def test_triangulation(self):
+        """Estimate spatial point from image measurements"""
+        p1 = [-1.0, 0.0, -1.0]
+        p2 = [ 1.0, 0.0, -1.0]
+        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        obj_point = np.array([0.0, 0.0, 0.0])
+        pose1 = gtsam.Pose3(q1, p1)
+        pose2 = gtsam.Pose3(q2, p2)
+        camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic)
+        camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic)
+        cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
+        measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
+        triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True)
+        self.gtsamAssertEquals(measurements[0], self.img_point)
+        self.gtsamAssertEquals(triangulated, obj_point)
+
+    def test_triangulation_rectify(self):
+        """Estimate spatial point from image measurements using rectification"""
+        p1 = [-1.0, 0.0, -1.0]
+        p2 = [ 1.0, 0.0, -1.0]
+        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        obj_point = np.array([0.0, 0.0, 0.0])
+        pose1 = gtsam.Pose3(q1, p1)
+        pose2 = gtsam.Pose3(q2, p2)
+        camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic)
+        camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic)
+        cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
+        measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
+        rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)])
+        shared_cal = gtsam.Cal3_S2()
+        poses = gtsam.Pose3Vector([pose1, pose2])
+        triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
+        self.gtsamAssertEquals(measurements[0], self.img_point)
+        self.gtsamAssertEquals(triangulated, obj_point)
+
     def test_retract(self):
         expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
                                      1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)

From 9bafebb5218f40ec40dfbaa2b0f7576fe27ec683 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:09:59 -0700
Subject: [PATCH 46/72] break interface file into multiple files

---
 gtsam/base/base.i             |  113 ++
 gtsam/geometry/geometry.i     |  942 +++++++++
 gtsam/gtsam.i                 | 3520 +--------------------------------
 gtsam/linear/linear.i         |  653 ++++++
 gtsam/navigation/navigation.i |  355 ++++
 gtsam/nonlinear/nonlinear.i   |  769 +++++++
 gtsam/sam/sam.i               |   96 +
 gtsam/sfm/sfm.i               |  209 ++
 gtsam/slam/slam.i             |  325 +++
 gtsam/symbolic/symbolic.i     |  201 ++
 10 files changed, 3709 insertions(+), 3474 deletions(-)
 create mode 100644 gtsam/base/base.i
 create mode 100644 gtsam/geometry/geometry.i
 create mode 100644 gtsam/linear/linear.i
 create mode 100644 gtsam/navigation/navigation.i
 create mode 100644 gtsam/nonlinear/nonlinear.i
 create mode 100644 gtsam/sam/sam.i
 create mode 100644 gtsam/sfm/sfm.i
 create mode 100644 gtsam/slam/slam.i
 create mode 100644 gtsam/symbolic/symbolic.i

diff --git a/gtsam/base/base.i b/gtsam/base/base.i
new file mode 100644
index 0000000000..09278ff5b7
--- /dev/null
+++ b/gtsam/base/base.i
@@ -0,0 +1,113 @@
+//*************************************************************************
+// base
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/StereoPoint2.h>
+#include <gtsam/navigation/ImuBias.h>
+
+// #####
+
+#include <gtsam/base/debug.h>
+bool isDebugVersion();
+
+#include <gtsam/base/DSFMap.h>
+class IndexPair {
+  IndexPair();
+  IndexPair(size_t i, size_t j);
+  size_t i() const;
+  size_t j() const;
+};
+
+// template<KEY = {gtsam::IndexPair}>
+// class DSFMap {
+//   DSFMap();
+//   KEY find(const KEY& key) const;
+//   void merge(const KEY& x, const KEY& y);
+//   std::map<KEY, Set> sets();
+// };
+
+class IndexPairSet {
+  IndexPairSet();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(gtsam::IndexPair key);
+  bool erase(gtsam::IndexPair key);        // returns true if value was removed
+  bool count(gtsam::IndexPair key) const;  // returns true if value exists
+};
+
+class IndexPairVector {
+  IndexPairVector();
+  IndexPairVector(const gtsam::IndexPairVector& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPair at(size_t i) const;
+  void push_back(gtsam::IndexPair key) const;
+};
+
+gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
+
+class IndexPairSetMap {
+  IndexPairSetMap();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPairSet at(gtsam::IndexPair& key);
+};
+
+class DSFMapIndexPair {
+  DSFMapIndexPair();
+  gtsam::IndexPair find(const gtsam::IndexPair& key) const;
+  void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y);
+  gtsam::IndexPairSetMap sets();
+};
+
+#include <gtsam/base/Matrix.h>
+bool linear_independent(Matrix A, Matrix B, double tol);
+
+#include <gtsam/base/Value.h>
+virtual class Value {
+  // No constructors because this is an abstract class
+
+  // Testable
+  void print(string s = "") const;
+
+  // Manifold
+  size_t dim() const;
+};
+
+#include <gtsam/base/GenericValue.h>
+template <T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2,
+               gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2,
+               gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
+               gtsam::EssentialMatrix, gtsam::CalibratedCamera,
+               gtsam::imuBias::ConstantBias}>
+virtual class GenericValue : gtsam::Value {
+  void serializable() const;
+};
+
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
new file mode 100644
index 0000000000..a39c227973
--- /dev/null
+++ b/gtsam/geometry/geometry.i
@@ -0,0 +1,942 @@
+//*************************************************************************
+// geometry
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/geometry/Point2.h>
+class Point2 {
+  // Standard Constructors
+  Point2();
+  Point2(double x, double y);
+  Point2(Vector v);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Point2& point, double tol) const;
+
+  // Group
+  static gtsam::Point2 identity();
+
+  // Standard Interface
+  double x() const;
+  double y() const;
+  Vector vector() const;
+  double distance(const gtsam::Point2& p2) const;
+  double norm() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+// std::vector<gtsam::Point2>
+class Point2Vector {
+  // Constructors
+  Point2Vector();
+  Point2Vector(const gtsam::Point2Vector& v);
+
+  // Capacity
+  size_t size() const;
+  size_t max_size() const;
+  void resize(size_t sz);
+  size_t capacity() const;
+  bool empty() const;
+  void reserve(size_t n);
+
+  // Element access
+  gtsam::Point2 at(size_t n) const;
+  gtsam::Point2 front() const;
+  gtsam::Point2 back() const;
+
+  // Modifiers
+  void assign(size_t n, const gtsam::Point2& u);
+  void push_back(const gtsam::Point2& x);
+  void pop_back();
+};
+
+#include <gtsam/geometry/StereoPoint2.h>
+class StereoPoint2 {
+  // Standard Constructors
+  StereoPoint2();
+  StereoPoint2(double uL, double uR, double v);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::StereoPoint2& point, double tol) const;
+
+  // Group
+  static gtsam::StereoPoint2 identity();
+  gtsam::StereoPoint2 inverse() const;
+  gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
+  gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
+
+  // Operator Overloads
+  gtsam::StereoPoint2 operator-() const;
+  // gtsam::StereoPoint2 operator+(Vector b) const;  //TODO Mixed types not yet
+  // supported
+  gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const;
+  gtsam::StereoPoint2 operator-(const gtsam::StereoPoint2& p2) const;
+
+  // Manifold
+  gtsam::StereoPoint2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::StereoPoint2& p) const;
+
+  // Lie Group
+  static gtsam::StereoPoint2 Expmap(Vector v);
+  static Vector Logmap(const gtsam::StereoPoint2& p);
+
+  // Standard Interface
+  Vector vector() const;
+  double uL() const;
+  double uR() const;
+  double v() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Point3.h>
+class Point3 {
+  // Standard Constructors
+  Point3();
+  Point3(double x, double y, double z);
+  Point3(Vector v);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Point3& p, double tol) const;
+
+  // Group
+  static gtsam::Point3 identity();
+
+  // Standard Interface
+  Vector vector() const;
+  double x() const;
+  double y() const;
+  double z() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+class Point3Pairs {
+  Point3Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Point3Pair at(size_t n) const;
+  void push_back(const gtsam::Point3Pair& point_pair);
+};
+
+#include <gtsam/geometry/Rot2.h>
+class Rot2 {
+  // Standard Constructors and Named Constructors
+  Rot2();
+  Rot2(double theta);
+  static gtsam::Rot2 fromAngle(double theta);
+  static gtsam::Rot2 fromDegrees(double theta);
+  static gtsam::Rot2 fromCosSin(double c, double s);
+
+  // Testable
+  void print(string s = "theta") const;
+  bool equals(const gtsam::Rot2& rot, double tol) const;
+
+  // Group
+  static gtsam::Rot2 identity();
+  gtsam::Rot2 inverse();
+  gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
+  gtsam::Rot2 between(const gtsam::Rot2& p2) const;
+
+  // Operator Overloads
+  gtsam::Rot2 operator*(const gtsam::Rot2& p2) const;
+
+  // Manifold
+  gtsam::Rot2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Rot2& p) const;
+
+  // Lie Group
+  static gtsam::Rot2 Expmap(Vector v);
+  static Vector Logmap(const gtsam::Rot2& p);
+  Vector logmap(const gtsam::Rot2& p);
+
+  // Group Action on Point2
+  gtsam::Point2 rotate(const gtsam::Point2& point) const;
+  gtsam::Point2 unrotate(const gtsam::Point2& point) const;
+
+  // Standard Interface
+  static gtsam::Rot2 relativeBearing(
+      const gtsam::Point2& d);  // Ignoring derivative
+  static gtsam::Rot2 atan2(double y, double x);
+  double theta() const;
+  double degrees() const;
+  double c() const;
+  double s() const;
+  Matrix matrix() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/SO3.h>
+class SO3 {
+  // Standard Constructors
+  SO3();
+  SO3(Matrix R);
+  static gtsam::SO3 FromMatrix(Matrix R);
+  static gtsam::SO3 AxisAngle(const Vector axis, double theta);
+  static gtsam::SO3 ClosestTo(const Matrix M);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::SO3& other, double tol) const;
+
+  // Group
+  static gtsam::SO3 identity();
+  gtsam::SO3 inverse() const;
+  gtsam::SO3 between(const gtsam::SO3& R) const;
+  gtsam::SO3 compose(const gtsam::SO3& R) const;
+
+  // Operator Overloads
+  gtsam::SO3 operator*(const gtsam::SO3& R) const;
+
+  // Manifold
+  gtsam::SO3 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::SO3& R) const;
+  static gtsam::SO3 Expmap(Vector v);
+
+  // Other methods
+  Vector vec() const;
+  Matrix matrix() const;
+};
+
+#include <gtsam/geometry/SO4.h>
+class SO4 {
+  // Standard Constructors
+  SO4();
+  SO4(Matrix R);
+  static gtsam::SO4 FromMatrix(Matrix R);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::SO4& other, double tol) const;
+
+  // Group
+  static gtsam::SO4 identity();
+  gtsam::SO4 inverse() const;
+  gtsam::SO4 between(const gtsam::SO4& Q) const;
+  gtsam::SO4 compose(const gtsam::SO4& Q) const;
+
+  // Operator Overloads
+  gtsam::SO4 operator*(const gtsam::SO4& Q) const;
+
+  // Manifold
+  gtsam::SO4 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::SO4& Q) const;
+  static gtsam::SO4 Expmap(Vector v);
+
+  // Other methods
+  Vector vec() const;
+  Matrix matrix() const;
+};
+
+#include <gtsam/geometry/SOn.h>
+class SOn {
+  // Standard Constructors
+  SOn(size_t n);
+  static gtsam::SOn FromMatrix(Matrix R);
+  static gtsam::SOn Lift(size_t n, Matrix R);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::SOn& other, double tol) const;
+
+  // Group
+  static gtsam::SOn identity();
+  gtsam::SOn inverse() const;
+  gtsam::SOn between(const gtsam::SOn& Q) const;
+  gtsam::SOn compose(const gtsam::SOn& Q) const;
+
+  // Operator Overloads
+  gtsam::SOn operator*(const gtsam::SOn& Q) const;
+
+  // Manifold
+  gtsam::SOn retract(Vector v) const;
+  Vector localCoordinates(const gtsam::SOn& Q) const;
+  static gtsam::SOn Expmap(Vector v);
+
+  // Other methods
+  Vector vec() const;
+  Matrix matrix() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Quaternion.h>
+class Quaternion {
+  double w() const;
+  double x() const;
+  double y() const;
+  double z() const;
+  Vector coeffs() const;
+};
+
+#include <gtsam/geometry/Rot3.h>
+class Rot3 {
+  // Standard Constructors and Named Constructors
+  Rot3();
+  Rot3(Matrix R);
+  Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2,
+       const gtsam::Point3& col3);
+  Rot3(double R11, double R12, double R13, double R21, double R22, double R23,
+       double R31, double R32, double R33);
+  Rot3(double w, double x, double y, double z);
+
+  static gtsam::Rot3 Rx(double t);
+  static gtsam::Rot3 Ry(double t);
+  static gtsam::Rot3 Rz(double t);
+  static gtsam::Rot3 RzRyRx(double x, double y, double z);
+  static gtsam::Rot3 RzRyRx(Vector xyz);
+  static gtsam::Rot3 Yaw(
+      double t);  // positive yaw is to right (as in aircraft heading)
+  static gtsam::Rot3 Pitch(
+      double t);  // positive pitch is up (increasing aircraft altitude)
+  static gtsam::Rot3 Roll(
+      double t);  // positive roll is to right (increasing yaw in aircraft)
+  static gtsam::Rot3 Ypr(double y, double p, double r);
+  static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
+  static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle);
+  static gtsam::Rot3 Rodrigues(Vector v);
+  static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
+  static gtsam::Rot3 ClosestTo(const Matrix M);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Rot3& rot, double tol) const;
+
+  // Group
+  static gtsam::Rot3 identity();
+  gtsam::Rot3 inverse() const;
+  gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
+  gtsam::Rot3 between(const gtsam::Rot3& p2) const;
+
+  // Operator Overloads
+  gtsam::Rot3 operator*(const gtsam::Rot3& p2) const;
+
+  // Manifold
+  // gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both
+  // Matrix and Quaternion options
+  gtsam::Rot3 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Rot3& p) const;
+
+  // Group Action on Point3
+  gtsam::Point3 rotate(const gtsam::Point3& p) const;
+  gtsam::Point3 unrotate(const gtsam::Point3& p) const;
+
+  // Standard Interface
+  static gtsam::Rot3 Expmap(Vector v);
+  static Vector Logmap(const gtsam::Rot3& p);
+  Vector logmap(const gtsam::Rot3& p);
+  Matrix matrix() const;
+  Matrix transpose() const;
+  gtsam::Point3 column(size_t index) const;
+  Vector xyz() const;
+  Vector ypr() const;
+  Vector rpy() const;
+  double roll() const;
+  double pitch() const;
+  double yaw() const;
+  pair<gtsam::Unit3, double> axisAngle() const;
+  gtsam::Quaternion toQuaternion() const;
+  Vector quaternion() const;
+  gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Pose2.h>
+class Pose2 {
+  // Standard Constructor
+  Pose2();
+  Pose2(const gtsam::Pose2& other);
+  Pose2(double x, double y, double theta);
+  Pose2(double theta, const gtsam::Point2& t);
+  Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
+  Pose2(Vector v);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Pose2& pose, double tol) const;
+
+  // Group
+  static gtsam::Pose2 identity();
+  gtsam::Pose2 inverse() const;
+  gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
+  gtsam::Pose2 between(const gtsam::Pose2& p2) const;
+
+  // Operator Overloads
+  gtsam::Pose2 operator*(const gtsam::Pose2& p2) const;
+
+  // Manifold
+  gtsam::Pose2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Pose2& p) const;
+
+  // Lie Group
+  static gtsam::Pose2 Expmap(Vector v);
+  static Vector Logmap(const gtsam::Pose2& p);
+  Vector logmap(const gtsam::Pose2& p);
+  static Matrix ExpmapDerivative(Vector v);
+  static Matrix LogmapDerivative(const gtsam::Pose2& v);
+  Matrix AdjointMap() const;
+  Vector Adjoint(Vector xi) const;
+  static Matrix adjointMap_(Vector v);
+  static Vector adjoint_(Vector xi, Vector y);
+  static Vector adjointTranspose(Vector xi, Vector y);
+  static Matrix wedge(double vx, double vy, double w);
+
+  // Group Actions on Point2
+  gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
+  gtsam::Point2 transformTo(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double x() const;
+  double y() const;
+  double theta() const;
+  gtsam::Rot2 bearing(const gtsam::Point2& point) const;
+  double range(const gtsam::Point2& point) const;
+  gtsam::Point2 translation() const;
+  gtsam::Rot2 rotation() const;
+  Matrix matrix() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Pose3.h>
+class Pose3 {
+  // Standard Constructors
+  Pose3();
+  Pose3(const gtsam::Pose3& other);
+  Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
+  Pose3(const gtsam::Pose2& pose2);
+  Pose3(Matrix mat);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Pose3& pose, double tol) const;
+
+  // Group
+  static gtsam::Pose3 identity();
+  gtsam::Pose3 inverse() const;
+  gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
+  gtsam::Pose3 between(const gtsam::Pose3& pose) const;
+
+  // Operator Overloads
+  gtsam::Pose3 operator*(const gtsam::Pose3& pose) const;
+
+  // Manifold
+  gtsam::Pose3 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Pose3& pose) const;
+
+  // Lie Group
+  static gtsam::Pose3 Expmap(Vector v);
+  static Vector Logmap(const gtsam::Pose3& pose);
+  Vector logmap(const gtsam::Pose3& pose);
+  Matrix AdjointMap() const;
+  Vector Adjoint(Vector xi) const;
+  static Matrix adjointMap_(Vector xi);
+  static Vector adjoint_(Vector xi, Vector y);
+  static Vector adjointTranspose(Vector xi, Vector y);
+  static Matrix ExpmapDerivative(Vector xi);
+  static Matrix LogmapDerivative(const gtsam::Pose3& xi);
+  static Matrix wedge(double wx, double wy, double wz, double vx, double vy,
+                      double vz);
+
+  // Group Action on Point3
+  gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
+  gtsam::Point3 transformTo(const gtsam::Point3& point) const;
+
+  // Standard Interface
+  gtsam::Rot3 rotation() const;
+  gtsam::Point3 translation() const;
+  double x() const;
+  double y() const;
+  double z() const;
+  Matrix matrix() const;
+  gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
+  gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Pose3& pose);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+class Pose3Pairs {
+  Pose3Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Pose3Pair at(size_t n) const;
+  void push_back(const gtsam::Pose3Pair& pose_pair);
+};
+
+class Pose3Vector {
+  Pose3Vector();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Pose3 at(size_t n) const;
+  void push_back(const gtsam::Pose3& pose);
+};
+
+#include <gtsam/geometry/Unit3.h>
+class Unit3 {
+  // Standard Constructors
+  Unit3();
+  Unit3(const gtsam::Point3& pose);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Unit3& pose, double tol) const;
+
+  // Other functionality
+  Matrix basis() const;
+  Matrix skew() const;
+  gtsam::Point3 point3() const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Unit3 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Unit3& s) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::Unit3& expected, double tol) const;
+};
+
+#include <gtsam/geometry/EssentialMatrix.h>
+class EssentialMatrix {
+  // Standard Constructors
+  EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::EssentialMatrix retract(Vector v) const;
+  Vector localCoordinates(const gtsam::EssentialMatrix& s) const;
+
+  // Other methods:
+  gtsam::Rot3 rotation() const;
+  gtsam::Unit3 direction() const;
+  Matrix matrix() const;
+  double error(Vector vA, Vector vB);
+};
+
+#include <gtsam/geometry/Cal3_S2.h>
+class Cal3_S2 {
+  // Standard Constructors
+  Cal3_S2();
+  Cal3_S2(double fx, double fy, double s, double u0, double v0);
+  Cal3_S2(Vector v);
+  Cal3_S2(double fov, int w, int h);
+
+  // Testable
+  void print(string s = "Cal3_S2") const;
+  bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3_S2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3_S2& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  Vector vector() const;
+  Matrix K() const;
+  Matrix inverse() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Cal3DS2_Base.h>
+virtual class Cal3DS2_Base {
+  // Standard Constructors
+  Cal3DS2_Base();
+
+  // Testable
+  void print(string s = "") const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  double k1() const;
+  double k2() const;
+  Matrix K() const;
+  Vector k() const;
+  Vector vector() const;
+
+  // Action on Point2
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Cal3DS2.h>
+virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
+  // Standard Constructors
+  Cal3DS2();
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
+          double k2);
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
+          double k2, double p1, double p2);
+  Cal3DS2(Vector v);
+
+  // Testable
+  bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
+
+  // Manifold
+  size_t dim() const;
+  static size_t Dim();
+  gtsam::Cal3DS2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3DS2& c) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Cal3Unified.h>
+virtual class Cal3Unified : gtsam::Cal3DS2_Base {
+  // Standard Constructors
+  Cal3Unified();
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
+              double k2);
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
+              double k2, double p1, double p2, double xi);
+  Cal3Unified(Vector v);
+
+  // Testable
+  bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
+
+  // Standard Interface
+  double xi() const;
+  gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
+  gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
+
+  // Manifold
+  size_t dim() const;
+  static size_t Dim();
+  gtsam::Cal3Unified retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Unified& c) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+class Cal3_S2Stereo {
+  // Standard Constructors
+  Cal3_S2Stereo();
+  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
+  Cal3_S2Stereo(Vector v);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  double baseline() const;
+};
+
+#include <gtsam/geometry/Cal3Bundler.h>
+class Cal3Bundler {
+  // Standard Constructors
+  Cal3Bundler();
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0,
+              double tol);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3Bundler retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double k1() const;
+  double k2() const;
+  double px() const;
+  double py() const;
+  Vector vector() const;
+  Vector k() const;
+  Matrix K() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/CalibratedCamera.h>
+class CalibratedCamera {
+  // Standard Constructors and Named Constructors
+  CalibratedCamera();
+  CalibratedCamera(const gtsam::Pose3& pose);
+  CalibratedCamera(Vector v);
+  static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2,
+                                       double height);
+
+  // Testable
+  void print(string s = "CalibratedCamera") const;
+  bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::CalibratedCamera retract(Vector d) const;
+  Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
+
+  // Action on Point3
+  gtsam::Point2 project(const gtsam::Point3& point) const;
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  double range(const gtsam::Point3& point) const;
+  double range(const gtsam::Pose3& pose) const;
+  double range(const gtsam::CalibratedCamera& camera) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/PinholeCamera.h>
+template <CALIBRATION>
+class PinholeCamera {
+  // Standard Constructors and Named Constructors
+  PinholeCamera();
+  PinholeCamera(const gtsam::Pose3& pose);
+  PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
+  static This Level(const CALIBRATION& K, const gtsam::Pose2& pose,
+                    double height);
+  static This Level(const gtsam::Pose2& pose, double height);
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+                     const gtsam::Point3& upVector, const CALIBRATION& K);
+
+  // Testable
+  void print(string s = "PinholeCamera") const;
+  bool equals(const This& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  CALIBRATION calibration() const;
+
+  // Manifold
+  This retract(Vector d) const;
+  Vector localCoordinates(const This& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Pose3& pose);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Similarity3.h>
+class Similarity3 {
+  // Standard Constructors
+  Similarity3();
+  Similarity3(double s);
+  Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s);
+  Similarity3(const Matrix& R, const Vector& t, double s);
+  Similarity3(const Matrix& T);
+
+  gtsam::Point3 transformFrom(const gtsam::Point3& p) const;
+  gtsam::Pose3 transformFrom(const gtsam::Pose3& T);
+
+  static gtsam::Similarity3 Align(const gtsam::Point3Pairs& abPointPairs);
+  static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
+
+  // Standard Interface
+  const Matrix matrix() const;
+  const gtsam::Rot3& rotation();
+  const gtsam::Point3& translation();
+  double scale() const;
+};
+
+// Forward declaration of PinholeCameraCalX is defined here.
+#include <gtsam/geometry/SimpleCamera.h>
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+
+template <T>
+class CameraSet {
+  CameraSet();
+
+  // structure specific methods
+  T at(size_t i) const;
+  void push_back(const T& cam);
+};
+
+#include <gtsam/geometry/StereoCamera.h>
+class StereoCamera {
+  // Standard Constructors and Named Constructors
+  StereoCamera();
+  StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::StereoCamera& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  double baseline() const;
+  gtsam::Cal3_S2Stereo calibration() const;
+
+  // Manifold
+  gtsam::StereoCamera retract(Vector d) const;
+  Vector localCoordinates(const gtsam::StereoCamera& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  gtsam::StereoPoint2 project(const gtsam::Point3& point);
+  gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/triangulation.h>
+
+// Templates appear not yet supported for free functions - issue raised at
+// borglab/wrap#14 to add support
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3_S2* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3DS2* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Bundler* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize);
+
+#include <gtsam/geometry/BearingRange.h>
+template <POSE, POINT, BEARING, RANGE>
+class BearingRange {
+  BearingRange(const BEARING& b, const RANGE& r);
+  BEARING bearing() const;
+  RANGE range() const;
+  static This Measure(const POSE& pose, const POINT& point);
+  static BEARING MeasureBearing(const POSE& pose, const POINT& point);
+  static RANGE MeasureRange(const POSE& pose, const POINT& point);
+  void print(string s = "") const;
+};
+
+typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double>
+    BearingRange2D;
+
+}  // namespace gtsam
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 049a0110c6..a55581e50d 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -2,10 +2,12 @@
 
  * GTSAM Wrap Module Definition
  *
- * These are the current classes available through the matlab and python wrappers,
+ * These are the current classes available through the matlab and python
+ wrappers,
  * add more functions/classes as they are available.
  *
- * Please refer to the wrapping docs: https://github.com/borglab/wrap/blob/master/README.md
+ * Please refer to the wrapping docs:
+ https://github.com/borglab/wrap/blob/master/README.md
  */
 
 namespace gtsam {
@@ -61,8 +63,8 @@ class KeySet {
   // structure specific methods
   void insert(size_t key);
   void merge(const gtsam::KeySet& other);
-  bool erase(size_t key); // returns true if value was removed
-  bool count(size_t key) const; // returns true if value exists
+  bool erase(size_t key);        // returns true if value was removed
+  bool count(size_t key) const;  // returns true if value exists
 
   void serialize() const;
 
@@ -123,8 +125,8 @@ class FactorIndexSet {
 
   // structure specific methods
   void insert(size_t factorIndex);
-  bool erase(size_t factorIndex); // returns true if value was removed
-  bool count(size_t factorIndex) const; // returns true if value exists
+  bool erase(size_t factorIndex);        // returns true if value was removed
+  bool count(size_t factorIndex) const;  // returns true if value exists
 };
 
 // Actually a vector<FactorIndex>
@@ -144,3483 +146,53 @@ class FactorIndices {
   void push_back(size_t factorIndex) const;
 };
 
-//*************************************************************************
-// base
-//*************************************************************************
-
-/** gtsam namespace functions */
-
-#include <gtsam/base/debug.h>
-bool isDebugVersion();
-
-#include <gtsam/base/DSFMap.h>
-class IndexPair {
-  IndexPair();
-  IndexPair(size_t i, size_t j);
-  size_t i() const;
-  size_t j() const;
-};
-
-// template<KEY = {gtsam::IndexPair}>
-// class DSFMap {
-//   DSFMap();
-//   KEY find(const KEY& key) const;
-//   void merge(const KEY& x, const KEY& y);
-//   std::map<KEY, Set> sets();
-// };
-
-class IndexPairSet {
-  IndexPairSet();
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
-
-  // structure specific methods
-  void insert(gtsam::IndexPair key);
-  bool erase(gtsam::IndexPair key); // returns true if value was removed
-  bool count(gtsam::IndexPair key) const; // returns true if value exists
-};
-
-class IndexPairVector {
-  IndexPairVector();
-  IndexPairVector(const gtsam::IndexPairVector& other);
-
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
-
-  // structure specific methods
-  gtsam::IndexPair at(size_t i) const;
-  void push_back(gtsam::IndexPair key) const;
-};
-
-gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
-
-class IndexPairSetMap {
-  IndexPairSetMap();
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
-
-  // structure specific methods
-  gtsam::IndexPairSet at(gtsam::IndexPair& key);
-};
-
-class DSFMapIndexPair {
-  DSFMapIndexPair();
-  gtsam::IndexPair find(const gtsam::IndexPair& key) const;
-  void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y);
-  gtsam::IndexPairSetMap sets();
-};
-
-#include <gtsam/base/Matrix.h>
-bool linear_independent(Matrix A, Matrix B, double tol);
-
-#include <gtsam/base/Value.h>
-virtual class Value {
-  // No constructors because this is an abstract class
-
-  // Testable
-  void print(string s = "") const;
-
-  // Manifold
-  size_t dim() const;
-};
-
-#include <gtsam/base/GenericValue.h>
-template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
-virtual class GenericValue : gtsam::Value {
-  void serializable() const;
-};
-
-//*************************************************************************
-// geometry
-//*************************************************************************
-
-#include <gtsam/geometry/Point2.h>
-class Point2 {
-  // Standard Constructors
-  Point2();
-  Point2(double x, double y);
-  Point2(Vector v);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::Point2& point, double tol) const;
-
-  // Group
-  static gtsam::Point2 identity();
-
-  // Standard Interface
-  double x() const;
-  double y() const;
-  Vector vector() const;
-  double distance(const gtsam::Point2& p2) const;
-  double norm() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-// std::vector<gtsam::Point2>
-class Point2Vector
-{
-  // Constructors
-  Point2Vector();
-  Point2Vector(const gtsam::Point2Vector& v);
-
-  //Capacity
-  size_t size() const;
-  size_t max_size() const;
-  void resize(size_t sz);
-  size_t capacity() const;
-  bool empty() const;
-  void reserve(size_t n);
-
-  //Element access
-  gtsam::Point2 at(size_t n) const;
-  gtsam::Point2 front() const;
-  gtsam::Point2 back() const;
-
-  //Modifiers
-  void assign(size_t n, const gtsam::Point2& u);
-  void push_back(const gtsam::Point2& x);
-  void pop_back();
-};
-
-#include <gtsam/geometry/StereoPoint2.h>
-class StereoPoint2 {
-  // Standard Constructors
-  StereoPoint2();
-  StereoPoint2(double uL, double uR, double v);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::StereoPoint2& point, double tol) const;
-
-  // Group
-  static gtsam::StereoPoint2 identity();
-  gtsam::StereoPoint2 inverse() const;
-  gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
-  gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
-
-  // Operator Overloads
-  gtsam::StereoPoint2 operator-() const;
-  // gtsam::StereoPoint2 operator+(Vector b) const;  //TODO Mixed types not yet supported
-  gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const;
-  gtsam::StereoPoint2 operator-(const gtsam::StereoPoint2& p2) const;
-
-  // Manifold
-  gtsam::StereoPoint2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::StereoPoint2& p) const;
-
-  // Lie Group
-  static gtsam::StereoPoint2 Expmap(Vector v);
-  static Vector Logmap(const gtsam::StereoPoint2& p);
-
-  // Standard Interface
-  Vector vector() const;
-  double uL() const;
-  double uR() const;
-  double v() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/Point3.h>
-class Point3 {
-  // Standard Constructors
-  Point3();
-  Point3(double x, double y, double z);
-  Point3(Vector v);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::Point3& p, double tol) const;
-
-  // Group
-  static gtsam::Point3 identity();
-
-  // Standard Interface
-  Vector vector() const;
-  double x() const;
-  double y() const;
-  double z() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-class Point3Pairs {
-  Point3Pairs();
-  size_t size() const;
-  bool empty() const;
-  gtsam::Point3Pair at(size_t n) const;
-  void push_back(const gtsam::Point3Pair& point_pair);
-};
-
-#include <gtsam/geometry/Rot2.h>
-class Rot2 {
-  // Standard Constructors and Named Constructors
-  Rot2();
-  Rot2(double theta);
-  static gtsam::Rot2 fromAngle(double theta);
-  static gtsam::Rot2 fromDegrees(double theta);
-  static gtsam::Rot2 fromCosSin(double c, double s);
-
-  // Testable
-  void print(string s = "theta") const;
-  bool equals(const gtsam::Rot2& rot, double tol) const;
-
-  // Group
-  static gtsam::Rot2 identity();
-  gtsam::Rot2 inverse();
-  gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
-  gtsam::Rot2 between(const gtsam::Rot2& p2) const;
-
-  // Operator Overloads
-  gtsam::Rot2 operator*(const gtsam::Rot2& p2) const;
-
-  // Manifold
-  gtsam::Rot2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Rot2& p) const;
-
-  // Lie Group
-  static gtsam::Rot2 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Rot2& p);
-  Vector logmap(const gtsam::Rot2& p);
-
-  // Group Action on Point2
-  gtsam::Point2 rotate(const gtsam::Point2& point) const;
-  gtsam::Point2 unrotate(const gtsam::Point2& point) const;
-
-  // Standard Interface
-  static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative
-  static gtsam::Rot2 atan2(double y, double x);
-  double theta() const;
-  double degrees() const;
-  double c() const;
-  double s() const;
-  Matrix matrix() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/SO3.h>
-class SO3 {
-  // Standard Constructors
-  SO3();
-  SO3(Matrix R);
-  static gtsam::SO3 FromMatrix(Matrix R);
-  static gtsam::SO3 AxisAngle(const Vector axis, double theta);
-  static gtsam::SO3 ClosestTo(const Matrix M);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::SO3& other, double tol) const;
-
-  // Group
-  static gtsam::SO3 identity();
-  gtsam::SO3 inverse() const;
-  gtsam::SO3 between(const gtsam::SO3& R) const;
-  gtsam::SO3 compose(const gtsam::SO3& R) const;
-
-  // Operator Overloads
-  gtsam::SO3 operator*(const gtsam::SO3& R) const;
-
-  // Manifold
-  gtsam::SO3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::SO3& R) const;
-  static gtsam::SO3 Expmap(Vector v);
-
-  // Other methods
-  Vector vec() const;
-  Matrix matrix() const;
-};
-
-#include <gtsam/geometry/SO4.h>
-class SO4 {
-  // Standard Constructors
-  SO4();
-  SO4(Matrix R);
-  static gtsam::SO4 FromMatrix(Matrix R);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::SO4& other, double tol) const;
-
-  // Group
-  static gtsam::SO4 identity();
-  gtsam::SO4 inverse() const;
-  gtsam::SO4 between(const gtsam::SO4& Q) const;
-  gtsam::SO4 compose(const gtsam::SO4& Q) const;
-
-  // Operator Overloads
-  gtsam::SO4 operator*(const gtsam::SO4& Q) const;
-
-  // Manifold
-  gtsam::SO4 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::SO4& Q) const;
-  static gtsam::SO4 Expmap(Vector v);
-
-  // Other methods
-  Vector vec() const;
-  Matrix matrix() const;
-};
-
-#include <gtsam/geometry/SOn.h>
-class SOn {
-  // Standard Constructors
-  SOn(size_t n);
-  static gtsam::SOn FromMatrix(Matrix R);
-  static gtsam::SOn Lift(size_t n, Matrix R);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::SOn& other, double tol) const;
-
-  // Group
-  static gtsam::SOn identity();
-  gtsam::SOn inverse() const;
-  gtsam::SOn between(const gtsam::SOn& Q) const;
-  gtsam::SOn compose(const gtsam::SOn& Q) const;
-
-  // Operator Overloads
-  gtsam::SOn operator*(const gtsam::SOn& Q) const;
-
-  // Manifold
-  gtsam::SOn retract(Vector v) const;
-  Vector localCoordinates(const gtsam::SOn& Q) const;
-  static gtsam::SOn Expmap(Vector v);
-
-  // Other methods
-  Vector vec() const;
-  Matrix matrix() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/geometry/Quaternion.h>
-class Quaternion {
-  double w() const;
-  double x() const;
-  double y() const;
-  double z() const;
-  Vector coeffs() const;
-};
-
-#include <gtsam/geometry/Rot3.h>
-class Rot3 {
-  // Standard Constructors and Named Constructors
-  Rot3();
-  Rot3(Matrix R);
-  Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3);
-  Rot3(double R11, double R12, double R13,
-      double R21, double R22, double R23,
-      double R31, double R32, double R33);
-  Rot3(double w, double x, double y, double z);
-
-  static gtsam::Rot3 Rx(double t);
-  static gtsam::Rot3 Ry(double t);
-  static gtsam::Rot3 Rz(double t);
-  static gtsam::Rot3 RzRyRx(double x, double y, double z);
-  static gtsam::Rot3 RzRyRx(Vector xyz);
-  static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading)
-  static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude)
-  static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
-  static gtsam::Rot3 Ypr(double y, double p, double r);
-  static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
-  static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle);
-  static gtsam::Rot3 Rodrigues(Vector v);
-  static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
-  static gtsam::Rot3 ClosestTo(const Matrix M);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::Rot3& rot, double tol) const;
-
-  // Group
-  static gtsam::Rot3 identity();
-    gtsam::Rot3 inverse() const;
-  gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
-  gtsam::Rot3 between(const gtsam::Rot3& p2) const;
-
-  // Operator Overloads
-  gtsam::Rot3 operator*(const gtsam::Rot3& p2) const;
-
-  // Manifold
-  //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options
-  gtsam::Rot3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Rot3& p) const;
-
-  // Group Action on Point3
-  gtsam::Point3 rotate(const gtsam::Point3& p) const;
-  gtsam::Point3 unrotate(const gtsam::Point3& p) const;
-
-  // Standard Interface
-  static gtsam::Rot3 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Rot3& p);
-  Vector logmap(const gtsam::Rot3& p);
-  Matrix matrix() const;
-  Matrix transpose() const;
-  gtsam::Point3 column(size_t index) const;
-  Vector xyz() const;
-  Vector ypr() const;
-  Vector rpy() const;
-  double roll() const;
-  double pitch() const;
-  double yaw() const;
-  pair<gtsam::Unit3, double> axisAngle() const;
-  gtsam::Quaternion toQuaternion() const;
-  Vector quaternion() const;
-  gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/Pose2.h>
-class Pose2 {
-  // Standard Constructor
-  Pose2();
-  Pose2(const gtsam::Pose2& other);
-  Pose2(double x, double y, double theta);
-  Pose2(double theta, const gtsam::Point2& t);
-  Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
-  Pose2(Vector v);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::Pose2& pose, double tol) const;
-
-  // Group
-  static gtsam::Pose2 identity();
-  gtsam::Pose2 inverse() const;
-  gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
-  gtsam::Pose2 between(const gtsam::Pose2& p2) const;
-
-  // Operator Overloads
-  gtsam::Pose2 operator*(const gtsam::Pose2& p2) const;
-
-  // Manifold
-  gtsam::Pose2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Pose2& p) const;
-
-  // Lie Group
-  static gtsam::Pose2 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Pose2& p);
-  Vector logmap(const gtsam::Pose2& p);
-  static Matrix ExpmapDerivative(Vector v);
-  static Matrix LogmapDerivative(const gtsam::Pose2& v);
-  Matrix AdjointMap() const;
-  Vector Adjoint(Vector xi) const;
-  static Matrix adjointMap_(Vector v);
-  static Vector adjoint_(Vector xi, Vector y);
-  static Vector adjointTranspose(Vector xi, Vector y);
-  static Matrix wedge(double vx, double vy, double w);
-
-  // Group Actions on Point2
-  gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
-  gtsam::Point2 transformTo(const gtsam::Point2& p) const;
-
-  // Standard Interface
-  double x() const;
-  double y() const;
-  double theta() const;
-  gtsam::Rot2 bearing(const gtsam::Point2& point) const;
-  double range(const gtsam::Point2& point) const;
-  gtsam::Point2 translation() const;
-  gtsam::Rot2 rotation() const;
-  Matrix matrix() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/Pose3.h>
-class Pose3 {
-  // Standard Constructors
-  Pose3();
-  Pose3(const gtsam::Pose3& other);
-  Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
-  Pose3(const gtsam::Pose2& pose2);
-  Pose3(Matrix mat);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::Pose3& pose, double tol) const;
-
-  // Group
-  static gtsam::Pose3 identity();
-  gtsam::Pose3 inverse() const;
-  gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
-  gtsam::Pose3 between(const gtsam::Pose3& pose) const;
-
-  // Operator Overloads
-  gtsam::Pose3 operator*(const gtsam::Pose3& pose) const;
-
-  // Manifold
-  gtsam::Pose3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Pose3& pose) const;
-
-  // Lie Group
-  static gtsam::Pose3 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Pose3& pose);
-  gtsam::Pose3 expmap(Vector v);
-  Vector logmap(const gtsam::Pose3& pose);
-  Matrix AdjointMap() const;
-  Vector Adjoint(Vector xi) const;
-  static Matrix adjointMap_(Vector xi);
-  static Vector adjoint_(Vector xi, Vector y);
-  static Vector adjointTranspose(Vector xi, Vector y);
-  static Matrix ExpmapDerivative(Vector xi);
-  static Matrix LogmapDerivative(const gtsam::Pose3& xi);
-  static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
-
-  // Group Action on Point3
-  gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
-  gtsam::Point3 transformTo(const gtsam::Point3& point) const;
-
-  // Standard Interface
-  gtsam::Rot3 rotation() const;
-  gtsam::Point3 translation() const;
-  double x() const;
-  double y() const;
-  double z() const;
-  Matrix matrix() const;
-  gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
-  gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
-  double range(const gtsam::Point3& point);
-  double range(const gtsam::Pose3& pose);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-class Pose3Pairs {
-  Pose3Pairs();
-  size_t size() const;
-  bool empty() const;
-  gtsam::Pose3Pair at(size_t n) const;
-  void push_back(const gtsam::Pose3Pair& pose_pair);
-};
-
-class Pose3Vector
-{
-  Pose3Vector();
-  size_t size() const;
-  bool empty() const;
-  gtsam::Pose3 at(size_t n) const;
-  void push_back(const gtsam::Pose3& pose);
-};
-
-#include <gtsam/geometry/Unit3.h>
-class Unit3 {
-  // Standard Constructors
-  Unit3();
-  Unit3(const gtsam::Point3& pose);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::Unit3& pose, double tol) const;
-
-  // Other functionality
-  Matrix basis() const;
-  Matrix skew() const;
-  gtsam::Point3 point3() const;
-
-  // Manifold
-  static size_t Dim();
-  size_t dim() const;
-  gtsam::Unit3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Unit3& s) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-
-  // enabling function to compare objects
-  bool equals(const gtsam::Unit3& expected, double tol) const;
-};
-
-#include <gtsam/geometry/EssentialMatrix.h>
-class EssentialMatrix {
-  // Standard Constructors
-  EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
-
-  // Manifold
-  static size_t Dim();
-  size_t dim() const;
-  gtsam::EssentialMatrix retract(Vector v) const;
-  Vector localCoordinates(const gtsam::EssentialMatrix& s) const;
-
-  // Other methods:
-  gtsam::Rot3 rotation() const;
-  gtsam::Unit3 direction() const;
-  Matrix matrix() const;
-  double error(Vector vA, Vector vB);
-};
-
-#include <gtsam/geometry/Cal3_S2.h>
-class Cal3_S2 {
-  // Standard Constructors
-  Cal3_S2();
-  Cal3_S2(double fx, double fy, double s, double u0, double v0);
-  Cal3_S2(Vector v);
-  Cal3_S2(double fov, int w, int h);
-
-  // Testable
-  void print(string s = "Cal3_S2") const;
-  bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
-
-  // Manifold
-  static size_t Dim();
-  size_t dim() const;
-  gtsam::Cal3_S2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Cal3_S2& c) const;
-
-  // Action on Point2
-  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
-  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
-
-  // Standard Interface
-  double fx() const;
-  double fy() const;
-  double skew() const;
-  double px() const;
-  double py() const;
-  gtsam::Point2 principalPoint() const;
-  Vector vector() const;
-  Matrix K() const;
-  Matrix inverse() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/Cal3DS2_Base.h>
-virtual class Cal3DS2_Base {
-  // Standard Constructors
-  Cal3DS2_Base();
-
-  // Testable
-  void print(string s = "") const;
-
-  // Standard Interface
-  double fx() const;
-  double fy() const;
-  double skew() const;
-  double px() const;
-  double py() const;
-  double k1() const;
-  double k2() const;
-  Matrix K() const;
-  Vector k() const;
-  Vector vector() const;
-
-  // Action on Point2
-  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
-  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/Cal3DS2.h>
-virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
-  // Standard Constructors
-  Cal3DS2();
-  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
-  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
-  Cal3DS2(Vector v);
-
-  // Testable
-  bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
-
-  // Manifold
-  size_t dim() const;
-  static size_t Dim();
-  gtsam::Cal3DS2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Cal3DS2& c) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/Cal3Unified.h>
-virtual class Cal3Unified : gtsam::Cal3DS2_Base {
-  // Standard Constructors
-  Cal3Unified();
-  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
-  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
-  Cal3Unified(Vector v);
-
-  // Testable
-  bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
-
-  // Standard Interface
-  double xi() const;
-  gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
-  gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
-
-  // Manifold
-  size_t dim() const;
-  static size_t Dim();
-  gtsam::Cal3Unified retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Cal3Unified& c) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/Cal3_S2Stereo.h>
-class Cal3_S2Stereo {
-  // Standard Constructors
-  Cal3_S2Stereo();
-  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
-  Cal3_S2Stereo(Vector v);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
-
-  // Standard Interface
-  double fx() const;
-  double fy() const;
-  double skew() const;
-  double px() const;
-  double py() const;
-  gtsam::Point2 principalPoint() const;
-  double baseline() const;
-};
-
-#include <gtsam/geometry/Cal3Bundler.h>
-class Cal3Bundler {
-  // Standard Constructors
-  Cal3Bundler();
-  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
-  Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
-
-  // Manifold
-  static size_t Dim();
-  size_t dim() const;
-  gtsam::Cal3Bundler retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
-
-  // Action on Point2
-  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
-  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
-
-  // Standard Interface
-  double fx() const;
-  double fy() const;
-  double k1() const;
-  double k2() const;
-  double px() const;
-  double py() const;
-  Vector vector() const;
-  Vector k() const;
-  Matrix K() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/CalibratedCamera.h>
-class CalibratedCamera {
-  // Standard Constructors and Named Constructors
-  CalibratedCamera();
-  CalibratedCamera(const gtsam::Pose3& pose);
-  CalibratedCamera(Vector v);
-  static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
-
-  // Testable
-  void print(string s = "CalibratedCamera") const;
-  bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
-
-  // Manifold
-  static size_t Dim();
-  size_t dim() const;
-  gtsam::CalibratedCamera retract(Vector d) const;
-  Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
-
-  // Action on Point3
-  gtsam::Point2 project(const gtsam::Point3& point) const;
-  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
-
-  // Standard Interface
-  gtsam::Pose3 pose() const;
-  double range(const gtsam::Point3& point) const;
-  double range(const gtsam::Pose3& pose) const;
-  double range(const gtsam::CalibratedCamera& camera) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/PinholeCamera.h>
-template<CALIBRATION>
-class PinholeCamera {
-  // Standard Constructors and Named Constructors
-  PinholeCamera();
-  PinholeCamera(const gtsam::Pose3& pose);
-  PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
-  static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height);
-  static This Level(const gtsam::Pose2& pose, double height);
-  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
-      const gtsam::Point3& upVector, const CALIBRATION& K);
-
-  // Testable
-  void print(string s = "PinholeCamera") const;
-  bool equals(const This& camera, double tol) const;
-
-  // Standard Interface
-  gtsam::Pose3 pose() const;
-  CALIBRATION calibration() const;
-
-  // Manifold
-  This retract(Vector d) const;
-  Vector localCoordinates(const This& T2) const;
-  size_t dim() const;
-  static size_t Dim();
-
-  // Transformations and measurement functions
-  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
-  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
-  gtsam::Point2 project(const gtsam::Point3& point);
-  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
-  double range(const gtsam::Point3& point);
-  double range(const gtsam::Pose3& pose);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-
-#include <gtsam/geometry/Similarity3.h>
-class Similarity3 {
-  // Standard Constructors
-  Similarity3();
-  Similarity3(double s);
-  Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s);
-  Similarity3(const Matrix& R, const Vector& t, double s);
-  Similarity3(const Matrix& T);
-
-  gtsam::Point3 transformFrom(const gtsam::Point3& p) const;
-  gtsam::Pose3 transformFrom(const gtsam::Pose3& T);
-
-  static gtsam::Similarity3 Align(const gtsam::Point3Pairs & abPointPairs);
-  static gtsam::Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs);
-
-  // Standard Interface
-  const Matrix matrix() const;
-  const gtsam::Rot3& rotation();
-  const gtsam::Point3& translation();
-  double scale() const;
-};
-
-
-// Forward declaration of PinholeCameraCalX is defined here.
-#include <gtsam/geometry/SimpleCamera.h>
-// Some typedefs for common camera types
-// PinholeCameraCal3_S2 is the same as SimpleCamera above
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
-
-template<T>
-class CameraSet {
-  CameraSet();
-
-  // structure specific methods
-  T at(size_t i) const;
-  void push_back(const T& cam);
-};
-
-#include <gtsam/geometry/StereoCamera.h>
-class StereoCamera {
-  // Standard Constructors and Named Constructors
-  StereoCamera();
-  StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::StereoCamera& camera, double tol) const;
-
-  // Standard Interface
-  gtsam::Pose3 pose() const;
-  double baseline() const;
-  gtsam::Cal3_S2Stereo calibration() const;
-
-  // Manifold
-  gtsam::StereoCamera retract(Vector d) const;
-  Vector localCoordinates(const gtsam::StereoCamera& T2) const;
-  size_t dim() const;
-  static size_t Dim();
-
-  // Transformations and measurement functions
-  gtsam::StereoPoint2 project(const gtsam::Point3& point);
-  gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/geometry/triangulation.h>
-
-// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-    gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
-    double rank_tol, bool optimize);
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-    gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements,
-    double rank_tol, bool optimize);
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-    gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
-    double rank_tol, bool optimize);
-gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
-    const gtsam::Point2Vector& measurements, double rank_tol,
-    bool optimize);
-gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
-    const gtsam::Point2Vector& measurements, double rank_tol,
-    bool optimize);
-    
-//*************************************************************************
-// Symbolic
-//*************************************************************************
-
-#include <gtsam/symbolic/SymbolicFactor.h>
-virtual class SymbolicFactor {
-  // Standard Constructors and Named Constructors
-  SymbolicFactor(const gtsam::SymbolicFactor& f);
-  SymbolicFactor();
-  SymbolicFactor(size_t j);
-  SymbolicFactor(size_t j1, size_t j2);
-  SymbolicFactor(size_t j1, size_t j2, size_t j3);
-  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4);
-  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5);
-  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6);
-  static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js);
-
-  // From Factor
-  size_t size() const;
-  void print(string s = "SymbolicFactor",
-             const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::SymbolicFactor& other, double tol) const;
-  gtsam::KeyVector keys();
-};
-
-#include <gtsam/symbolic/SymbolicFactorGraph.h>
-virtual class SymbolicFactorGraph {
-  SymbolicFactorGraph();
-  SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
-  SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
-
-  // From FactorGraph
-  void push_back(gtsam::SymbolicFactor* factor);
-  void print(string s = "SymbolicFactorGraph",
-             const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
-  size_t size() const;
-  bool exists(size_t idx) const;
-
-  // Standard interface
-  gtsam::KeySet keys() const;
-  void push_back(const gtsam::SymbolicFactorGraph& graph);
-  void push_back(const gtsam::SymbolicBayesNet& bayesNet);
-  void push_back(const gtsam::SymbolicBayesTree& bayesTree);
-
-  //Advanced Interface
-  void push_factor(size_t key);
-  void push_factor(size_t key1, size_t key2);
-  void push_factor(size_t key1, size_t key2, size_t key3);
-  void push_factor(size_t key1, size_t key2, size_t key3, size_t key4);
-
-  gtsam::SymbolicBayesNet* eliminateSequential();
-  gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
-  gtsam::SymbolicBayesTree* eliminateMultifrontal();
-  gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
-  pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*> eliminatePartialSequential(
-      const gtsam::Ordering& ordering);
-  pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*> eliminatePartialSequential(
-      const gtsam::KeyVector& keys);
-  pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*> eliminatePartialMultifrontal(
-      const gtsam::Ordering& ordering);
-  pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*> eliminatePartialMultifrontal(
-      const gtsam::KeyVector& keys);
-  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering);
-  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector);
-  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering,
-      const gtsam::Ordering& marginalizedVariableOrdering);
-  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector,
-      const gtsam::Ordering& marginalizedVariableOrdering);
-  gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector);
-};
-
-#include <gtsam/symbolic/SymbolicConditional.h>
-virtual class SymbolicConditional : gtsam::SymbolicFactor {
-  // Standard Constructors and Named Constructors
-  SymbolicConditional();
-  SymbolicConditional(const gtsam::SymbolicConditional& other);
-  SymbolicConditional(size_t key);
-  SymbolicConditional(size_t key, size_t parent);
-  SymbolicConditional(size_t key, size_t parent1, size_t parent2);
-  SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3);
-  static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals);
-
-  // Testable
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::SymbolicConditional& other, double tol) const;
-
-  // Standard interface
-  size_t nrFrontals() const;
-  size_t nrParents() const;
-};
-
-#include <gtsam/symbolic/SymbolicBayesNet.h>
-class SymbolicBayesNet {
-  SymbolicBayesNet();
-  SymbolicBayesNet(const gtsam::SymbolicBayesNet& other);
-  // Testable
-  void print(string s = "SymbolicBayesNet",
-             const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::SymbolicBayesNet& other, double tol) const;
-
-  // Standard interface
-  size_t size() const;
-  void saveGraph(string s) const;
-  gtsam::SymbolicConditional* at(size_t idx) const;
-  gtsam::SymbolicConditional* front() const;
-  gtsam::SymbolicConditional* back() const;
-  void push_back(gtsam::SymbolicConditional* conditional);
-  void push_back(const gtsam::SymbolicBayesNet& bayesNet);
-};
-
-#include <gtsam/symbolic/SymbolicBayesTree.h>
-class SymbolicBayesTree {
-
-    //Constructors
-    SymbolicBayesTree();
-    SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
-
-    // Testable
-    void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                  gtsam::DefaultKeyFormatter);
-    bool equals(const gtsam::SymbolicBayesTree& other, double tol) const;
-
-    //Standard Interface
-    //size_t findParentClique(const gtsam::IndexVector& parents) const;
-    size_t size();
-    void saveGraph(string s) const;
-    void clear();
-    void deleteCachedShortcuts();
-    size_t numCachedSeparatorMarginals() const;
-
-  gtsam::SymbolicConditional* marginalFactor(size_t key) const;
-  gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
-  gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
-};
-
-class SymbolicBayesTreeClique {
-  SymbolicBayesTreeClique();
-  // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional);
-
-  bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const;
-  void print(string s = "",
-             const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
-  size_t numCachedSeparatorMarginals() const;
-  // gtsam::sharedConditional* conditional() const;
-  bool isRoot() const;
-  size_t treeSize() const;
-  gtsam::SymbolicBayesTreeClique* parent() const;
-
-//   // TODO: need wrapped versions graphs, BayesNet
-//  BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
-//  FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
-//  FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
-//
-  void deleteCachedShortcuts();
-};
-
-#include <gtsam/inference/VariableIndex.h>
-class VariableIndex {
-  // Standard Constructors and Named Constructors
-  VariableIndex();
-  // TODO: Templetize constructor when wrap supports it
-  //template<T = {gtsam::FactorGraph}>
-  //VariableIndex(const T& factorGraph, size_t nVariables);
-  //VariableIndex(const T& factorGraph);
-  VariableIndex(const gtsam::SymbolicFactorGraph& sfg);
-  VariableIndex(const gtsam::GaussianFactorGraph& gfg);
-  VariableIndex(const gtsam::NonlinearFactorGraph& fg);
-  VariableIndex(const gtsam::VariableIndex& other);
-
-  // Testable
-  bool equals(const gtsam::VariableIndex& other, double tol) const;
-  void print(string s = "VariableIndex: ",
-             const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
-
-  // Standard interface
-  size_t size() const;
-  size_t nFactors() const;
-  size_t nEntries() const;
-};
-
-//*************************************************************************
-// linear
-//*************************************************************************
-
-namespace noiseModel {
-#include <gtsam/linear/NoiseModel.h>
-virtual class Base {
-  void print(string s = "") const;
-  // Methods below are available for all noise models. However, can't add them
-  // because wrap (incorrectly) thinks robust classes derive from this Base as well.
-  // bool isConstrained() const;
-  // bool isUnit() const;
-  // size_t dim() const;
-  // Vector sigmas() const;
-};
-
-virtual class Gaussian : gtsam::noiseModel::Base {
-  static gtsam::noiseModel::Gaussian* Information(Matrix R);
-  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
-  static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
-
-  bool equals(gtsam::noiseModel::Base& expected, double tol);
-
-  // access to noise model
-  Matrix R() const;
-  Matrix information() const;
-  Matrix covariance() const;
-
-  // Whitening operations
-  Vector whiten(Vector v) const;
-  Vector unwhiten(Vector v) const;
-  Matrix Whiten(Matrix H) const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Diagonal : gtsam::noiseModel::Gaussian {
-  static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
-  static gtsam::noiseModel::Diagonal* Variances(Vector variances);
-  static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
-  Matrix R() const;
-
-  // access to noise model
-  Vector sigmas() const;
-  Vector invsigmas() const;
-  Vector precisions() const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Constrained : gtsam::noiseModel::Diagonal {
-    static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas);
-    static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas);
-    static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances);
-    static gtsam::noiseModel::Constrained* MixedVariances(Vector variances);
-    static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions);
-    static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions);
-
-    static gtsam::noiseModel::Constrained* All(size_t dim);
-    static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
-
-    gtsam::noiseModel::Constrained* unit() const;
-
-    // enabling serialization functionality
-    void serializable() const;
-};
-
-virtual class Isotropic : gtsam::noiseModel::Diagonal {
-  static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
-  static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
-  static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
-
-  // access to noise model
-  double sigma() const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Unit : gtsam::noiseModel::Isotropic {
-  static gtsam::noiseModel::Unit* Create(size_t dim);
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-namespace mEstimator {
-virtual class Base {
-  void print(string s = "") const;
-};
-
-virtual class Null: gtsam::noiseModel::mEstimator::Base {
-  Null();
-  static gtsam::noiseModel::mEstimator::Null* Create();
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-virtual class Fair: gtsam::noiseModel::mEstimator::Base {
-  Fair(double c);
-  static gtsam::noiseModel::mEstimator::Fair* Create(double c);
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-virtual class Huber: gtsam::noiseModel::mEstimator::Base {
-  Huber(double k);
-  static gtsam::noiseModel::mEstimator::Huber* Create(double k);
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
-  Cauchy(double k);
-  static gtsam::noiseModel::mEstimator::Cauchy* Create(double k);
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
-  Tukey(double k);
-  static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
-  Welsch(double k);
-  static gtsam::noiseModel::mEstimator::Welsch* Create(double k);
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
-  GemanMcClure(double c);
-  static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c);
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-virtual class DCS: gtsam::noiseModel::mEstimator::Base {
-  DCS(double c);
-  static gtsam::noiseModel::mEstimator::DCS* Create(double c);
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
-  L2WithDeadZone(double k);
-  static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k);
-
-  // enabling serialization functionality
-  void serializable() const;
-
-  double weight(double error) const;
-  double loss(double error) const;
-};
-
-}///\namespace mEstimator
-
-virtual class Robust : gtsam::noiseModel::Base {
-  Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
-  static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-}///\namespace noiseModel
-
-#include <gtsam/linear/Sampler.h>
-class Sampler {
-  // Constructors
-  Sampler(gtsam::noiseModel::Diagonal* model, int seed);
-  Sampler(Vector sigmas, int seed);
-
-  // Standard Interface
-  size_t dim() const;
-  Vector sigmas() const;
-  gtsam::noiseModel::Diagonal* model() const;
-  Vector sample();
-};
-
-#include <gtsam/linear/VectorValues.h>
-class VectorValues {
-  //Constructors
-  VectorValues();
-  VectorValues(const gtsam::VectorValues& other);
-
-  //Named Constructors
-  static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
-
-  //Standard Interface
-  size_t size() const;
-  size_t dim(size_t j) const;
-  bool exists(size_t j) const;
-  void print(string s = "VectorValues",
-             const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::VectorValues& expected, double tol) const;
-  void insert(size_t j, Vector value);
-  Vector vector() const;
-  Vector at(size_t j) const;
-  void update(const gtsam::VectorValues& values);
-
-  //Advanced Interface
-  void setZero();
-
-  gtsam::VectorValues add(const gtsam::VectorValues& c) const;
-  void addInPlace(const gtsam::VectorValues& c);
-  gtsam::VectorValues subtract(const gtsam::VectorValues& c) const;
-  gtsam::VectorValues scale(double a) const;
-  void scaleInPlace(double a);
-
-  bool hasSameStructure(const gtsam::VectorValues& other)  const;
-  double dot(const gtsam::VectorValues& V) const;
-  double norm() const;
-  double squaredNorm() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/linear/GaussianFactor.h>
-virtual class GaussianFactor {
-  gtsam::KeyVector keys() const;
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
-  double error(const gtsam::VectorValues& c) const;
-  gtsam::GaussianFactor* clone() const;
-  gtsam::GaussianFactor* negate() const;
-  Matrix augmentedInformation() const;
-  Matrix information() const;
-  Matrix augmentedJacobian() const;
-  pair<Matrix, Vector> jacobian() const;
-  size_t size() const;
-  bool empty() const;
-};
-
-#include <gtsam/linear/JacobianFactor.h>
-virtual class JacobianFactor : gtsam::GaussianFactor {
-  //Constructors
-  JacobianFactor();
-  JacobianFactor(const gtsam::GaussianFactor& factor);
-  JacobianFactor(Vector b_in);
-  JacobianFactor(size_t i1, Matrix A1, Vector b,
-      const gtsam::noiseModel::Diagonal* model);
-  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
-      const gtsam::noiseModel::Diagonal* model);
-  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
-      Vector b, const gtsam::noiseModel::Diagonal* model);
-  JacobianFactor(const gtsam::GaussianFactorGraph& graph);
-
-  //Testable
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  void printKeys(string s) const;
-  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
-  size_t size() const;
-  Vector unweighted_error(const gtsam::VectorValues& c) const;
-  Vector error_vector(const gtsam::VectorValues& c) const;
-  double error(const gtsam::VectorValues& c) const;
-
-  //Standard Interface
-  Matrix getA() const;
-  Vector getb() const;
-  size_t rows() const;
-  size_t cols() const;
-  bool isConstrained() const;
-  pair<Matrix, Vector> jacobianUnweighted() const;
-  Matrix augmentedJacobianUnweighted() const;
-
-  void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const;
-  gtsam::JacobianFactor whiten() const;
-
-  pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
-
-  void setModel(bool anyConstrained, Vector sigmas);
-
-  gtsam::noiseModel::Diagonal* get_model() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/linear/HessianFactor.h>
-virtual class HessianFactor : gtsam::GaussianFactor {
-  //Constructors
-  HessianFactor();
-  HessianFactor(const gtsam::GaussianFactor& factor);
-  HessianFactor(size_t j, Matrix G, Vector g, double f);
-  HessianFactor(size_t j, Vector mu, Matrix Sigma);
-  HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
-      Vector g2, double f);
-  HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13,
-      Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
-      double f);
-  HessianFactor(const gtsam::GaussianFactorGraph& factors);
-
-  //Testable
-  size_t size() const;
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  void printKeys(string s) const;
-  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
-  double error(const gtsam::VectorValues& c) const;
-
-  //Standard Interface
-  size_t rows() const;
-  Matrix information() const;
-  double constantTerm() const;
-  Vector linearTerm() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/linear/GaussianFactorGraph.h>
-class GaussianFactorGraph {
-  GaussianFactorGraph();
-  GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet);
-  GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree);
-
-  // From FactorGraph
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
-  size_t size() const;
-  gtsam::GaussianFactor* at(size_t idx) const;
-  gtsam::KeySet keys() const;
-  gtsam::KeyVector keyVector() const;
-  bool exists(size_t idx) const;
-
-  // Building the graph
-  void push_back(const gtsam::GaussianFactor* factor);
-  void push_back(const gtsam::GaussianConditional* conditional);
-  void push_back(const gtsam::GaussianFactorGraph& graph);
-  void push_back(const gtsam::GaussianBayesNet& bayesNet);
-  void push_back(const gtsam::GaussianBayesTree& bayesTree);
-  void add(const gtsam::GaussianFactor& factor);
-  void add(Vector b);
-  void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
-  void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b,
-      const gtsam::noiseModel::Diagonal* model);
-  void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3,
-      Vector b, const gtsam::noiseModel::Diagonal* model);
-
-  // error and probability
-  double error(const gtsam::VectorValues& c) const;
-  double probPrime(const gtsam::VectorValues& c) const;
-
-  gtsam::GaussianFactorGraph clone() const;
-  gtsam::GaussianFactorGraph negate() const;
-
-  // Optimizing and linear algebra
-  gtsam::VectorValues optimize() const;
-  gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;
-  gtsam::VectorValues optimizeGradientSearch() const;
-  gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
-  gtsam::VectorValues gradientAtZero() const;
-
-  // Elimination and marginals
-  gtsam::GaussianBayesNet* eliminateSequential();
-  gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
-  gtsam::GaussianBayesTree* eliminateMultifrontal();
-  gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
-  pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
-    const gtsam::Ordering& ordering);
-  pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
-    const gtsam::KeyVector& keys);
-  pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
-    const gtsam::Ordering& ordering);
-  pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
-    const gtsam::KeyVector& keys);
-  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering);
-  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector);
-  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering,
-    const gtsam::Ordering& marginalizedVariableOrdering);
-  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector,
-    const gtsam::Ordering& marginalizedVariableOrdering);
-  gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector);
-
-  // Conversion to matrices
-  Matrix sparseJacobian_() const;
-  Matrix augmentedJacobian() const;
-  Matrix augmentedJacobian(const gtsam::Ordering& ordering) const;
-  pair<Matrix,Vector> jacobian() const;
-  pair<Matrix,Vector> jacobian(const gtsam::Ordering& ordering) const;
-  Matrix augmentedHessian() const;
-  Matrix augmentedHessian(const gtsam::Ordering& ordering) const;
-  pair<Matrix,Vector> hessian() const;
-  pair<Matrix,Vector> hessian(const gtsam::Ordering& ordering) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/linear/GaussianConditional.h>
-virtual class GaussianConditional : gtsam::JacobianFactor {
-  //Constructors
-  GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
-  GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
-      const gtsam::noiseModel::Diagonal* sigmas);
-  GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
-      size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas);
-
-  //Constructors with no noise model
-  GaussianConditional(size_t key, Vector d, Matrix R);
-    GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S);
-    GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
-        size_t name2, Matrix T);
-
-  //Standard Interface
-    void print(string s = "GaussianConditional",
-               const gtsam::KeyFormatter& keyFormatter =
-                   gtsam::DefaultKeyFormatter) const;
-    bool equals(const gtsam::GaussianConditional& cg, double tol) const;
-
-    // Advanced Interface
-    gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
-    gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
-                                      const gtsam::VectorValues& rhs) const;
-    void solveTransposeInPlace(gtsam::VectorValues& gy) const;
-    void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
-    Matrix R() const;
-    Matrix S() const;
-    Vector d() const;
-
-    // enabling serialization functionality
-    void serialize() const;
-};
-
-#include <gtsam/linear/GaussianDensity.h>
-virtual class GaussianDensity : gtsam::GaussianConditional {
-    //Constructors
-  GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
-
-  //Standard Interface
-  void print(string s = "GaussianDensity",
-             const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GaussianDensity &cg, double tol) const;
-  Vector mean() const;
-  Matrix covariance() const;
-};
-
-#include <gtsam/linear/GaussianBayesNet.h>
-virtual class GaussianBayesNet {
-    //Constructors
-  GaussianBayesNet();
-  GaussianBayesNet(const gtsam::GaussianConditional* conditional);
-
-  // Testable
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
-  size_t size() const;
-
-  // FactorGraph derived interface
-  // size_t size() const;
-  gtsam::GaussianConditional* at(size_t idx) const;
-  gtsam::KeySet keys() const;
-  bool exists(size_t idx) const;
-
-  void saveGraph(const string& s) const;
-
-  gtsam::GaussianConditional* front() const;
-  gtsam::GaussianConditional* back() const;
-  void push_back(gtsam::GaussianConditional* conditional);
-  void push_back(const gtsam::GaussianBayesNet& bayesNet);
-
-  gtsam::VectorValues optimize() const;
-  gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
-  gtsam::VectorValues optimizeGradientSearch() const;
-  gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
-  gtsam::VectorValues gradientAtZero() const;
-  double error(const gtsam::VectorValues& x) const;
-  double determinant() const;
-  double logDeterminant() const;
-  gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
-  gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
-};
-
-#include <gtsam/linear/GaussianBayesTree.h>
-virtual class GaussianBayesTree {
-  // Standard Constructors and Named Constructors
-  GaussianBayesTree();
-  GaussianBayesTree(const gtsam::GaussianBayesTree& other);
-  bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter);
-  size_t size() const;
-  bool empty() const;
-  size_t numCachedSeparatorMarginals() const;
-  void saveGraph(string s) const;
-
-  gtsam::VectorValues optimize() const;
-  gtsam::VectorValues optimizeGradientSearch() const;
-  gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
-  gtsam::VectorValues gradientAtZero() const;
-  double error(const gtsam::VectorValues& x) const;
-  double determinant() const;
-  double logDeterminant() const;
-  Matrix marginalCovariance(size_t key) const;
-  gtsam::GaussianConditional* marginalFactor(size_t key) const;
-  gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const;
-  gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const;
-};
-
-#include <gtsam/linear/Errors.h>
-class Errors {
-    //Constructors
-    Errors();
-    Errors(const gtsam::VectorValues& V);
-
-    //Testable
-    void print(string s = "Errors");
-    bool equals(const gtsam::Errors& expected, double tol) const;
-};
-
-#include <gtsam/linear/GaussianISAM.h>
-class GaussianISAM {
-  //Constructor
-  GaussianISAM();
-
-  //Standard Interface
-  void update(const gtsam::GaussianFactorGraph& newFactors);
-  void saveGraph(string s) const;
-  void clear();
-};
-
-#include <gtsam/linear/IterativeSolver.h>
-virtual class IterativeOptimizationParameters {
-  string getVerbosity() const;
-  void setVerbosity(string s) ;
-};
-
-//virtual class IterativeSolver {
-//  IterativeSolver();
-//  gtsam::VectorValues optimize ();
-//};
-
-#include <gtsam/linear/ConjugateGradientSolver.h>
-virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters {
-  ConjugateGradientParameters();
-  int getMinIterations() const ;
-  int getMaxIterations() const ;
-  int getReset() const;
-  double getEpsilon_rel() const;
-  double getEpsilon_abs() const;
-
-  void setMinIterations(int value);
-  void setMaxIterations(int value);
-  void setReset(int value);
-  void setEpsilon_rel(double value);
-  void setEpsilon_abs(double value);
-};
-
-#include <gtsam/linear/Preconditioner.h>
-virtual class PreconditionerParameters {
-  PreconditionerParameters();
-};
-
-virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
-  DummyPreconditionerParameters();
-};
-
-#include <gtsam/linear/PCGSolver.h>
-virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
-  PCGSolverParameters();
-  void print(string s = "");
-  void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
-};
-
-#include <gtsam/linear/SubgraphSolver.h>
-virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
-  SubgraphSolverParameters();
-};
-
-virtual class SubgraphSolver  {
-  SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
-  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
-  gtsam::VectorValues optimize() const;
-};
-
-#include <gtsam/linear/KalmanFilter.h>
-class KalmanFilter {
-  KalmanFilter(size_t n);
-  // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
-  gtsam::GaussianDensity* init(Vector x0, Matrix P0);
-  void print(string s = "") const;
-  static size_t step(gtsam::GaussianDensity* p);
-  gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
-      Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ);
-  gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F,
-      Matrix B, Vector u, Matrix Q);
-  gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0,
-      Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
-  gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H,
-      Vector z, const gtsam::noiseModel::Diagonal* model);
-  gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H,
-      Vector z, Matrix Q);
-};
-
-//*************************************************************************
-// nonlinear
-//*************************************************************************
-
-#include <gtsam/inference/Symbol.h>
-
-class Symbol {
-  Symbol();
-  Symbol(char c, uint64_t j);
-  Symbol(size_t key);
-
-  size_t key() const;
-  void print(const string& s = "") const;
-  bool equals(const gtsam::Symbol& expected, double tol) const;
-
-  char chr() const;
-  uint64_t index() const;
-  string string() const;
-};
-
-size_t symbol(char chr, size_t index);
-char symbolChr(size_t key);
-size_t symbolIndex(size_t key);
-
-namespace symbol_shorthand {
-  size_t A(size_t j);
-  size_t B(size_t j);
-  size_t C(size_t j);
-  size_t D(size_t j);
-  size_t E(size_t j);
-  size_t F(size_t j);
-  size_t G(size_t j);
-  size_t H(size_t j);
-  size_t I(size_t j);
-  size_t J(size_t j);
-  size_t K(size_t j);
-  size_t L(size_t j);
-  size_t M(size_t j);
-  size_t N(size_t j);
-  size_t O(size_t j);
-  size_t P(size_t j);
-  size_t Q(size_t j);
-  size_t R(size_t j);
-  size_t S(size_t j);
-  size_t T(size_t j);
-  size_t U(size_t j);
-  size_t V(size_t j);
-  size_t W(size_t j);
-  size_t X(size_t j);
-  size_t Y(size_t j);
-  size_t Z(size_t j);
-}///\namespace symbol
-
-// Default keyformatter
-void PrintKeyList  (const gtsam::KeyList& keys);
-void PrintKeyList  (const gtsam::KeyList& keys, string s);
-void PrintKeyVector(const gtsam::KeyVector& keys);
-void PrintKeyVector(const gtsam::KeyVector& keys, string s);
-void PrintKeySet   (const gtsam::KeySet& keys);
-void PrintKeySet   (const gtsam::KeySet& keys, string s);
-
-#include <gtsam/inference/LabeledSymbol.h>
-class LabeledSymbol {
-  LabeledSymbol(size_t full_key);
-  LabeledSymbol(const gtsam::LabeledSymbol& key);
-  LabeledSymbol(unsigned char valType, unsigned char label, size_t j);
-
-  size_t key() const;
-  unsigned char label() const;
-  unsigned char chr() const;
-  size_t index() const;
-
-  gtsam::LabeledSymbol upper() const;
-  gtsam::LabeledSymbol lower() const;
-  gtsam::LabeledSymbol newChr(unsigned char c) const;
-  gtsam::LabeledSymbol newLabel(unsigned char label) const;
-
-  void print(string s = "") const;
-};
-
-size_t mrsymbol(unsigned char c, unsigned char label, size_t j);
-unsigned char mrsymbolChr(size_t key);
-unsigned char mrsymbolLabel(size_t key);
-size_t mrsymbolIndex(size_t key);
-
-#include <gtsam/inference/Ordering.h>
-class Ordering {
-  // Standard Constructors and Named Constructors
-  Ordering();
-  Ordering(const gtsam::Ordering& other);
-
-  // Testable
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::Ordering& ord, double tol) const;
-
-  // Standard interface
-  size_t size() const;
-  size_t at(size_t key) const;
-  void push_back(size_t key);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-class NonlinearFactorGraph {
-  NonlinearFactorGraph();
-  NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
-
-  // FactorGraph
-  void print(string s = "NonlinearFactorGraph: ",
-             const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
-  size_t size() const;
-  bool empty() const;
-  void remove(size_t i);
-  void replace(size_t i, gtsam::NonlinearFactor* factors);
-  void resize(size_t size);
-  size_t nrFactors() const;
-  gtsam::NonlinearFactor* at(size_t idx) const;
-  void push_back(const gtsam::NonlinearFactorGraph& factors);
-  void push_back(gtsam::NonlinearFactor* factor);
-  void add(gtsam::NonlinearFactor* factor);
-  bool exists(size_t idx) const;
-  gtsam::KeySet keys() const;
-  gtsam::KeyVector keyVector() const;
-
-  template <T = {double, Vector, gtsam::Point2, gtsam::StereoPoint2,
-                 gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4,
-                 gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,
-                 gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2,
-                 gtsam::PinholeCamera<gtsam::Cal3Bundler>,
-                 gtsam::imuBias::ConstantBias}>
-  void addPrior(size_t key, const T& prior,
-                const gtsam::noiseModel::Base* noiseModel);
-
-  // NonlinearFactorGraph
-  void printErrors(const gtsam::Values& values) const;
-  double error(const gtsam::Values& values) const;
-  double probPrime(const gtsam::Values& values) const;
-  gtsam::Ordering orderingCOLAMD() const;
-  // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
-  gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
-  gtsam::NonlinearFactorGraph clone() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-
-  void saveGraph(const string& s) const;
-};
-
-#include <gtsam/nonlinear/NonlinearFactor.h>
-virtual class NonlinearFactor {
-  // Factor base class
-  size_t size() const;
-  gtsam::KeyVector keys() const;
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  void printKeys(string s) const;
-  // NonlinearFactor
-  bool equals(const gtsam::NonlinearFactor& other, double tol) const;
-  double error(const gtsam::Values& c) const;
-  size_t dim() const;
-  bool active(const gtsam::Values& c) const;
-  gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;
-  gtsam::NonlinearFactor* clone() const;
-  gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const;
-};
-
-#include <gtsam/nonlinear/NonlinearFactor.h>
-virtual class NoiseModelFactor: gtsam::NonlinearFactor {
-  bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
-  gtsam::noiseModel::Base* noiseModel() const;
-  Vector unwhitenedError(const gtsam::Values& x) const;
-  Vector whitenedError(const gtsam::Values& x) const;
-};
-
-#include <gtsam/nonlinear/CustomFactor.h>
-virtual class CustomFactor: gtsam::NoiseModelFactor {
-  /*
-   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there.
-   * This is achieved by adding `gtsam::CustomFactor` to the ignore list in `matlab/CMakeLists.txt`.
-   */
-  CustomFactor();
-  /*
-   * Example:
-   * ```
-   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
-   *    <calculated error>
-   *    if not H is None:
-   *        <calculate the Jacobian>
-   *        H[0] = J1 # 2-d numpy array for a Jacobian block
-   *        H[1] = J2
-   *        ...
-   *    return error # 1-d numpy array
-   *
-   * cf = CustomFactor(noise_model, keys, error_func)
-   * ```
-   */
-  CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys,
-               const gtsam::CustomErrorFunction& errorFunction);
-
-  void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
-};
-
-#include <gtsam/nonlinear/Values.h>
-class Values {
-  Values();
-  Values(const gtsam::Values& other);
-
-  size_t size() const;
-  bool empty() const;
-  void clear();
-  size_t dim() const;
-
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::Values& other, double tol) const;
-
-  void insert(const gtsam::Values& values);
-  void update(const gtsam::Values& values);
-  void erase(size_t j);
-  void swap(gtsam::Values& values);
-
-  bool exists(size_t j) const;
-  gtsam::KeyVector keys() const;
-
-  gtsam::VectorValues zeroVectors() const;
-
-  gtsam::Values retract(const gtsam::VectorValues& delta) const;
-  gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-
-  // New in 4.0, we have to specialize every insert/update/at to generate wrappers
-  // Instead of the old:
-  // void insert(size_t j, const gtsam::Value& value);
-  // void update(size_t j, const gtsam::Value& val);
-  // gtsam::Value at(size_t j) const;
-  
-  // The order is important: Vector has to precede Point2/Point3 so `atVector`
-  // can work for those fixed-size vectors.
-  void insert(size_t j, Vector vector);
-  void insert(size_t j, Matrix matrix);
-  void insert(size_t j, const gtsam::Point2& point2);
-  void insert(size_t j, const gtsam::Point3& point3);
-  void insert(size_t j, const gtsam::Rot2& rot2);
-  void insert(size_t j, const gtsam::Pose2& pose2);
-  void insert(size_t j, const gtsam::SO3& R);
-  void insert(size_t j, const gtsam::SO4& Q);
-  void insert(size_t j, const gtsam::SOn& P);
-  void insert(size_t j, const gtsam::Rot3& rot3);
-  void insert(size_t j, const gtsam::Pose3& pose3);
-  void insert(size_t j, const gtsam::Unit3& unit3);
-  void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
-  void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
-  void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
-  void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
-  void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
-  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
-  void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
-  void insert(size_t j, const gtsam::NavState& nav_state);
-  void insert(size_t j, double c);
-
-  void update(size_t j, const gtsam::Point2& point2);
-  void update(size_t j, const gtsam::Point3& point3);
-  void update(size_t j, const gtsam::Rot2& rot2);
-  void update(size_t j, const gtsam::Pose2& pose2);
-  void update(size_t j, const gtsam::SO3& R);
-  void update(size_t j, const gtsam::SO4& Q);
-  void update(size_t j, const gtsam::SOn& P);
-  void update(size_t j, const gtsam::Rot3& rot3);
-  void update(size_t j, const gtsam::Pose3& pose3);
-  void update(size_t j, const gtsam::Unit3& unit3);
-  void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
-  void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
-  void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
-  void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
-  void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
-  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
-  void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
-  void update(size_t j, const gtsam::NavState& nav_state);
-  void update(size_t j, Vector vector);
-  void update(size_t j, Matrix matrix);
-  void update(size_t j, double c);
-
-  template <T = {gtsam::Point2,
-                 gtsam::Point3,
-                 gtsam::Rot2,
-                 gtsam::Pose2,
-                 gtsam::SO3,
-                 gtsam::SO4,
-                 gtsam::SOn,
-                 gtsam::Rot3,
-                 gtsam::Pose3,
-                 gtsam::Unit3,
-                 gtsam::Cal3_S2,
-                 gtsam::Cal3DS2,
-                 gtsam::Cal3Bundler,
-                 gtsam::EssentialMatrix,
-                 gtsam::PinholeCameraCal3_S2,
-                 gtsam::PinholeCamera<gtsam::Cal3Bundler>,
-                 gtsam::imuBias::ConstantBias,
-                 gtsam::NavState,
-                 Vector,
-                 Matrix,
-                 double}>
-  T at(size_t j);
-
-};
-
-#include <gtsam/nonlinear/Marginals.h>
-class Marginals {
-  Marginals(const gtsam::NonlinearFactorGraph& graph,
-      const gtsam::Values& solution);
-  Marginals(const gtsam::GaussianFactorGraph& gfgraph,
-      const gtsam::Values& solution);
-  Marginals(const gtsam::GaussianFactorGraph& gfgraph,
-      const gtsam::VectorValues& solutionvec);
-
-  void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter =
-                                           gtsam::DefaultKeyFormatter) const;
-  Matrix marginalCovariance(size_t variable) const;
-  Matrix marginalInformation(size_t variable) const;
-  gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const;
-  gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const;
-};
-
-class JointMarginal {
-  Matrix at(size_t iVariable, size_t jVariable) const;
-  Matrix fullMatrix() const;
-  void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const;
-};
-
-#include <gtsam/nonlinear/LinearContainerFactor.h>
-virtual class LinearContainerFactor : gtsam::NonlinearFactor {
-
-  LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint);
-  LinearContainerFactor(gtsam::GaussianFactor* factor);
-
-  gtsam::GaussianFactor* factor() const;
-//  const boost::optional<Values>& linearizationPoint() const;
-
-  bool isJacobian() const;
-  gtsam::JacobianFactor* toJacobian() const;
-  gtsam::HessianFactor* toHessian() const;
-
-  static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
-      const gtsam::Values& linearizationPoint);
-
-  static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph);
-
-  // enabling serialization functionality
-  void serializable() const;
-}; // \class LinearContainerFactor
-
-// Summarization functionality
-//#include <gtsam/nonlinear/summarization.h>
-//
-//// Uses partial QR approach by default
-//gtsam::GaussianFactorGraph summarize(
-//    const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
-//    const gtsam::KeySet& saved_keys);
-//
-//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer(
-//    const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
-//    const gtsam::KeySet& saved_keys);
-
-//*************************************************************************
-// Nonlinear optimizers
-//*************************************************************************
-#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
-virtual class NonlinearOptimizerParams {
-  NonlinearOptimizerParams();
-  void print(string s = "") const;
-
-  int getMaxIterations() const;
-  double getRelativeErrorTol() const;
-  double getAbsoluteErrorTol() const;
-  double getErrorTol() const;
-  string getVerbosity() const;
-
-  void setMaxIterations(int value);
-  void setRelativeErrorTol(double value);
-  void setAbsoluteErrorTol(double value);
-  void setErrorTol(double value);
-  void setVerbosity(string s);
-
-  string getLinearSolverType() const;
-  void setLinearSolverType(string solver);
-
-  void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
-  void setOrdering(const gtsam::Ordering& ordering);
-  string getOrderingType() const;
-  void setOrderingType(string ordering);
-
-  bool isMultifrontal() const;
-  bool isSequential() const;
-  bool isCholmod() const;
-  bool isIterative() const;
-};
-
-bool checkConvergence(double relativeErrorTreshold,
-                      double absoluteErrorTreshold, double errorThreshold,
-                      double currentError, double newError);
-bool checkConvergence(const gtsam::NonlinearOptimizerParams& params,
-                      double currentError, double newError);
-
-#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
-  GaussNewtonParams();
-};
-
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
-  LevenbergMarquardtParams();
-
-  bool getDiagonalDamping() const;
-  double getlambdaFactor() const;
-  double getlambdaInitial() const;
-  double getlambdaLowerBound() const;
-  double getlambdaUpperBound() const;
-  bool getUseFixedLambdaFactor();
-  string getLogFile() const;
-  string getVerbosityLM() const;
-
-  void setDiagonalDamping(bool flag);
-  void setlambdaFactor(double value);
-  void setlambdaInitial(double value);
-  void setlambdaLowerBound(double value);
-  void setlambdaUpperBound(double value);
-  void setUseFixedLambdaFactor(bool flag);
-  void setLogFile(string s);
-  void setVerbosityLM(string s);
-
-  static gtsam::LevenbergMarquardtParams LegacyDefaults();
-  static gtsam::LevenbergMarquardtParams CeresDefaults();
-
-  static gtsam::LevenbergMarquardtParams EnsureHasOrdering(
-      gtsam::LevenbergMarquardtParams params,
-      const gtsam::NonlinearFactorGraph& graph);
-  static gtsam::LevenbergMarquardtParams ReplaceOrdering(
-      gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering);
-};
-
-#include <gtsam/nonlinear/DoglegOptimizer.h>
-virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
-  DoglegParams();
-
-  double getDeltaInitial() const;
-  string getVerbosityDL() const;
-
-  void setDeltaInitial(double deltaInitial) const;
-  void setVerbosityDL(string verbosityDL) const;
-};
-
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
-virtual class NonlinearOptimizer {
-  gtsam::Values optimize();
-  gtsam::Values optimizeSafely();
-  double error() const;
-  int iterations() const;
-  gtsam::Values values() const;
-  gtsam::NonlinearFactorGraph graph() const;
-  gtsam::GaussianFactorGraph* iterate() const;
-};
-
-#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer {
-  GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues);
-  GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params);
-};
-
-#include <gtsam/nonlinear/DoglegOptimizer.h>
-virtual class DoglegOptimizer : gtsam::NonlinearOptimizer {
-  DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues);
-  DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params);
-  double getDelta() const;
-};
-
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {
-  LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues);
-  LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params);
-  double lambda() const;
-  void print(string s = "") const;
-};
-
-#include <gtsam/nonlinear/ISAM2.h>
-class ISAM2GaussNewtonParams {
-  ISAM2GaussNewtonParams();
-
-  void print(string s = "") const;
-
-  /** Getters and Setters for all properties */
-  double getWildfireThreshold() const;
-  void setWildfireThreshold(double wildfireThreshold);
-};
-
-class ISAM2DoglegParams {
-  ISAM2DoglegParams();
-
-  void print(string s = "") const;
-
-  /** Getters and Setters for all properties */
-  double getWildfireThreshold() const;
-  void setWildfireThreshold(double wildfireThreshold);
-  double getInitialDelta() const;
-  void setInitialDelta(double initialDelta);
-  string getAdaptationMode() const;
-  void setAdaptationMode(string adaptationMode);
-  bool isVerbose() const;
-  void setVerbose(bool verbose);
-};
-
-class ISAM2ThresholdMapValue {
-  ISAM2ThresholdMapValue(char c, Vector thresholds);
-  ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other);
-};
-
-class ISAM2ThresholdMap {
-  ISAM2ThresholdMap();
-  ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other);
-
-  // Note: no print function
-
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
-
-  // structure specific methods
-  void insert(const gtsam::ISAM2ThresholdMapValue& value) const;
-};
-
-class ISAM2Params {
-  ISAM2Params();
-
-  void print(string s = "") const;
-
-  /** Getters and Setters for all properties */
-  void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params);
-  void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params);
-  void setRelinearizeThreshold(double threshold);
-  void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map);
-  int getRelinearizeSkip() const;
-  void setRelinearizeSkip(int relinearizeSkip);
-  bool isEnableRelinearization() const;
-  void setEnableRelinearization(bool enableRelinearization);
-  bool isEvaluateNonlinearError() const;
-  void setEvaluateNonlinearError(bool evaluateNonlinearError);
-  string getFactorization() const;
-  void setFactorization(string factorization);
-  bool isCacheLinearizedFactors() const;
-  void setCacheLinearizedFactors(bool cacheLinearizedFactors);
-  bool isEnableDetailedResults() const;
-  void setEnableDetailedResults(bool enableDetailedResults);
-  bool isEnablePartialRelinearizationCheck() const;
-  void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck);
-};
-
-class ISAM2Clique {
-
-    //Constructors
-    ISAM2Clique();
-
-    //Standard Interface
-    Vector gradientContribution() const;
-    void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
-};
-
-class ISAM2Result {
-  ISAM2Result();
-
-  void print(string s = "") const;
-
-  /** Getters and Setters for all properties */
-  size_t getVariablesRelinearized() const;
-  size_t getVariablesReeliminated() const;
-  size_t getCliques() const;
-  double getErrorBefore() const;
-  double getErrorAfter() const;
-};
-
-class ISAM2 {
-  ISAM2();
-  ISAM2(const gtsam::ISAM2Params& params);
-  ISAM2(const gtsam::ISAM2& other);
-
-  bool equals(const gtsam::ISAM2& other, double tol) const;
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  void printStats() const;
-  void saveGraph(string s) const;
-
-  gtsam::ISAM2Result update();
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices);
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys);
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys);
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize);
-
-  gtsam::Values getLinearizationPoint() const;
-  gtsam::Values calculateEstimate() const;
-  template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
-                     gtsam::Cal3Bundler, gtsam::EssentialMatrix,
-                     gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, 
-                     Vector, Matrix}>
-  VALUE calculateEstimate(size_t key) const;
-  gtsam::Values calculateBestEstimate() const;
-  Matrix marginalCovariance(size_t key) const;
-  gtsam::VectorValues getDelta() const;
-  gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
-  gtsam::VariableIndex getVariableIndex() const;
-  gtsam::ISAM2Params params() const;
-};
-
-#include <gtsam/nonlinear/NonlinearISAM.h>
-class NonlinearISAM {
-  NonlinearISAM();
-  NonlinearISAM(int reorderInterval);
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  void printStats() const;
-  void saveGraph(string s) const;
-  gtsam::Values estimate() const;
-  Matrix marginalCovariance(size_t key) const;
-  int reorderInterval() const;
-  int reorderCounter() const;
-  void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues);
-  void reorder_relinearize();
-
-  // These might be expensive as instead of a reference the wrapper will make a copy
-  gtsam::GaussianISAM bayesTree() const;
-  gtsam::Values getLinearizationPoint() const;
-  gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
-};
-
-//*************************************************************************
-// Nonlinear factor types
-//*************************************************************************
-#include <gtsam/nonlinear/PriorFactor.h>
-template <T = {double,
-               Vector,
-               gtsam::Point2,
-               gtsam::StereoPoint2,
-               gtsam::Point3,
-               gtsam::Rot2,
-               gtsam::SO3,
-               gtsam::SO4,
-               gtsam::SOn,
-               gtsam::Rot3,
-               gtsam::Pose2,
-               gtsam::Pose3,
-               gtsam::Unit3,
-               gtsam::Cal3_S2,
-               gtsam::Cal3DS2,
-               gtsam::Cal3Bundler,
-               gtsam::CalibratedCamera,
-               gtsam::PinholeCameraCal3_S2,
-               gtsam::imuBias::ConstantBias,
-               gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
-virtual class PriorFactor : gtsam::NoiseModelFactor {
-  PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
-  T prior() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/slam/BetweenFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
-virtual class BetweenFactor : gtsam::NoiseModelFactor {
-  BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
-  T measured() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-};
-
-#include <gtsam/nonlinear/NonlinearEquality.h>
-template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
-               gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
-               gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
-               gtsam::PinholeCameraCal3_S2,
-               gtsam::imuBias::ConstantBias}>
-virtual class NonlinearEquality : gtsam::NoiseModelFactor {
-  // Constructor - forces exact evaluation
-  NonlinearEquality(size_t j, const T& feasible);
-  // Constructor - allows inexact evaluation
-  NonlinearEquality(size_t j, const T& feasible, double error_gain);
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/sam/RangeFactor.h>
-template<POSE, POINT>
-virtual class RangeFactor : gtsam::NoiseModelFactor {
-  RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
-typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
-typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
-typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
-typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
-typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> RangeFactorSimpleCameraPoint;
-typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
-typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3_S2> RangeFactorSimpleCamera;
-
-
-#include <gtsam/sam/RangeFactor.h>
-template<POSE, POINT>
-virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
-  RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor);
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransform2D;
-typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransform3D;
-typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Pose2> RangeFactorWithTransformPose2;
-typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Pose3> RangeFactorWithTransformPose3;
-
-#include <gtsam/sam/BearingFactor.h>
-template<POSE, POINT, BEARING>
-virtual class BearingFactor : gtsam::NoiseModelFactor {
-  BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel);
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
-typedef gtsam::BearingFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3> BearingFactor3D;
-typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
-
-#include <gtsam/geometry/BearingRange.h>
-template <POSE, POINT, BEARING, RANGE>
-class BearingRange {
-  BearingRange(const BEARING& b, const RANGE& r);
-  BEARING bearing() const;
-  RANGE range() const;
-  static This Measure(const POSE& pose, const POINT& point);
-  static BEARING MeasureBearing(const POSE& pose, const POINT& point);
-  static RANGE MeasureRange(const POSE& pose, const POINT& point);
-  void print(string s = "") const;
-};
-
-typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D;
-
-#include <gtsam/sam/BearingRangeFactor.h>
-template<POSE, POINT, BEARING, RANGE>
-virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
-  BearingRangeFactor(size_t poseKey, size_t pointKey,
-      const BEARING& measuredBearing, const RANGE& measuredRange,
-      const gtsam::noiseModel::Base* noiseModel);
-
-  gtsam::BearingRange<POSE, POINT, BEARING, RANGE> measured() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
-typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double> BearingRangeFactorPose2;
-
-
-#include <gtsam/slam/ProjectionFactor.h>
-template<POSE, LANDMARK, CALIBRATION>
-virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
-  GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-    size_t poseKey, size_t pointKey, const CALIBRATION* k);
-  GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-    size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor);
-
-  GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-      size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
-    GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-      size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality,
-      const POSE& body_P_sensor);
-
-  gtsam::Point2 measured() const;
-  CALIBRATION* calibration() const;
-  bool verboseCheirality() const;
-  bool throwCheirality() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
-typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
-
-
-#include <gtsam/slam/GeneralSFMFactor.h>
-template<CAMERA, LANDMARK>
-virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
-  GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
-  gtsam::Point2 measured() const;
-};
-typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
-typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
-typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
-
-template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
-virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
-  GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
-  gtsam::Point2 measured() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/slam/SmartProjectionFactor.h>
-
-/// Linearization mode: what factor to linearize to
-enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD };
-
-/// How to manage degeneracy
-enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY };
-
-class SmartProjectionParams {
-  SmartProjectionParams();
-  void setLinearizationMode(gtsam::LinearizationMode linMode);
-  void setDegeneracyMode(gtsam::DegeneracyMode degMode);
-  void setRankTolerance(double rankTol);
-  void setEnableEPI(bool enableEPI);
-  void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
-  void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
-};
-
-#include <gtsam/slam/SmartProjectionPoseFactor.h>
-template<CALIBRATION>
-virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
-
-  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
-      const CALIBRATION* K);
-  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
-      const CALIBRATION* K,
-      const gtsam::Pose3& body_P_sensor);
-  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
-      const CALIBRATION* K,
-      const gtsam::SmartProjectionParams& params);
-  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
-      const CALIBRATION* K,
-      const gtsam::Pose3& body_P_sensor,
-      const gtsam::SmartProjectionParams& params);
-
-  void add(const gtsam::Point2& measured_i, size_t poseKey_i);
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2> SmartProjectionPose3Factor;
-
-
-#include <gtsam/slam/StereoFactor.h>
-template<POSE, LANDMARK>
-virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
-  GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel,
-    size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
-  gtsam::StereoPoint2 measured() const;
-  gtsam::Cal3_S2Stereo* calibration() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
-
-#include <gtsam/slam/PoseTranslationPrior.h>
-template<POSE>
-virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
-  PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
-};
-
-typedef gtsam::PoseTranslationPrior<gtsam::Pose2> PoseTranslationPrior2D;
-typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
-
-#include <gtsam/slam/PoseRotationPrior.h>
-template<POSE>
-virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
-  PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
-};
-
-typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
-typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
-
-#include <gtsam/slam/EssentialMatrixFactor.h>
-virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
-  EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB,
-      const gtsam::noiseModel::Base* noiseModel);
-};
-
-#include <gtsam/slam/dataset.h>
-
-class SfmTrack {
-  SfmTrack();
-  SfmTrack(const gtsam::Point3& pt);
-  const Point3& point3() const;
-
-  double r;
-  double g;
-  double b;
-
-  std::vector<pair<size_t, gtsam::Point2>> measurements;
-
-  size_t number_measurements() const;
-  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
-  pair<size_t, size_t> siftIndex(size_t idx) const;
-  void add_measurement(size_t idx, const gtsam::Point2& m);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-
-  // enabling function to compare objects
-  bool equals(const gtsam::SfmTrack& expected, double tol) const;
-};
-
-class SfmData {
-  SfmData();
-  size_t number_cameras() const;
-  size_t number_tracks() const;
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
-  gtsam::SfmTrack track(size_t idx) const;
-  void add_track(const gtsam::SfmTrack& t) ;
-  void add_camera(const gtsam::SfmCamera& cam);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enable pickling in python
-  void pickle() const;
-
-  // enabling function to compare objects
-  bool equals(const gtsam::SfmData& expected, double tol) const;
-};
-
-gtsam::SfmData readBal(string filename);
-bool writeBAL(string filename, gtsam::SfmData& data);
-gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
-gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
-
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxIndex);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
-    gtsam::noiseModel::Base* model, int maxIndex);
-void save2D(const gtsam::NonlinearFactorGraph& graph,
-    const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
-    string filename);
-
-// std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>
-// Ignored by pybind -> will be List[BetweenFactorPose2]
-class BetweenFactorPose2s
-{
-  BetweenFactorPose2s();
-  size_t size() const;
-  gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
-  void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
-};
-gtsam::BetweenFactorPose2s parse2DFactors(string filename);
-
-// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
-// Ignored by pybind -> will be List[BetweenFactorPose3]
-class BetweenFactorPose3s
-{
-  BetweenFactorPose3s();
-  size_t size() const;
-  gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
-  void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
-};
-gtsam::BetweenFactorPose3s parse3DFactors(string filename);
-
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
-
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
-void writeG2o(const gtsam::NonlinearFactorGraph& graph,
-    const gtsam::Values& estimate, string filename);
-
-#include <gtsam/slam/InitializePose3.h>
-class InitializePose3 {
-  static gtsam::Values computeOrientationsChordal(
-      const gtsam::NonlinearFactorGraph& pose3Graph);
-  static gtsam::Values computeOrientationsGradient(
-      const gtsam::NonlinearFactorGraph& pose3Graph,
-      const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
-  static gtsam::Values computeOrientationsGradient(
-      const gtsam::NonlinearFactorGraph& pose3Graph,
-      const gtsam::Values& givenGuess);
-  static gtsam::NonlinearFactorGraph buildPose3graph(
-      const gtsam::NonlinearFactorGraph& graph);
-  static gtsam::Values initializeOrientations(
-      const gtsam::NonlinearFactorGraph& graph);
-  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
-                                  const gtsam::Values& givenGuess,
-                                  bool useGradient);
-  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
-};
-
-#include <gtsam/slam/KarcherMeanFactor-inl.h>
-template<T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
-virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
-  KarcherMeanFactor(const gtsam::KeyVector& keys);
-};
-
-#include <gtsam/slam/FrobeniusFactor.h>
-gtsam::noiseModel::Isotropic* ConvertNoiseModel(
-    gtsam::noiseModel::Base* model, size_t d);
-
-template<T = {gtsam::SO3, gtsam::SO4}>
-virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
-  FrobeniusFactor(size_t key1, size_t key2);
-  FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
-
-  Vector evaluateError(const T& R1, const T& R2);
-};
-
-template<T = {gtsam::SO3, gtsam::SO4}>
-virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
-  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
-  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model);
-
-  Vector evaluateError(const T& R1, const T& R2);
-};
-
-#include <gtsam/sfm/ShonanFactor.h>
-
-virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
-  ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12,
-                          size_t p);
-  ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12,
-                          size_t p, gtsam::noiseModel::Base *model);
-  Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2);
-};
-
-#include <gtsam/sfm/BinaryMeasurement.h>
-template<T>
-class BinaryMeasurement {
-  BinaryMeasurement(size_t key1, size_t key2, const T& measured,
-                    const gtsam::noiseModel::Base* model);
-  size_t key1() const;
-  size_t key2() const;
-  T measured() const;
-  gtsam::noiseModel::Base* noiseModel() const;
-};
-
-typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
-typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
-
-class BinaryMeasurementsUnit3 {
-  BinaryMeasurementsUnit3();
-  size_t size() const;
-  gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const;
-  void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
-};
-
-#include <gtsam/sfm/ShonanAveraging.h>
-
-// TODO(frank): copy/pasta below until we have integer template paremeters in wrap!
-
-class ShonanAveragingParameters2 {
-  ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm);
-  ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, string method);
-  gtsam::LevenbergMarquardtParams getLMParams() const;  
-  void setOptimalityThreshold(double value);
-  double getOptimalityThreshold() const;
-  void setAnchor(size_t index, const gtsam::Rot2& value);
-  pair<size_t, gtsam::Rot2> getAnchor();
-  void setAnchorWeight(double value);
-  double getAnchorWeight() const;
-  void setKarcherWeight(double value);
-  double getKarcherWeight() const;
-  void setGaugesWeight(double value);
-  double getGaugesWeight() const;
-  void setUseHuber(bool value);
-  bool getUseHuber() const;
-  void setCertifyOptimality(bool value);
-  bool getCertifyOptimality() const;
-};
-
-class ShonanAveragingParameters3 {
-  ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm);
-  ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, string method);
-  gtsam::LevenbergMarquardtParams getLMParams() const;  
-  void setOptimalityThreshold(double value);
-  double getOptimalityThreshold() const;
-  void setAnchor(size_t index, const gtsam::Rot3& value);
-  pair<size_t, gtsam::Rot3> getAnchor();
-  void setAnchorWeight(double value);
-  double getAnchorWeight() const;
-  void setKarcherWeight(double value);
-  double getKarcherWeight() const;
-  void setGaugesWeight(double value);
-  double getGaugesWeight() const;
-  void setUseHuber(bool value);
-  bool getUseHuber() const;
-  void setCertifyOptimality(bool value);
-  bool getCertifyOptimality() const;
-};
-
-class ShonanAveraging2 {
-  ShonanAveraging2(string g2oFile);
-  ShonanAveraging2(string g2oFile,
-                   const gtsam::ShonanAveragingParameters2 &parameters);
-
-  // Query properties
-  size_t nrUnknowns() const;
-  size_t nrMeasurements() const;
-  gtsam::Rot2 measured(size_t i);
-  gtsam::KeyVector keys(size_t i);
-
-  // Matrix API (advanced use, debugging)
-  Matrix denseD() const;
-  Matrix denseQ() const;
-  Matrix denseL() const;
-  // Matrix computeLambda_(Matrix S) const;
-  Matrix computeLambda_(const gtsam::Values& values) const;
-  Matrix computeA_(const gtsam::Values& values) const;
-  double computeMinEigenValue(const gtsam::Values& values) const;
-  gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
-                               const Vector& minEigenVector, double minEigenValue) const;
-
-  // Advanced API
-  gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
-  gtsam::Values initializeRandomlyAt(size_t p) const;
-  double costAt(size_t p, const gtsam::Values& values) const;
-  pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
-  bool checkOptimality(const gtsam::Values& values) const;
-  gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial);
-  // gtsam::Values tryOptimizingAt(size_t p) const;
-  gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
-  gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
-  gtsam::Values roundSolution(const gtsam::Values& values) const;
-
-  // Basic API
-  double cost(const gtsam::Values& values) const;
-  gtsam::Values initializeRandomly() const;
-  pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
-};
-
-class ShonanAveraging3 {
-  ShonanAveraging3(string g2oFile);
-  ShonanAveraging3(string g2oFile,
-                   const gtsam::ShonanAveragingParameters3 &parameters);
-
-  // TODO(frank): deprecate once we land pybind wrapper
-  ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors);
-  ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors,
-                   const gtsam::ShonanAveragingParameters3 &parameters);
-
-  // Query properties
-  size_t nrUnknowns() const;
-  size_t nrMeasurements() const;
-  gtsam::Rot3 measured(size_t i);
-  gtsam::KeyVector keys(size_t i);
-
-  // Matrix API (advanced use, debugging)
-  Matrix denseD() const;
-  Matrix denseQ() const;
-  Matrix denseL() const;
-  // Matrix computeLambda_(Matrix S) const;
-  Matrix computeLambda_(const gtsam::Values& values) const;
-  Matrix computeA_(const gtsam::Values& values) const;
-  double computeMinEigenValue(const gtsam::Values& values) const;
-  gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
-                               const Vector& minEigenVector, double minEigenValue) const;
-
-  // Advanced API
-  gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
-  gtsam::Values initializeRandomlyAt(size_t p) const;
-  double costAt(size_t p, const gtsam::Values& values) const;
-  pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
-  bool checkOptimality(const gtsam::Values& values) const;
-  gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial);
-  // gtsam::Values tryOptimizingAt(size_t p) const;
-  gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
-  gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
-  gtsam::Values roundSolution(const gtsam::Values& values) const;
-
-  // Basic API
-  double cost(const gtsam::Values& values) const;
-  gtsam::Values initializeRandomly() const;
-  pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
-};
-
-#include <gtsam/sfm/MFAS.h>
-
-class KeyPairDoubleMap {
-  KeyPairDoubleMap();
-  KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
-
-  size_t size() const;
-  bool empty() const;
-  void clear();
-  size_t at(const pair<size_t, size_t>& keypair) const;
-};
-
-class MFAS {
-  MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
-       const gtsam::Unit3& projectionDirection);
-
-  gtsam::KeyPairDoubleMap computeOutlierWeights() const;
-  gtsam::KeyVector computeOrdering() const;
-};
-
-#include <gtsam/sfm/TranslationRecovery.h>
-class TranslationRecovery {
-  TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations,
-                      const gtsam::LevenbergMarquardtParams &lmParams);
-  TranslationRecovery(
-      const gtsam::BinaryMeasurementsUnit3 & relativeTranslations);  // default LevenbergMarquardtParams
-  gtsam::Values run(const double scale) const;
-  gtsam::Values run() const;    // default scale = 1.0
-};
-
-//*************************************************************************
-// Navigation
-//*************************************************************************
-namespace imuBias {
-#include <gtsam/navigation/ImuBias.h>
-
-class ConstantBias {
-  // Constructors
-  ConstantBias();
-  ConstantBias(Vector biasAcc, Vector biasGyro);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
-
-  // Group
-  static gtsam::imuBias::ConstantBias identity();
-  gtsam::imuBias::ConstantBias inverse() const;
-  gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const;
-  gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
-
-  // Operator Overloads
-  gtsam::imuBias::ConstantBias operator-() const;
-  gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const;
-  gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const;
-
-  // Manifold
-  gtsam::imuBias::ConstantBias retract(Vector v) const;
-  Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
-
-  // Lie Group
-  static gtsam::imuBias::ConstantBias Expmap(Vector v);
-  static Vector Logmap(const gtsam::imuBias::ConstantBias& b);
-
-  // Standard Interface
-  Vector vector() const;
-  Vector accelerometer() const;
-  Vector gyroscope() const;
-  Vector correctAccelerometer(Vector measurement) const;
-  Vector correctGyroscope(Vector measurement) const;
-};
-
-}///\namespace imuBias
-
-#include <gtsam/navigation/NavState.h>
-class NavState {
-  // Constructors
-  NavState();
-  NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v);
-  NavState(const gtsam::Pose3& pose, Vector v);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::NavState& expected, double tol) const;
-
-  // Access
-  gtsam::Rot3 attitude() const;
-  gtsam::Point3 position() const;
-  Vector velocity() const;
-  gtsam::Pose3 pose() const;
-
-  gtsam::NavState retract(const Vector& x) const;
-  Vector localCoordinates(const gtsam::NavState& g) const;
-};
-
-#include <gtsam/navigation/PreintegratedRotation.h>
-virtual class PreintegratedRotationParams {
-  PreintegratedRotationParams();
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
-
-  void setGyroscopeCovariance(Matrix cov);
-  void setOmegaCoriolis(Vector omega);
-  void setBodyPSensor(const gtsam::Pose3& pose);
-
-  Matrix getGyroscopeCovariance() const;
-
-  boost::optional<Vector> getOmegaCoriolis() const;
-  boost::optional<gtsam::Pose3> getBodyPSensor() const;
-};
-
-#include <gtsam/navigation/PreintegrationParams.h>
-virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
-  PreintegrationParams(Vector n_gravity);
-
-  static gtsam::PreintegrationParams* MakeSharedD(double g);
-  static gtsam::PreintegrationParams* MakeSharedU(double g);
-  static gtsam::PreintegrationParams* MakeSharedD();  // default g = 9.81
-  static gtsam::PreintegrationParams* MakeSharedU();  // default g = 9.81
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::PreintegrationParams& expected, double tol);
-
-  void setAccelerometerCovariance(Matrix cov);
-  void setIntegrationCovariance(Matrix cov);
-  void setUse2ndOrderCoriolis(bool flag);
-
-  Matrix getAccelerometerCovariance() const;
-  Matrix getIntegrationCovariance()   const;
-  bool   getUse2ndOrderCoriolis()     const;
-};
-
-#include <gtsam/navigation/ImuFactor.h>
-class PreintegratedImuMeasurements {
-  // Constructors
-  PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
-  PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
-      const gtsam::imuBias::ConstantBias& bias);
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
-
-  // Standard Interface
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
-      double deltaT);
-  void resetIntegration();
-  void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
-
-  Matrix preintMeasCov() const;
-  Vector preintegrated() const;
-  double deltaTij() const;
-  gtsam::Rot3 deltaRij() const;
-  Vector deltaPij() const;
-  Vector deltaVij() const;
-  gtsam::imuBias::ConstantBias biasHat() const;
-  Vector biasHatVector() const;
-  gtsam::NavState predict(const gtsam::NavState& state_i,
-      const gtsam::imuBias::ConstantBias& bias) const;
-};
-
-virtual class ImuFactor: gtsam::NonlinearFactor {
-  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
-      size_t bias,
-      const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements);
-
-  // Standard Interface
-  gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
-  Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
-      const gtsam::Pose3& pose_j, Vector vel_j,
-      const gtsam::imuBias::ConstantBias& bias);
-};
-
-#include <gtsam/navigation/CombinedImuFactor.h>
-virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
-  PreintegrationCombinedParams(Vector n_gravity);
-
-  static gtsam::PreintegrationCombinedParams* MakeSharedD(double g);
-  static gtsam::PreintegrationCombinedParams* MakeSharedU(double g);
-  static gtsam::PreintegrationCombinedParams* MakeSharedD();  // default g = 9.81
-  static gtsam::PreintegrationCombinedParams* MakeSharedU();  // default g = 9.81
-
-  // Testable
-  void print(string s = "") const;
-  bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
-
-  void setBiasAccCovariance(Matrix cov);
-  void setBiasOmegaCovariance(Matrix cov);
-  void setBiasAccOmegaInt(Matrix cov);
-  
-  Matrix getBiasAccCovariance() const ;
-  Matrix getBiasOmegaCovariance() const ;
-  Matrix getBiasAccOmegaInt() const;
- 
-};
-
-class PreintegratedCombinedMeasurements {
-// Constructors
-  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params);
-  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
-				    const gtsam::imuBias::ConstantBias& bias);
-  // Testable
-  void print(string s = "Preintegrated Measurements:") const;
-  bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
-      double tol);
-
-  // Standard Interface
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
-      double deltaT);
-  void resetIntegration();
-  void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
-
-  Matrix preintMeasCov() const;
-  double deltaTij() const;
-  gtsam::Rot3 deltaRij() const;
-  Vector deltaPij() const;
-  Vector deltaVij() const;
-  gtsam::imuBias::ConstantBias biasHat() const;
-  Vector biasHatVector() const;
-  gtsam::NavState predict(const gtsam::NavState& state_i,
-      const gtsam::imuBias::ConstantBias& bias) const;
-};
-
-virtual class CombinedImuFactor: gtsam::NonlinearFactor {
-  CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
-      size_t bias_i, size_t bias_j,
-      const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);
-
-  // Standard Interface
-  gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
-  Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
-      const gtsam::Pose3& pose_j, Vector vel_j,
-      const gtsam::imuBias::ConstantBias& bias_i,
-      const gtsam::imuBias::ConstantBias& bias_j);
-};
-
-#include <gtsam/navigation/AHRSFactor.h>
-class PreintegratedAhrsMeasurements {
-  // Standard Constructor
-  PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
-  PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
-
-  // Testable
-  void print(string s = "Preintegrated Measurements: ") const;
-  bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
-
-  // get Data
-  gtsam::Rot3 deltaRij() const;
-  double deltaTij() const;
-  Vector biasHat() const;
-
-  // Standard Interface
-  void integrateMeasurement(Vector measuredOmega, double deltaT);
-  void resetIntegration() ;
-};
-
-virtual class AHRSFactor : gtsam::NonlinearFactor {
-  AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
-      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
-  AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
-      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
-      const gtsam::Pose3& body_P_sensor);
-
-  // Standard Interface
-  gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
-  Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
-      Vector bias) const;
-  gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
-      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
-      Vector omegaCoriolis) const;
-};
-
-#include <gtsam/navigation/AttitudeFactor.h>
-//virtual class AttitudeFactor : gtsam::NonlinearFactor {
-//  AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
-//  AttitudeFactor();
-//};
-virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
-  Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
-      const gtsam::Unit3& bRef);
-  Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
-  Rot3AttitudeFactor();
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
-  gtsam::Unit3 nZ() const;
-  gtsam::Unit3 bRef() const;
-};
-
-virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
-  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
-                      const gtsam::noiseModel::Diagonal* model,
-                      const gtsam::Unit3& bRef);
-  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
-                      const gtsam::noiseModel::Diagonal* model);
-  Pose3AttitudeFactor();
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
-  gtsam::Unit3 nZ() const;
-  gtsam::Unit3 bRef() const;
-};
-
-#include <gtsam/navigation/GPSFactor.h>
-virtual class GPSFactor : gtsam::NonlinearFactor{
-  GPSFactor(size_t key, const gtsam::Point3& gpsIn,
-            const gtsam::noiseModel::Base* model);
-
-  // Testable
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GPSFactor& expected, double tol);
-
-  // Standard Interface
-  gtsam::Point3 measurementIn() const;
-};
-
-virtual class GPSFactor2 : gtsam::NonlinearFactor {
-  GPSFactor2(size_t key, const gtsam::Point3& gpsIn,
-            const gtsam::noiseModel::Base* model);
-
-  // Testable
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GPSFactor2& expected, double tol);
-
-  // Standard Interface
-  gtsam::Point3 measurementIn() const;
-};
-
-#include <gtsam/navigation/Scenario.h>
-virtual class Scenario {
-  gtsam::Pose3 pose(double t) const;
-  Vector omega_b(double t) const;
-  Vector velocity_n(double t) const;
-  Vector acceleration_n(double t) const;
-  gtsam::Rot3 rotation(double t) const;
-  gtsam::NavState navState(double t) const;
-  Vector velocity_b(double t) const;
-  Vector acceleration_b(double t) const;
-};
-
-virtual class ConstantTwistScenario : gtsam::Scenario {
-  ConstantTwistScenario(Vector w, Vector v);
-  ConstantTwistScenario(Vector w, Vector v,
-                        const gtsam::Pose3& nTb0);
-};
-
-virtual class AcceleratingScenario : gtsam::Scenario {
-  AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
-                       Vector v0, Vector a_n,
-                       Vector omega_b);
-};
-
-#include <gtsam/navigation/ScenarioRunner.h>
-class ScenarioRunner {
-  ScenarioRunner(const gtsam::Scenario& scenario,
-                 const gtsam::PreintegrationParams* p,
-                 double imuSampleTime,
-                 const gtsam::imuBias::ConstantBias& bias);
-  Vector gravity_n() const;
-  Vector actualAngularVelocity(double t) const;
-  Vector actualSpecificForce(double t) const;
-  Vector measuredAngularVelocity(double t) const;
-  Vector measuredSpecificForce(double t) const;
-  double imuSampleTime() const;
-  gtsam::PreintegratedImuMeasurements integrate(
-      double T, const gtsam::imuBias::ConstantBias& estimatedBias,
-      bool corrupted) const;
-  gtsam::NavState predict(
-      const gtsam::PreintegratedImuMeasurements& pim,
-      const gtsam::imuBias::ConstantBias& estimatedBias) const;
-  Matrix estimateCovariance(
-      double T, size_t N,
-      const gtsam::imuBias::ConstantBias& estimatedBias) const;
-  Matrix estimateNoiseCovariance(size_t N) const;
-};
-
 //*************************************************************************
 // Utilities
 //*************************************************************************
 
 namespace utilities {
 
-  #include <gtsam/nonlinear/utilities.h>
-  gtsam::KeyList createKeyList(Vector I);
-  gtsam::KeyList createKeyList(string s, Vector I);
-  gtsam::KeyVector createKeyVector(Vector I);
-  gtsam::KeyVector createKeyVector(string s, Vector I);
-  gtsam::KeySet createKeySet(Vector I);
-  gtsam::KeySet createKeySet(string s, Vector I);
-  Matrix extractPoint2(const gtsam::Values& values);
-  Matrix extractPoint3(const gtsam::Values& values);
-  gtsam::Values allPose2s(gtsam::Values& values);
-  Matrix extractPose2(const gtsam::Values& values);
-  gtsam::Values allPose3s(gtsam::Values& values);
-  Matrix extractPose3(const gtsam::Values& values);
-  void perturbPoint2(gtsam::Values& values, double sigma, int seed);
-  void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
-  void perturbPoint3(gtsam::Values& values, double sigma, int seed);
-  void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth);
-  void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
-  void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
-  Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
-  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
-  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
-
-} //\namespace utilities
-
 #include <gtsam/nonlinear/utilities.h>
+gtsam::KeyList createKeyList(Vector I);
+gtsam::KeyList createKeyList(string s, Vector I);
+gtsam::KeyVector createKeyVector(Vector I);
+gtsam::KeyVector createKeyVector(string s, Vector I);
+gtsam::KeySet createKeySet(Vector I);
+gtsam::KeySet createKeySet(string s, Vector I);
+Matrix extractPoint2(const gtsam::Values& values);
+Matrix extractPoint3(const gtsam::Values& values);
+gtsam::Values allPose2s(gtsam::Values& values);
+Matrix extractPose2(const gtsam::Values& values);
+gtsam::Values allPose3s(gtsam::Values& values);
+Matrix extractPose3(const gtsam::Values& values);
+void perturbPoint2(gtsam::Values& values, double sigma, int seed);
+void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR,
+                  int seed);
+void perturbPoint3(gtsam::Values& values, double sigma, int seed);
+void insertBackprojections(gtsam::Values& values,
+                           const gtsam::PinholeCamera<gtsam::Cal3_S2>& c,
+                           Vector J, Matrix Z, double depth);
+void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i,
+                             Vector J, Matrix Z,
+                             const gtsam::noiseModel::Base* model,
+                             const gtsam::Cal3_S2* K);
+void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i,
+                             Vector J, Matrix Z,
+                             const gtsam::noiseModel::Base* model,
+                             const gtsam::Cal3_S2* K,
+                             const gtsam::Pose3& body_P_sensor);
+Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph,
+                          const gtsam::Values& values);
+gtsam::Values localToWorld(const gtsam::Values& local,
+                           const gtsam::Pose2& base);
+gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base,
+                           const gtsam::KeyVector& keys);
+
+}  // namespace utilities
+
 class RedirectCout {
   RedirectCout();
   string str();
 };
 
-}
+}  // namespace gtsam
diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
new file mode 100644
index 0000000000..63047bf4f3
--- /dev/null
+++ b/gtsam/linear/linear.i
@@ -0,0 +1,653 @@
+//*************************************************************************
+// linear
+//*************************************************************************
+namespace gtsam {
+
+namespace noiseModel {
+#include <gtsam/linear/NoiseModel.h>
+virtual class Base {
+  void print(string s = "") const;
+  // Methods below are available for all noise models. However, can't add them
+  // because wrap (incorrectly) thinks robust classes derive from this Base as well.
+  // bool isConstrained() const;
+  // bool isUnit() const;
+  // size_t dim() const;
+  // Vector sigmas() const;
+};
+
+virtual class Gaussian : gtsam::noiseModel::Base {
+  static gtsam::noiseModel::Gaussian* Information(Matrix R);
+  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
+  static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
+
+  bool equals(gtsam::noiseModel::Base& expected, double tol);
+
+  // access to noise model
+  Matrix R() const;
+  Matrix information() const;
+  Matrix covariance() const;
+
+  // Whitening operations
+  Vector whiten(Vector v) const;
+  Vector unwhiten(Vector v) const;
+  Matrix Whiten(Matrix H) const;
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+virtual class Diagonal : gtsam::noiseModel::Gaussian {
+  static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
+  static gtsam::noiseModel::Diagonal* Variances(Vector variances);
+  static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
+  Matrix R() const;
+
+  // access to noise model
+  Vector sigmas() const;
+  Vector invsigmas() const;
+  Vector precisions() const;
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+virtual class Constrained : gtsam::noiseModel::Diagonal {
+    static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas);
+    static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas);
+    static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances);
+    static gtsam::noiseModel::Constrained* MixedVariances(Vector variances);
+    static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions);
+    static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions);
+
+    static gtsam::noiseModel::Constrained* All(size_t dim);
+    static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
+
+    gtsam::noiseModel::Constrained* unit() const;
+
+    // enabling serialization functionality
+    void serializable() const;
+};
+
+virtual class Isotropic : gtsam::noiseModel::Diagonal {
+  static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
+  static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
+  static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
+
+  // access to noise model
+  double sigma() const;
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+virtual class Unit : gtsam::noiseModel::Isotropic {
+  static gtsam::noiseModel::Unit* Create(size_t dim);
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+namespace mEstimator {
+virtual class Base {
+  void print(string s = "") const;
+};
+
+virtual class Null: gtsam::noiseModel::mEstimator::Base {
+  Null();
+  static gtsam::noiseModel::mEstimator::Null* Create();
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class Fair: gtsam::noiseModel::mEstimator::Base {
+  Fair(double c);
+  static gtsam::noiseModel::mEstimator::Fair* Create(double c);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class Huber: gtsam::noiseModel::mEstimator::Base {
+  Huber(double k);
+  static gtsam::noiseModel::mEstimator::Huber* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
+  Cauchy(double k);
+  static gtsam::noiseModel::mEstimator::Cauchy* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
+  Tukey(double k);
+  static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
+  Welsch(double k);
+  static gtsam::noiseModel::mEstimator::Welsch* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
+  GemanMcClure(double c);
+  static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class DCS: gtsam::noiseModel::mEstimator::Base {
+  DCS(double c);
+  static gtsam::noiseModel::mEstimator::DCS* Create(double c);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
+  L2WithDeadZone(double k);
+  static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+}///\namespace mEstimator
+
+virtual class Robust : gtsam::noiseModel::Base {
+  Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
+  static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+}///\namespace noiseModel
+
+#include <gtsam/linear/Sampler.h>
+class Sampler {
+  // Constructors
+  Sampler(gtsam::noiseModel::Diagonal* model, int seed);
+  Sampler(Vector sigmas, int seed);
+
+  // Standard Interface
+  size_t dim() const;
+  Vector sigmas() const;
+  gtsam::noiseModel::Diagonal* model() const;
+  Vector sample();
+};
+
+#include <gtsam/linear/VectorValues.h>
+class VectorValues {
+  //Constructors
+  VectorValues();
+  VectorValues(const gtsam::VectorValues& other);
+
+  //Named Constructors
+  static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
+
+  //Standard Interface
+  size_t size() const;
+  size_t dim(size_t j) const;
+  bool exists(size_t j) const;
+  void print(string s = "VectorValues",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::VectorValues& expected, double tol) const;
+  void insert(size_t j, Vector value);
+  Vector vector() const;
+  Vector at(size_t j) const;
+  void update(const gtsam::VectorValues& values);
+
+  //Advanced Interface
+  void setZero();
+
+  gtsam::VectorValues add(const gtsam::VectorValues& c) const;
+  void addInPlace(const gtsam::VectorValues& c);
+  gtsam::VectorValues subtract(const gtsam::VectorValues& c) const;
+  gtsam::VectorValues scale(double a) const;
+  void scaleInPlace(double a);
+
+  bool hasSameStructure(const gtsam::VectorValues& other)  const;
+  double dot(const gtsam::VectorValues& V) const;
+  double norm() const;
+  double squaredNorm() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/linear/GaussianFactor.h>
+virtual class GaussianFactor {
+  gtsam::KeyVector keys() const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
+  double error(const gtsam::VectorValues& c) const;
+  gtsam::GaussianFactor* clone() const;
+  gtsam::GaussianFactor* negate() const;
+  Matrix augmentedInformation() const;
+  Matrix information() const;
+  Matrix augmentedJacobian() const;
+  pair<Matrix, Vector> jacobian() const;
+  size_t size() const;
+  bool empty() const;
+};
+
+#include <gtsam/linear/JacobianFactor.h>
+virtual class JacobianFactor : gtsam::GaussianFactor {
+  //Constructors
+  JacobianFactor();
+  JacobianFactor(const gtsam::GaussianFactor& factor);
+  JacobianFactor(Vector b_in);
+  JacobianFactor(size_t i1, Matrix A1, Vector b,
+      const gtsam::noiseModel::Diagonal* model);
+  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
+      const gtsam::noiseModel::Diagonal* model);
+  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
+      Vector b, const gtsam::noiseModel::Diagonal* model);
+  JacobianFactor(const gtsam::GaussianFactorGraph& graph);
+
+  //Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  void printKeys(string s) const;
+  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
+  size_t size() const;
+  Vector unweighted_error(const gtsam::VectorValues& c) const;
+  Vector error_vector(const gtsam::VectorValues& c) const;
+  double error(const gtsam::VectorValues& c) const;
+
+  //Standard Interface
+  Matrix getA() const;
+  Vector getb() const;
+  size_t rows() const;
+  size_t cols() const;
+  bool isConstrained() const;
+  pair<Matrix, Vector> jacobianUnweighted() const;
+  Matrix augmentedJacobianUnweighted() const;
+
+  void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const;
+  gtsam::JacobianFactor whiten() const;
+
+  pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
+
+  void setModel(bool anyConstrained, Vector sigmas);
+
+  gtsam::noiseModel::Diagonal* get_model() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/linear/HessianFactor.h>
+virtual class HessianFactor : gtsam::GaussianFactor {
+  //Constructors
+  HessianFactor();
+  HessianFactor(const gtsam::GaussianFactor& factor);
+  HessianFactor(size_t j, Matrix G, Vector g, double f);
+  HessianFactor(size_t j, Vector mu, Matrix Sigma);
+  HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
+      Vector g2, double f);
+  HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13,
+      Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
+      double f);
+  HessianFactor(const gtsam::GaussianFactorGraph& factors);
+
+  //Testable
+  size_t size() const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  void printKeys(string s) const;
+  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
+  double error(const gtsam::VectorValues& c) const;
+
+  //Standard Interface
+  size_t rows() const;
+  Matrix information() const;
+  double constantTerm() const;
+  Vector linearTerm() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+class GaussianFactorGraph {
+  GaussianFactorGraph();
+  GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet);
+  GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree);
+
+  // From FactorGraph
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
+  size_t size() const;
+  gtsam::GaussianFactor* at(size_t idx) const;
+  gtsam::KeySet keys() const;
+  gtsam::KeyVector keyVector() const;
+  bool exists(size_t idx) const;
+
+  // Building the graph
+  void push_back(const gtsam::GaussianFactor* factor);
+  void push_back(const gtsam::GaussianConditional* conditional);
+  void push_back(const gtsam::GaussianFactorGraph& graph);
+  void push_back(const gtsam::GaussianBayesNet& bayesNet);
+  void push_back(const gtsam::GaussianBayesTree& bayesTree);
+  void add(const gtsam::GaussianFactor& factor);
+  void add(Vector b);
+  void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
+  void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b,
+      const gtsam::noiseModel::Diagonal* model);
+  void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3,
+      Vector b, const gtsam::noiseModel::Diagonal* model);
+
+  // error and probability
+  double error(const gtsam::VectorValues& c) const;
+  double probPrime(const gtsam::VectorValues& c) const;
+
+  gtsam::GaussianFactorGraph clone() const;
+  gtsam::GaussianFactorGraph negate() const;
+
+  // Optimizing and linear algebra
+  gtsam::VectorValues optimize() const;
+  gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;
+  gtsam::VectorValues optimizeGradientSearch() const;
+  gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
+  gtsam::VectorValues gradientAtZero() const;
+
+  // Elimination and marginals
+  gtsam::GaussianBayesNet* eliminateSequential();
+  gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
+  gtsam::GaussianBayesTree* eliminateMultifrontal();
+  gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
+  pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
+    const gtsam::Ordering& ordering);
+  pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
+    const gtsam::KeyVector& keys);
+  pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
+    const gtsam::Ordering& ordering);
+  pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
+    const gtsam::KeyVector& keys);
+  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering);
+  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector);
+  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering,
+    const gtsam::Ordering& marginalizedVariableOrdering);
+  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector,
+    const gtsam::Ordering& marginalizedVariableOrdering);
+  gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector);
+
+  // Conversion to matrices
+  Matrix sparseJacobian_() const;
+  Matrix augmentedJacobian() const;
+  Matrix augmentedJacobian(const gtsam::Ordering& ordering) const;
+  pair<Matrix,Vector> jacobian() const;
+  pair<Matrix,Vector> jacobian(const gtsam::Ordering& ordering) const;
+  Matrix augmentedHessian() const;
+  Matrix augmentedHessian(const gtsam::Ordering& ordering) const;
+  pair<Matrix,Vector> hessian() const;
+  pair<Matrix,Vector> hessian(const gtsam::Ordering& ordering) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/linear/GaussianConditional.h>
+virtual class GaussianConditional : gtsam::JacobianFactor {
+  //Constructors
+  GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
+  GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
+      const gtsam::noiseModel::Diagonal* sigmas);
+  GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
+      size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas);
+
+  //Constructors with no noise model
+  GaussianConditional(size_t key, Vector d, Matrix R);
+    GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S);
+    GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
+        size_t name2, Matrix T);
+
+  //Standard Interface
+    void print(string s = "GaussianConditional",
+               const gtsam::KeyFormatter& keyFormatter =
+                   gtsam::DefaultKeyFormatter) const;
+    bool equals(const gtsam::GaussianConditional& cg, double tol) const;
+
+    // Advanced Interface
+    gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
+    gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
+                                      const gtsam::VectorValues& rhs) const;
+    void solveTransposeInPlace(gtsam::VectorValues& gy) const;
+    void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
+    Matrix R() const;
+    Matrix S() const;
+    Vector d() const;
+
+    // enabling serialization functionality
+    void serialize() const;
+};
+
+#include <gtsam/linear/GaussianDensity.h>
+virtual class GaussianDensity : gtsam::GaussianConditional {
+    //Constructors
+  GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
+
+  //Standard Interface
+  void print(string s = "GaussianDensity",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::GaussianDensity &cg, double tol) const;
+  Vector mean() const;
+  Matrix covariance() const;
+};
+
+#include <gtsam/linear/GaussianBayesNet.h>
+virtual class GaussianBayesNet {
+    //Constructors
+  GaussianBayesNet();
+  GaussianBayesNet(const gtsam::GaussianConditional* conditional);
+
+  // Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
+  size_t size() const;
+
+  // FactorGraph derived interface
+  // size_t size() const;
+  gtsam::GaussianConditional* at(size_t idx) const;
+  gtsam::KeySet keys() const;
+  bool exists(size_t idx) const;
+
+  void saveGraph(const string& s) const;
+
+  gtsam::GaussianConditional* front() const;
+  gtsam::GaussianConditional* back() const;
+  void push_back(gtsam::GaussianConditional* conditional);
+  void push_back(const gtsam::GaussianBayesNet& bayesNet);
+
+  gtsam::VectorValues optimize() const;
+  gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
+  gtsam::VectorValues optimizeGradientSearch() const;
+  gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
+  gtsam::VectorValues gradientAtZero() const;
+  double error(const gtsam::VectorValues& x) const;
+  double determinant() const;
+  double logDeterminant() const;
+  gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
+  gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
+};
+
+#include <gtsam/linear/GaussianBayesTree.h>
+virtual class GaussianBayesTree {
+  // Standard Constructors and Named Constructors
+  GaussianBayesTree();
+  GaussianBayesTree(const gtsam::GaussianBayesTree& other);
+  bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter);
+  size_t size() const;
+  bool empty() const;
+  size_t numCachedSeparatorMarginals() const;
+  void saveGraph(string s) const;
+
+  gtsam::VectorValues optimize() const;
+  gtsam::VectorValues optimizeGradientSearch() const;
+  gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
+  gtsam::VectorValues gradientAtZero() const;
+  double error(const gtsam::VectorValues& x) const;
+  double determinant() const;
+  double logDeterminant() const;
+  Matrix marginalCovariance(size_t key) const;
+  gtsam::GaussianConditional* marginalFactor(size_t key) const;
+  gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const;
+  gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const;
+};
+
+#include <gtsam/linear/Errors.h>
+class Errors {
+    //Constructors
+    Errors();
+    Errors(const gtsam::VectorValues& V);
+
+    //Testable
+    void print(string s = "Errors");
+    bool equals(const gtsam::Errors& expected, double tol) const;
+};
+
+#include <gtsam/linear/GaussianISAM.h>
+class GaussianISAM {
+  //Constructor
+  GaussianISAM();
+
+  //Standard Interface
+  void update(const gtsam::GaussianFactorGraph& newFactors);
+  void saveGraph(string s) const;
+  void clear();
+};
+
+#include <gtsam/linear/IterativeSolver.h>
+virtual class IterativeOptimizationParameters {
+  string getVerbosity() const;
+  void setVerbosity(string s) ;
+};
+
+//virtual class IterativeSolver {
+//  IterativeSolver();
+//  gtsam::VectorValues optimize ();
+//};
+
+#include <gtsam/linear/ConjugateGradientSolver.h>
+virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters {
+  ConjugateGradientParameters();
+  int getMinIterations() const ;
+  int getMaxIterations() const ;
+  int getReset() const;
+  double getEpsilon_rel() const;
+  double getEpsilon_abs() const;
+
+  void setMinIterations(int value);
+  void setMaxIterations(int value);
+  void setReset(int value);
+  void setEpsilon_rel(double value);
+  void setEpsilon_abs(double value);
+};
+
+#include <gtsam/linear/Preconditioner.h>
+virtual class PreconditionerParameters {
+  PreconditionerParameters();
+};
+
+virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
+  DummyPreconditionerParameters();
+};
+
+#include <gtsam/linear/PCGSolver.h>
+virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
+  PCGSolverParameters();
+  void print(string s = "");
+  void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
+};
+
+#include <gtsam/linear/SubgraphSolver.h>
+virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
+  SubgraphSolverParameters();
+};
+
+virtual class SubgraphSolver  {
+  SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
+  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
+  gtsam::VectorValues optimize() const;
+};
+
+#include <gtsam/linear/KalmanFilter.h>
+class KalmanFilter {
+  KalmanFilter(size_t n);
+  // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
+  gtsam::GaussianDensity* init(Vector x0, Matrix P0);
+  void print(string s = "") const;
+  static size_t step(gtsam::GaussianDensity* p);
+  gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
+      Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ);
+  gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F,
+      Matrix B, Vector u, Matrix Q);
+  gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0,
+      Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
+  gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H,
+      Vector z, const gtsam::noiseModel::Diagonal* model);
+  gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H,
+      Vector z, Matrix Q);
+};
+
+}
\ No newline at end of file
diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i
new file mode 100644
index 0000000000..48a5a35de6
--- /dev/null
+++ b/gtsam/navigation/navigation.i
@@ -0,0 +1,355 @@
+//*************************************************************************
+// Navigation
+//*************************************************************************
+
+namespace gtsam {
+
+namespace imuBias {
+#include <gtsam/navigation/ImuBias.h>
+
+class ConstantBias {
+  // Constructors
+  ConstantBias();
+  ConstantBias(Vector biasAcc, Vector biasGyro);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
+
+  // Group
+  static gtsam::imuBias::ConstantBias identity();
+  gtsam::imuBias::ConstantBias inverse() const;
+  gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const;
+  gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
+
+  // Operator Overloads
+  gtsam::imuBias::ConstantBias operator-() const;
+  gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const;
+  gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const;
+
+  // Manifold
+  gtsam::imuBias::ConstantBias retract(Vector v) const;
+  Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
+
+  // Lie Group
+  static gtsam::imuBias::ConstantBias Expmap(Vector v);
+  static Vector Logmap(const gtsam::imuBias::ConstantBias& b);
+
+  // Standard Interface
+  Vector vector() const;
+  Vector accelerometer() const;
+  Vector gyroscope() const;
+  Vector correctAccelerometer(Vector measurement) const;
+  Vector correctGyroscope(Vector measurement) const;
+};
+
+}///\namespace imuBias
+
+#include <gtsam/navigation/NavState.h>
+class NavState {
+  // Constructors
+  NavState();
+  NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v);
+  NavState(const gtsam::Pose3& pose, Vector v);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::NavState& expected, double tol) const;
+
+  // Access
+  gtsam::Rot3 attitude() const;
+  gtsam::Point3 position() const;
+  Vector velocity() const;
+  gtsam::Pose3 pose() const;
+
+  gtsam::NavState retract(const Vector& x) const;
+  Vector localCoordinates(const gtsam::NavState& g) const;
+};
+
+#include <gtsam/navigation/PreintegratedRotation.h>
+virtual class PreintegratedRotationParams {
+  PreintegratedRotationParams();
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
+
+  void setGyroscopeCovariance(Matrix cov);
+  void setOmegaCoriolis(Vector omega);
+  void setBodyPSensor(const gtsam::Pose3& pose);
+
+  Matrix getGyroscopeCovariance() const;
+
+  boost::optional<Vector> getOmegaCoriolis() const;
+  boost::optional<gtsam::Pose3> getBodyPSensor() const;
+};
+
+#include <gtsam/navigation/PreintegrationParams.h>
+virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
+  PreintegrationParams(Vector n_gravity);
+
+  static gtsam::PreintegrationParams* MakeSharedD(double g);
+  static gtsam::PreintegrationParams* MakeSharedU(double g);
+  static gtsam::PreintegrationParams* MakeSharedD();  // default g = 9.81
+  static gtsam::PreintegrationParams* MakeSharedU();  // default g = 9.81
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::PreintegrationParams& expected, double tol);
+
+  void setAccelerometerCovariance(Matrix cov);
+  void setIntegrationCovariance(Matrix cov);
+  void setUse2ndOrderCoriolis(bool flag);
+
+  Matrix getAccelerometerCovariance() const;
+  Matrix getIntegrationCovariance()   const;
+  bool   getUse2ndOrderCoriolis()     const;
+};
+
+#include <gtsam/navigation/ImuFactor.h>
+class PreintegratedImuMeasurements {
+  // Constructors
+  PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
+  PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
+      const gtsam::imuBias::ConstantBias& bias);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
+
+  // Standard Interface
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
+      double deltaT);
+  void resetIntegration();
+  void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
+
+  Matrix preintMeasCov() const;
+  Vector preintegrated() const;
+  double deltaTij() const;
+  gtsam::Rot3 deltaRij() const;
+  Vector deltaPij() const;
+  Vector deltaVij() const;
+  gtsam::imuBias::ConstantBias biasHat() const;
+  Vector biasHatVector() const;
+  gtsam::NavState predict(const gtsam::NavState& state_i,
+      const gtsam::imuBias::ConstantBias& bias) const;
+};
+
+virtual class ImuFactor: gtsam::NonlinearFactor {
+  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
+      size_t bias,
+      const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements);
+
+  // Standard Interface
+  gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
+  Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
+      const gtsam::Pose3& pose_j, Vector vel_j,
+      const gtsam::imuBias::ConstantBias& bias);
+};
+
+#include <gtsam/navigation/CombinedImuFactor.h>
+virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
+  PreintegrationCombinedParams(Vector n_gravity);
+
+  static gtsam::PreintegrationCombinedParams* MakeSharedD(double g);
+  static gtsam::PreintegrationCombinedParams* MakeSharedU(double g);
+  static gtsam::PreintegrationCombinedParams* MakeSharedD();  // default g = 9.81
+  static gtsam::PreintegrationCombinedParams* MakeSharedU();  // default g = 9.81
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
+
+  void setBiasAccCovariance(Matrix cov);
+  void setBiasOmegaCovariance(Matrix cov);
+  void setBiasAccOmegaInt(Matrix cov);
+  
+  Matrix getBiasAccCovariance() const ;
+  Matrix getBiasOmegaCovariance() const ;
+  Matrix getBiasAccOmegaInt() const;
+ 
+};
+
+class PreintegratedCombinedMeasurements {
+// Constructors
+  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params);
+  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
+				    const gtsam::imuBias::ConstantBias& bias);
+  // Testable
+  void print(string s = "Preintegrated Measurements:") const;
+  bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
+      double tol);
+
+  // Standard Interface
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
+      double deltaT);
+  void resetIntegration();
+  void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
+
+  Matrix preintMeasCov() const;
+  double deltaTij() const;
+  gtsam::Rot3 deltaRij() const;
+  Vector deltaPij() const;
+  Vector deltaVij() const;
+  gtsam::imuBias::ConstantBias biasHat() const;
+  Vector biasHatVector() const;
+  gtsam::NavState predict(const gtsam::NavState& state_i,
+      const gtsam::imuBias::ConstantBias& bias) const;
+};
+
+virtual class CombinedImuFactor: gtsam::NonlinearFactor {
+  CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
+      size_t bias_i, size_t bias_j,
+      const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);
+
+  // Standard Interface
+  gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
+  Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
+      const gtsam::Pose3& pose_j, Vector vel_j,
+      const gtsam::imuBias::ConstantBias& bias_i,
+      const gtsam::imuBias::ConstantBias& bias_j);
+};
+
+#include <gtsam/navigation/AHRSFactor.h>
+class PreintegratedAhrsMeasurements {
+  // Standard Constructor
+  PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
+  PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
+
+  // Testable
+  void print(string s = "Preintegrated Measurements: ") const;
+  bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
+
+  // get Data
+  gtsam::Rot3 deltaRij() const;
+  double deltaTij() const;
+  Vector biasHat() const;
+
+  // Standard Interface
+  void integrateMeasurement(Vector measuredOmega, double deltaT);
+  void resetIntegration() ;
+};
+
+virtual class AHRSFactor : gtsam::NonlinearFactor {
+  AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
+  AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
+      const gtsam::Pose3& body_P_sensor);
+
+  // Standard Interface
+  gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
+  Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
+      Vector bias) const;
+  gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
+      Vector omegaCoriolis) const;
+};
+
+#include <gtsam/navigation/AttitudeFactor.h>
+//virtual class AttitudeFactor : gtsam::NonlinearFactor {
+//  AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
+//  AttitudeFactor();
+//};
+virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
+  Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
+      const gtsam::Unit3& bRef);
+  Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
+  Rot3AttitudeFactor();
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
+  gtsam::Unit3 nZ() const;
+  gtsam::Unit3 bRef() const;
+};
+
+virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
+  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
+                      const gtsam::noiseModel::Diagonal* model,
+                      const gtsam::Unit3& bRef);
+  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
+                      const gtsam::noiseModel::Diagonal* model);
+  Pose3AttitudeFactor();
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
+  gtsam::Unit3 nZ() const;
+  gtsam::Unit3 bRef() const;
+};
+
+#include <gtsam/navigation/GPSFactor.h>
+virtual class GPSFactor : gtsam::NonlinearFactor{
+  GPSFactor(size_t key, const gtsam::Point3& gpsIn,
+            const gtsam::noiseModel::Base* model);
+
+  // Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::GPSFactor& expected, double tol);
+
+  // Standard Interface
+  gtsam::Point3 measurementIn() const;
+};
+
+virtual class GPSFactor2 : gtsam::NonlinearFactor {
+  GPSFactor2(size_t key, const gtsam::Point3& gpsIn,
+            const gtsam::noiseModel::Base* model);
+
+  // Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::GPSFactor2& expected, double tol);
+
+  // Standard Interface
+  gtsam::Point3 measurementIn() const;
+};
+
+#include <gtsam/navigation/Scenario.h>
+virtual class Scenario {
+  gtsam::Pose3 pose(double t) const;
+  Vector omega_b(double t) const;
+  Vector velocity_n(double t) const;
+  Vector acceleration_n(double t) const;
+  gtsam::Rot3 rotation(double t) const;
+  gtsam::NavState navState(double t) const;
+  Vector velocity_b(double t) const;
+  Vector acceleration_b(double t) const;
+};
+
+virtual class ConstantTwistScenario : gtsam::Scenario {
+  ConstantTwistScenario(Vector w, Vector v);
+  ConstantTwistScenario(Vector w, Vector v,
+                        const gtsam::Pose3& nTb0);
+};
+
+virtual class AcceleratingScenario : gtsam::Scenario {
+  AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
+                       Vector v0, Vector a_n,
+                       Vector omega_b);
+};
+
+#include <gtsam/navigation/ScenarioRunner.h>
+class ScenarioRunner {
+  ScenarioRunner(const gtsam::Scenario& scenario,
+                 const gtsam::PreintegrationParams* p,
+                 double imuSampleTime,
+                 const gtsam::imuBias::ConstantBias& bias);
+  Vector gravity_n() const;
+  Vector actualAngularVelocity(double t) const;
+  Vector actualSpecificForce(double t) const;
+  Vector measuredAngularVelocity(double t) const;
+  Vector measuredSpecificForce(double t) const;
+  double imuSampleTime() const;
+  gtsam::PreintegratedImuMeasurements integrate(
+      double T, const gtsam::imuBias::ConstantBias& estimatedBias,
+      bool corrupted) const;
+  gtsam::NavState predict(
+      const gtsam::PreintegratedImuMeasurements& pim,
+      const gtsam::imuBias::ConstantBias& estimatedBias) const;
+  Matrix estimateCovariance(
+      double T, size_t N,
+      const gtsam::imuBias::ConstantBias& estimatedBias) const;
+  Matrix estimateNoiseCovariance(size_t N) const;
+};
+
+}
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
new file mode 100644
index 0000000000..e22ac5954b
--- /dev/null
+++ b/gtsam/nonlinear/nonlinear.i
@@ -0,0 +1,769 @@
+//*************************************************************************
+// nonlinear
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SO3.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/geometry/SOn.h>
+#include <gtsam/geometry/StereoPoint2.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/NavState.h>
+
+class Symbol {
+  Symbol();
+  Symbol(char c, uint64_t j);
+  Symbol(size_t key);
+
+  size_t key() const;
+  void print(const string& s = "") const;
+  bool equals(const gtsam::Symbol& expected, double tol) const;
+
+  char chr() const;
+  uint64_t index() const;
+  string string() const;
+};
+
+size_t symbol(char chr, size_t index);
+char symbolChr(size_t key);
+size_t symbolIndex(size_t key);
+
+namespace symbol_shorthand {
+size_t A(size_t j);
+size_t B(size_t j);
+size_t C(size_t j);
+size_t D(size_t j);
+size_t E(size_t j);
+size_t F(size_t j);
+size_t G(size_t j);
+size_t H(size_t j);
+size_t I(size_t j);
+size_t J(size_t j);
+size_t K(size_t j);
+size_t L(size_t j);
+size_t M(size_t j);
+size_t N(size_t j);
+size_t O(size_t j);
+size_t P(size_t j);
+size_t Q(size_t j);
+size_t R(size_t j);
+size_t S(size_t j);
+size_t T(size_t j);
+size_t U(size_t j);
+size_t V(size_t j);
+size_t W(size_t j);
+size_t X(size_t j);
+size_t Y(size_t j);
+size_t Z(size_t j);
+}  // namespace symbol_shorthand
+
+// Default keyformatter
+void PrintKeyList(const gtsam::KeyList& keys);
+void PrintKeyList(const gtsam::KeyList& keys, string s);
+void PrintKeyVector(const gtsam::KeyVector& keys);
+void PrintKeyVector(const gtsam::KeyVector& keys, string s);
+void PrintKeySet(const gtsam::KeySet& keys);
+void PrintKeySet(const gtsam::KeySet& keys, string s);
+
+#include <gtsam/inference/LabeledSymbol.h>
+class LabeledSymbol {
+  LabeledSymbol(size_t full_key);
+  LabeledSymbol(const gtsam::LabeledSymbol& key);
+  LabeledSymbol(unsigned char valType, unsigned char label, size_t j);
+
+  size_t key() const;
+  unsigned char label() const;
+  unsigned char chr() const;
+  size_t index() const;
+
+  gtsam::LabeledSymbol upper() const;
+  gtsam::LabeledSymbol lower() const;
+  gtsam::LabeledSymbol newChr(unsigned char c) const;
+  gtsam::LabeledSymbol newLabel(unsigned char label) const;
+
+  void print(string s = "") const;
+};
+
+size_t mrsymbol(unsigned char c, unsigned char label, size_t j);
+unsigned char mrsymbolChr(size_t key);
+unsigned char mrsymbolLabel(size_t key);
+size_t mrsymbolIndex(size_t key);
+
+#include <gtsam/inference/Ordering.h>
+class Ordering {
+  // Standard Constructors and Named Constructors
+  Ordering();
+  Ordering(const gtsam::Ordering& other);
+
+  // Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::Ordering& ord, double tol) const;
+
+  // Standard interface
+  size_t size() const;
+  size_t at(size_t key) const;
+  void push_back(size_t key);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+class NonlinearFactorGraph {
+  NonlinearFactorGraph();
+  NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
+
+  // FactorGraph
+  void print(string s = "NonlinearFactorGraph: ",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
+  size_t size() const;
+  bool empty() const;
+  void remove(size_t i);
+  void replace(size_t i, gtsam::NonlinearFactor* factors);
+  void resize(size_t size);
+  size_t nrFactors() const;
+  gtsam::NonlinearFactor* at(size_t idx) const;
+  void push_back(const gtsam::NonlinearFactorGraph& factors);
+  void push_back(gtsam::NonlinearFactor* factor);
+  void add(gtsam::NonlinearFactor* factor);
+  bool exists(size_t idx) const;
+  gtsam::KeySet keys() const;
+  gtsam::KeyVector keyVector() const;
+
+  template <T = {double, Vector, gtsam::Point2, gtsam::StereoPoint2,
+                 gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4,
+                 gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,
+                 gtsam::CalibratedCamera, gtsam::PinholeCamera<gtsam::Cal3_S2>,
+                 gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                 gtsam::imuBias::ConstantBias}>
+  void addPrior(size_t key, const T& prior,
+                const gtsam::noiseModel::Base* noiseModel);
+
+  // NonlinearFactorGraph
+  void printErrors(const gtsam::Values& values) const;
+  double error(const gtsam::Values& values) const;
+  double probPrime(const gtsam::Values& values) const;
+  gtsam::Ordering orderingCOLAMD() const;
+  // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const
+  // std::map<gtsam::Key,int>& constraints) const;
+  gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
+  gtsam::NonlinearFactorGraph clone() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+
+  void saveGraph(const string& s) const;
+};
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+virtual class NonlinearFactor {
+  // Factor base class
+  size_t size() const;
+  gtsam::KeyVector keys() const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  void printKeys(string s) const;
+  // NonlinearFactor
+  bool equals(const gtsam::NonlinearFactor& other, double tol) const;
+  double error(const gtsam::Values& c) const;
+  size_t dim() const;
+  bool active(const gtsam::Values& c) const;
+  gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;
+  gtsam::NonlinearFactor* clone() const;
+  gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const;
+};
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+virtual class NoiseModelFactor : gtsam::NonlinearFactor {
+  bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
+  gtsam::noiseModel::Base* noiseModel() const;
+  Vector unwhitenedError(const gtsam::Values& x) const;
+  Vector whitenedError(const gtsam::Values& x) const;
+};
+
+#include <gtsam/nonlinear/CustomFactor.h>
+virtual class CustomFactor : gtsam::NoiseModelFactor {
+  /*
+   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
+   * machinery there. This is achieved by adding `gtsam::CustomFactor` to the
+   * ignore list in `matlab/CMakeLists.txt`.
+   */
+  CustomFactor();
+  /*
+   * Example:
+   * ```
+   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+   *    <calculated error>
+   *    if not H is None:
+   *        <calculate the Jacobian>
+   *        H[0] = J1 # 2-d numpy array for a Jacobian block
+   *        H[1] = J2
+   *        ...
+   *    return error # 1-d numpy array
+   *
+   * cf = CustomFactor(noise_model, keys, error_func)
+   * ```
+   */
+  CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
+               const gtsam::KeyVector& keys,
+               const gtsam::CustomErrorFunction& errorFunction);
+
+  void print(string s = "",
+             gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
+};
+
+#include <gtsam/nonlinear/Values.h>
+class Values {
+  Values();
+  Values(const gtsam::Values& other);
+
+  size_t size() const;
+  bool empty() const;
+  void clear();
+  size_t dim() const;
+
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::Values& other, double tol) const;
+
+  void insert(const gtsam::Values& values);
+  void update(const gtsam::Values& values);
+  void erase(size_t j);
+  void swap(gtsam::Values& values);
+
+  bool exists(size_t j) const;
+  gtsam::KeyVector keys() const;
+
+  gtsam::VectorValues zeroVectors() const;
+
+  gtsam::Values retract(const gtsam::VectorValues& delta) const;
+  gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+
+  // New in 4.0, we have to specialize every insert/update/at to generate
+  // wrappers Instead of the old: void insert(size_t j, const gtsam::Value&
+  // value); void update(size_t j, const gtsam::Value& val); gtsam::Value
+  // at(size_t j) const;
+
+  // The order is important: Vector has to precede Point2/Point3 so `atVector`
+  // can work for those fixed-size vectors.
+  void insert(size_t j, Vector vector);
+  void insert(size_t j, Matrix matrix);
+  void insert(size_t j, const gtsam::Point2& point2);
+  void insert(size_t j, const gtsam::Point3& point3);
+  void insert(size_t j, const gtsam::Rot2& rot2);
+  void insert(size_t j, const gtsam::Pose2& pose2);
+  void insert(size_t j, const gtsam::SO3& R);
+  void insert(size_t j, const gtsam::SO4& Q);
+  void insert(size_t j, const gtsam::SOn& P);
+  void insert(size_t j, const gtsam::Rot3& rot3);
+  void insert(size_t j, const gtsam::Pose3& pose3);
+  void insert(size_t j, const gtsam::Unit3& unit3);
+  void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
+  void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
+  void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
+  void insert(size_t j,
+              const gtsam::PinholeCamera<gtsam::Cal3_S2>& simple_camera);
+  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
+  void insert(size_t j, const gtsam::NavState& nav_state);
+  void insert(size_t j, double c);
+
+  void update(size_t j, const gtsam::Point2& point2);
+  void update(size_t j, const gtsam::Point3& point3);
+  void update(size_t j, const gtsam::Rot2& rot2);
+  void update(size_t j, const gtsam::Pose2& pose2);
+  void update(size_t j, const gtsam::SO3& R);
+  void update(size_t j, const gtsam::SO4& Q);
+  void update(size_t j, const gtsam::SOn& P);
+  void update(size_t j, const gtsam::Rot3& rot3);
+  void update(size_t j, const gtsam::Pose3& pose3);
+  void update(size_t j, const gtsam::Unit3& unit3);
+  void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
+  void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
+  void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
+  void update(size_t j,
+              const gtsam::PinholeCamera<gtsam::Cal3_S2>& simple_camera);
+  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
+  void update(size_t j, const gtsam::NavState& nav_state);
+  void update(size_t j, Vector vector);
+  void update(size_t j, Matrix matrix);
+  void update(size_t j, double c);
+
+  template <T = {gtsam::Point2,
+                 gtsam::Point3,
+                 gtsam::Rot2,
+                 gtsam::Pose2,
+                 gtsam::SO3,
+                 gtsam::SO4,
+                 gtsam::SOn,
+                 gtsam::Rot3,
+                 gtsam::Pose3,
+                 gtsam::Unit3,
+                 gtsam::Cal3_S2,
+                 gtsam::Cal3DS2,
+                 gtsam::Cal3Bundler,
+                 gtsam::EssentialMatrix,
+                 gtsam::PinholeCamera<gtsam::Cal3_S2>,
+                 gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                 gtsam::imuBias::ConstantBias,
+                 gtsam::NavState,
+                 Vector,
+                 Matrix,
+                 double}>
+  T at(size_t j);
+};
+
+#include <gtsam/nonlinear/Marginals.h>
+class Marginals {
+  Marginals(const gtsam::NonlinearFactorGraph& graph,
+            const gtsam::Values& solution);
+  Marginals(const gtsam::GaussianFactorGraph& gfgraph,
+            const gtsam::Values& solution);
+  Marginals(const gtsam::GaussianFactorGraph& gfgraph,
+            const gtsam::VectorValues& solutionvec);
+
+  void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter =
+                                           gtsam::DefaultKeyFormatter) const;
+  Matrix marginalCovariance(size_t variable) const;
+  Matrix marginalInformation(size_t variable) const;
+  gtsam::JointMarginal jointMarginalCovariance(
+      const gtsam::KeyVector& variables) const;
+  gtsam::JointMarginal jointMarginalInformation(
+      const gtsam::KeyVector& variables) const;
+};
+
+class JointMarginal {
+  Matrix at(size_t iVariable, size_t jVariable) const;
+  Matrix fullMatrix() const;
+  void print(string s = "", gtsam::KeyFormatter keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+};
+
+#include <gtsam/nonlinear/LinearContainerFactor.h>
+virtual class LinearContainerFactor : gtsam::NonlinearFactor {
+  LinearContainerFactor(gtsam::GaussianFactor* factor,
+                        const gtsam::Values& linearizationPoint);
+  LinearContainerFactor(gtsam::GaussianFactor* factor);
+
+  gtsam::GaussianFactor* factor() const;
+  //  const boost::optional<Values>& linearizationPoint() const;
+
+  bool isJacobian() const;
+  gtsam::JacobianFactor* toJacobian() const;
+  gtsam::HessianFactor* toHessian() const;
+
+  static gtsam::NonlinearFactorGraph ConvertLinearGraph(
+      const gtsam::GaussianFactorGraph& linear_graph,
+      const gtsam::Values& linearizationPoint);
+
+  static gtsam::NonlinearFactorGraph ConvertLinearGraph(
+      const gtsam::GaussianFactorGraph& linear_graph);
+
+  // enabling serialization functionality
+  void serializable() const;
+};  // \class LinearContainerFactor
+
+// Summarization functionality
+//#include <gtsam/nonlinear/summarization.h>
+//
+//// Uses partial QR approach by default
+// gtsam::GaussianFactorGraph summarize(
+//    const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
+//    const gtsam::KeySet& saved_keys);
+//
+// gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer(
+//    const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
+//    const gtsam::KeySet& saved_keys);
+
+//*************************************************************************
+// Nonlinear optimizers
+//*************************************************************************
+#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
+virtual class NonlinearOptimizerParams {
+  NonlinearOptimizerParams();
+  void print(string s = "") const;
+
+  int getMaxIterations() const;
+  double getRelativeErrorTol() const;
+  double getAbsoluteErrorTol() const;
+  double getErrorTol() const;
+  string getVerbosity() const;
+
+  void setMaxIterations(int value);
+  void setRelativeErrorTol(double value);
+  void setAbsoluteErrorTol(double value);
+  void setErrorTol(double value);
+  void setVerbosity(string s);
+
+  string getLinearSolverType() const;
+  void setLinearSolverType(string solver);
+
+  void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
+  void setOrdering(const gtsam::Ordering& ordering);
+  string getOrderingType() const;
+  void setOrderingType(string ordering);
+
+  bool isMultifrontal() const;
+  bool isSequential() const;
+  bool isCholmod() const;
+  bool isIterative() const;
+};
+
+bool checkConvergence(double relativeErrorTreshold,
+                      double absoluteErrorTreshold, double errorThreshold,
+                      double currentError, double newError);
+bool checkConvergence(const gtsam::NonlinearOptimizerParams& params,
+                      double currentError, double newError);
+
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
+  GaussNewtonParams();
+};
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
+  LevenbergMarquardtParams();
+
+  bool getDiagonalDamping() const;
+  double getlambdaFactor() const;
+  double getlambdaInitial() const;
+  double getlambdaLowerBound() const;
+  double getlambdaUpperBound() const;
+  bool getUseFixedLambdaFactor();
+  string getLogFile() const;
+  string getVerbosityLM() const;
+
+  void setDiagonalDamping(bool flag);
+  void setlambdaFactor(double value);
+  void setlambdaInitial(double value);
+  void setlambdaLowerBound(double value);
+  void setlambdaUpperBound(double value);
+  void setUseFixedLambdaFactor(bool flag);
+  void setLogFile(string s);
+  void setVerbosityLM(string s);
+
+  static gtsam::LevenbergMarquardtParams LegacyDefaults();
+  static gtsam::LevenbergMarquardtParams CeresDefaults();
+
+  static gtsam::LevenbergMarquardtParams EnsureHasOrdering(
+      gtsam::LevenbergMarquardtParams params,
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::LevenbergMarquardtParams ReplaceOrdering(
+      gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering);
+};
+
+#include <gtsam/nonlinear/DoglegOptimizer.h>
+virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
+  DoglegParams();
+
+  double getDeltaInitial() const;
+  string getVerbosityDL() const;
+
+  void setDeltaInitial(double deltaInitial) const;
+  void setVerbosityDL(string verbosityDL) const;
+};
+
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
+virtual class NonlinearOptimizer {
+  gtsam::Values optimize();
+  gtsam::Values optimizeSafely();
+  double error() const;
+  int iterations() const;
+  gtsam::Values values() const;
+  gtsam::NonlinearFactorGraph graph() const;
+  gtsam::GaussianFactorGraph* iterate() const;
+};
+
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer {
+  GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph,
+                       const gtsam::Values& initialValues);
+  GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph,
+                       const gtsam::Values& initialValues,
+                       const gtsam::GaussNewtonParams& params);
+};
+
+#include <gtsam/nonlinear/DoglegOptimizer.h>
+virtual class DoglegOptimizer : gtsam::NonlinearOptimizer {
+  DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph,
+                  const gtsam::Values& initialValues);
+  DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph,
+                  const gtsam::Values& initialValues,
+                  const gtsam::DoglegParams& params);
+  double getDelta() const;
+};
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {
+  LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph,
+                              const gtsam::Values& initialValues);
+  LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph,
+                              const gtsam::Values& initialValues,
+                              const gtsam::LevenbergMarquardtParams& params);
+  double lambda() const;
+  void print(string s = "") const;
+};
+
+#include <gtsam/nonlinear/ISAM2.h>
+class ISAM2GaussNewtonParams {
+  ISAM2GaussNewtonParams();
+
+  void print(string s = "") const;
+
+  /** Getters and Setters for all properties */
+  double getWildfireThreshold() const;
+  void setWildfireThreshold(double wildfireThreshold);
+};
+
+class ISAM2DoglegParams {
+  ISAM2DoglegParams();
+
+  void print(string s = "") const;
+
+  /** Getters and Setters for all properties */
+  double getWildfireThreshold() const;
+  void setWildfireThreshold(double wildfireThreshold);
+  double getInitialDelta() const;
+  void setInitialDelta(double initialDelta);
+  string getAdaptationMode() const;
+  void setAdaptationMode(string adaptationMode);
+  bool isVerbose() const;
+  void setVerbose(bool verbose);
+};
+
+class ISAM2ThresholdMapValue {
+  ISAM2ThresholdMapValue(char c, Vector thresholds);
+  ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other);
+};
+
+class ISAM2ThresholdMap {
+  ISAM2ThresholdMap();
+  ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other);
+
+  // Note: no print function
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(const gtsam::ISAM2ThresholdMapValue& value) const;
+};
+
+class ISAM2Params {
+  ISAM2Params();
+
+  void print(string s = "") const;
+
+  /** Getters and Setters for all properties */
+  void setOptimizationParams(
+      const gtsam::ISAM2GaussNewtonParams& gauss_newton__params);
+  void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params);
+  void setRelinearizeThreshold(double threshold);
+  void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map);
+  int getRelinearizeSkip() const;
+  void setRelinearizeSkip(int relinearizeSkip);
+  bool isEnableRelinearization() const;
+  void setEnableRelinearization(bool enableRelinearization);
+  bool isEvaluateNonlinearError() const;
+  void setEvaluateNonlinearError(bool evaluateNonlinearError);
+  string getFactorization() const;
+  void setFactorization(string factorization);
+  bool isCacheLinearizedFactors() const;
+  void setCacheLinearizedFactors(bool cacheLinearizedFactors);
+  bool isEnableDetailedResults() const;
+  void setEnableDetailedResults(bool enableDetailedResults);
+  bool isEnablePartialRelinearizationCheck() const;
+  void setEnablePartialRelinearizationCheck(
+      bool enablePartialRelinearizationCheck);
+};
+
+class ISAM2Clique {
+  // Constructors
+  ISAM2Clique();
+
+  // Standard Interface
+  Vector gradientContribution() const;
+  void print(string s = "",
+             gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
+};
+
+class ISAM2Result {
+  ISAM2Result();
+
+  void print(string s = "") const;
+
+  /** Getters and Setters for all properties */
+  size_t getVariablesRelinearized() const;
+  size_t getVariablesReeliminated() const;
+  size_t getCliques() const;
+  double getErrorBefore() const;
+  double getErrorAfter() const;
+};
+
+class ISAM2 {
+  ISAM2();
+  ISAM2(const gtsam::ISAM2Params& params);
+  ISAM2(const gtsam::ISAM2& other);
+
+  bool equals(const gtsam::ISAM2& other, double tol) const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  void printStats() const;
+  void saveGraph(string s) const;
+
+  gtsam::ISAM2Result update();
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+                            const gtsam::Values& newTheta);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+                            const gtsam::Values& newTheta,
+                            const gtsam::FactorIndices& removeFactorIndices);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+                            const gtsam::Values& newTheta,
+                            const gtsam::FactorIndices& removeFactorIndices,
+                            const gtsam::KeyGroupMap& constrainedKeys);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+                            const gtsam::Values& newTheta,
+                            const gtsam::FactorIndices& removeFactorIndices,
+                            gtsam::KeyGroupMap& constrainedKeys,
+                            const gtsam::KeyList& noRelinKeys);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+                            const gtsam::Values& newTheta,
+                            const gtsam::FactorIndices& removeFactorIndices,
+                            gtsam::KeyGroupMap& constrainedKeys,
+                            const gtsam::KeyList& noRelinKeys,
+                            const gtsam::KeyList& extraReelimKeys);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+                            const gtsam::Values& newTheta,
+                            const gtsam::FactorIndices& removeFactorIndices,
+                            gtsam::KeyGroupMap& constrainedKeys,
+                            const gtsam::KeyList& noRelinKeys,
+                            const gtsam::KeyList& extraReelimKeys,
+                            bool force_relinearize);
+
+  gtsam::Values getLinearizationPoint() const;
+  gtsam::Values calculateEstimate() const;
+  template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
+                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+                     gtsam::Cal3Bundler, gtsam::EssentialMatrix,
+                     gtsam::PinholeCamera<gtsam::Cal3_S2>,
+                     gtsam::PinholeCamera<gtsam::Cal3Bundler>, Vector, Matrix}>
+  VALUE calculateEstimate(size_t key) const;
+  gtsam::Values calculateBestEstimate() const;
+  Matrix marginalCovariance(size_t key) const;
+  gtsam::VectorValues getDelta() const;
+  gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
+  gtsam::VariableIndex getVariableIndex() const;
+  gtsam::ISAM2Params params() const;
+};
+
+#include <gtsam/nonlinear/NonlinearISAM.h>
+class NonlinearISAM {
+  NonlinearISAM();
+  NonlinearISAM(int reorderInterval);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  void printStats() const;
+  void saveGraph(string s) const;
+  gtsam::Values estimate() const;
+  Matrix marginalCovariance(size_t key) const;
+  int reorderInterval() const;
+  int reorderCounter() const;
+  void update(const gtsam::NonlinearFactorGraph& newFactors,
+              const gtsam::Values& initialValues);
+  void reorder_relinearize();
+
+  // These might be expensive as instead of a reference the wrapper will make a
+  // copy
+  gtsam::GaussianISAM bayesTree() const;
+  gtsam::Values getLinearizationPoint() const;
+  gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
+};
+
+//*************************************************************************
+// Nonlinear factor types
+//*************************************************************************
+#include <gtsam/nonlinear/PriorFactor.h>
+template <T = {double,
+               Vector,
+               gtsam::Point2,
+               gtsam::StereoPoint2,
+               gtsam::Point3,
+               gtsam::Rot2,
+               gtsam::SO3,
+               gtsam::SO4,
+               gtsam::SOn,
+               gtsam::Rot3,
+               gtsam::Pose2,
+               gtsam::Pose3,
+               gtsam::Unit3,
+               gtsam::Cal3_S2,
+               gtsam::Cal3DS2,
+               gtsam::Cal3Bundler,
+               gtsam::CalibratedCamera,
+               gtsam::PinholeCamera<gtsam::Cal3_S2>,
+               gtsam::imuBias::ConstantBias,
+               gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
+virtual class PriorFactor : gtsam::NoiseModelFactor {
+  PriorFactor(size_t key, const T& prior,
+              const gtsam::noiseModel::Base* noiseModel);
+  T prior() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/nonlinear/NonlinearEquality.h>
+template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
+               gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
+               gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
+               gtsam::PinholeCamera<gtsam::Cal3_S2>,
+               gtsam::imuBias::ConstantBias}>
+virtual class NonlinearEquality : gtsam::NoiseModelFactor {
+  // Constructor - forces exact evaluation
+  NonlinearEquality(size_t j, const T& feasible);
+  // Constructor - allows inexact evaluation
+  NonlinearEquality(size_t j, const T& feasible, double error_gain);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+}  // namespace gtsam
diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i
new file mode 100644
index 0000000000..370e1c3ea7
--- /dev/null
+++ b/gtsam/sam/sam.i
@@ -0,0 +1,96 @@
+//*************************************************************************
+// sam
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+
+// #####
+
+#include <gtsam/sam/RangeFactor.h>
+template <POSE, POINT>
+virtual class RangeFactor : gtsam::NoiseModelFactor {
+  RangeFactor(size_t key1, size_t key2, double measured,
+              const gtsam::noiseModel::Base* noiseModel);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
+typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3>
+    RangeFactorCalibratedCameraPoint;
+typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::Point3>
+    RangeFactorSimpleCameraPoint;
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera>
+    RangeFactorCalibratedCamera;
+typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>,
+                           gtsam::PinholeCamera<gtsam::Cal3_S2>>
+    RangeFactorSimpleCamera;
+
+#include <gtsam/sam/RangeFactor.h>
+template <POSE, POINT>
+virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
+  RangeFactorWithTransform(size_t key1, size_t key2, double measured,
+                           const gtsam::noiseModel::Base* noiseModel,
+                           const POSE& body_T_sensor);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2>
+    RangeFactorWithTransform2D;
+typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3>
+    RangeFactorWithTransform3D;
+typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Pose2>
+    RangeFactorWithTransformPose2;
+typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Pose3>
+    RangeFactorWithTransformPose3;
+
+#include <gtsam/sam/BearingFactor.h>
+template <POSE, POINT, BEARING>
+virtual class BearingFactor : gtsam::NoiseModelFactor {
+  BearingFactor(size_t key1, size_t key2, const BEARING& measured,
+                const gtsam::noiseModel::Base* noiseModel);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2>
+    BearingFactor2D;
+typedef gtsam::BearingFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3>
+    BearingFactor3D;
+typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2>
+    BearingFactorPose2;
+
+#include <gtsam/sam/BearingRangeFactor.h>
+template <POSE, POINT, BEARING, RANGE>
+virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
+  BearingRangeFactor(size_t poseKey, size_t pointKey,
+                     const BEARING& measuredBearing, const RANGE& measuredRange,
+                     const gtsam::noiseModel::Base* noiseModel);
+
+  gtsam::BearingRange<POSE, POINT, BEARING, RANGE> measured() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2,
+                                  double>
+    BearingRangeFactor2D;
+typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2,
+                                  double>
+    BearingRangeFactorPose2;
+
+}  // namespace gtsam
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
new file mode 100644
index 0000000000..c86de8d88e
--- /dev/null
+++ b/gtsam/sfm/sfm.i
@@ -0,0 +1,209 @@
+//*************************************************************************
+// sfm
+//*************************************************************************
+
+namespace gtsam {
+
+// #####
+
+#include <gtsam/sfm/ShonanFactor.h>
+
+virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
+  ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p);
+  ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p,
+                gtsam::noiseModel::Base* model);
+  Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
+};
+
+#include <gtsam/sfm/BinaryMeasurement.h>
+template <T>
+class BinaryMeasurement {
+  BinaryMeasurement(size_t key1, size_t key2, const T& measured,
+                    const gtsam::noiseModel::Base* model);
+  size_t key1() const;
+  size_t key2() const;
+  T measured() const;
+  gtsam::noiseModel::Base* noiseModel() const;
+};
+
+typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
+typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
+
+class BinaryMeasurementsUnit3 {
+  BinaryMeasurementsUnit3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
+};
+
+#include <gtsam/sfm/ShonanAveraging.h>
+
+// TODO(frank): copy/pasta below until we have integer template paremeters in
+// wrap!
+
+class ShonanAveragingParameters2 {
+  ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm);
+  ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm,
+                             string method);
+  gtsam::LevenbergMarquardtParams getLMParams() const;
+  void setOptimalityThreshold(double value);
+  double getOptimalityThreshold() const;
+  void setAnchor(size_t index, const gtsam::Rot2& value);
+  pair<size_t, gtsam::Rot2> getAnchor();
+  void setAnchorWeight(double value);
+  double getAnchorWeight() const;
+  void setKarcherWeight(double value);
+  double getKarcherWeight() const;
+  void setGaugesWeight(double value);
+  double getGaugesWeight() const;
+  void setUseHuber(bool value);
+  bool getUseHuber() const;
+  void setCertifyOptimality(bool value);
+  bool getCertifyOptimality() const;
+};
+
+class ShonanAveragingParameters3 {
+  ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm);
+  ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm,
+                             string method);
+  gtsam::LevenbergMarquardtParams getLMParams() const;
+  void setOptimalityThreshold(double value);
+  double getOptimalityThreshold() const;
+  void setAnchor(size_t index, const gtsam::Rot3& value);
+  pair<size_t, gtsam::Rot3> getAnchor();
+  void setAnchorWeight(double value);
+  double getAnchorWeight() const;
+  void setKarcherWeight(double value);
+  double getKarcherWeight() const;
+  void setGaugesWeight(double value);
+  double getGaugesWeight() const;
+  void setUseHuber(bool value);
+  bool getUseHuber() const;
+  void setCertifyOptimality(bool value);
+  bool getCertifyOptimality() const;
+};
+
+class ShonanAveraging2 {
+  ShonanAveraging2(string g2oFile);
+  ShonanAveraging2(string g2oFile,
+                   const gtsam::ShonanAveragingParameters2& parameters);
+
+  // Query properties
+  size_t nrUnknowns() const;
+  size_t nrMeasurements() const;
+  gtsam::Rot2 measured(size_t i);
+  gtsam::KeyVector keys(size_t i);
+
+  // Matrix API (advanced use, debugging)
+  Matrix denseD() const;
+  Matrix denseQ() const;
+  Matrix denseL() const;
+  // Matrix computeLambda_(Matrix S) const;
+  Matrix computeLambda_(const gtsam::Values& values) const;
+  Matrix computeA_(const gtsam::Values& values) const;
+  double computeMinEigenValue(const gtsam::Values& values) const;
+  gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
+                                      const Vector& minEigenVector,
+                                      double minEigenValue) const;
+
+  // Advanced API
+  gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
+  gtsam::Values initializeRandomlyAt(size_t p) const;
+  double costAt(size_t p, const gtsam::Values& values) const;
+  pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
+  bool checkOptimality(const gtsam::Values& values) const;
+  gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(
+      size_t p, const gtsam::Values& initial);
+  // gtsam::Values tryOptimizingAt(size_t p) const;
+  gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
+  gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
+  gtsam::Values roundSolution(const gtsam::Values& values) const;
+
+  // Basic API
+  double cost(const gtsam::Values& values) const;
+  gtsam::Values initializeRandomly() const;
+  pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p,
+                                  size_t max_p) const;
+};
+
+class ShonanAveraging3 {
+  ShonanAveraging3(string g2oFile);
+  ShonanAveraging3(string g2oFile,
+                   const gtsam::ShonanAveragingParameters3& parameters);
+
+  // TODO(frank): deprecate once we land pybind wrapper
+  ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors);
+  ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors,
+                   const gtsam::ShonanAveragingParameters3& parameters);
+
+  // Query properties
+  size_t nrUnknowns() const;
+  size_t nrMeasurements() const;
+  gtsam::Rot3 measured(size_t i);
+  gtsam::KeyVector keys(size_t i);
+
+  // Matrix API (advanced use, debugging)
+  Matrix denseD() const;
+  Matrix denseQ() const;
+  Matrix denseL() const;
+  // Matrix computeLambda_(Matrix S) const;
+  Matrix computeLambda_(const gtsam::Values& values) const;
+  Matrix computeA_(const gtsam::Values& values) const;
+  double computeMinEigenValue(const gtsam::Values& values) const;
+  gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
+                                      const Vector& minEigenVector,
+                                      double minEigenValue) const;
+
+  // Advanced API
+  gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
+  gtsam::Values initializeRandomlyAt(size_t p) const;
+  double costAt(size_t p, const gtsam::Values& values) const;
+  pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
+  bool checkOptimality(const gtsam::Values& values) const;
+  gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(
+      size_t p, const gtsam::Values& initial);
+  // gtsam::Values tryOptimizingAt(size_t p) const;
+  gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
+  gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
+  gtsam::Values roundSolution(const gtsam::Values& values) const;
+
+  // Basic API
+  double cost(const gtsam::Values& values) const;
+  gtsam::Values initializeRandomly() const;
+  pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p,
+                                  size_t max_p) const;
+};
+
+#include <gtsam/sfm/MFAS.h>
+
+class KeyPairDoubleMap {
+  KeyPairDoubleMap();
+  KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
+
+  size_t size() const;
+  bool empty() const;
+  void clear();
+  size_t at(const pair<size_t, size_t>& keypair) const;
+};
+
+class MFAS {
+  MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+       const gtsam::Unit3& projectionDirection);
+
+  gtsam::KeyPairDoubleMap computeOutlierWeights() const;
+  gtsam::KeyVector computeOrdering() const;
+};
+
+#include <gtsam/sfm/TranslationRecovery.h>
+class TranslationRecovery {
+  TranslationRecovery(
+      const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+      const gtsam::LevenbergMarquardtParams& lmParams);
+  TranslationRecovery(
+      const gtsam::BinaryMeasurementsUnit3&
+          relativeTranslations);  // default LevenbergMarquardtParams
+  gtsam::Values run(const double scale) const;
+  gtsam::Values run() const;  // default scale = 1.0
+};
+
+}  // namespace gtsam
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
new file mode 100644
index 0000000000..457e907b92
--- /dev/null
+++ b/gtsam/slam/slam.i
@@ -0,0 +1,325 @@
+//*************************************************************************
+// slam
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/navigation/ImuBias.h>
+
+// ######
+
+#include <gtsam/slam/BetweenFactor.h>
+template <T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
+               gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3,
+               gtsam::imuBias::ConstantBias}>
+virtual class BetweenFactor : gtsam::NoiseModelFactor {
+  BetweenFactor(size_t key1, size_t key2, const T& relativePose,
+                const gtsam::noiseModel::Base* noiseModel);
+  T measured() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/slam/ProjectionFactor.h>
+template <POSE, LANDMARK, CALIBRATION>
+virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
+  GenericProjectionFactor(const gtsam::Point2& measured,
+                          const gtsam::noiseModel::Base* noiseModel,
+                          size_t poseKey, size_t pointKey,
+                          const CALIBRATION* k);
+  GenericProjectionFactor(const gtsam::Point2& measured,
+                          const gtsam::noiseModel::Base* noiseModel,
+                          size_t poseKey, size_t pointKey, const CALIBRATION* k,
+                          const POSE& body_P_sensor);
+
+  GenericProjectionFactor(const gtsam::Point2& measured,
+                          const gtsam::noiseModel::Base* noiseModel,
+                          size_t poseKey, size_t pointKey, const CALIBRATION* k,
+                          bool throwCheirality, bool verboseCheirality);
+  GenericProjectionFactor(const gtsam::Point2& measured,
+                          const gtsam::noiseModel::Base* noiseModel,
+                          size_t poseKey, size_t pointKey, const CALIBRATION* k,
+                          bool throwCheirality, bool verboseCheirality,
+                          const POSE& body_P_sensor);
+
+  gtsam::Point2 measured() const;
+  CALIBRATION* calibration() const;
+  bool verboseCheirality() const;
+  bool throwCheirality() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
+                                       gtsam::Cal3_S2>
+    GenericProjectionFactorCal3_S2;
+typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
+                                       gtsam::Cal3DS2>
+    GenericProjectionFactorCal3DS2;
+
+#include <gtsam/slam/GeneralSFMFactor.h>
+template <CAMERA, LANDMARK>
+virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
+  GeneralSFMFactor(const gtsam::Point2& measured,
+                   const gtsam::noiseModel::Base* model, size_t cameraKey,
+                   size_t landmarkKey);
+  gtsam::Point2 measured() const;
+};
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>,
+                                gtsam::Point3>
+    GeneralSFMFactorCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3DS2>,
+                                gtsam::Point3>
+    GeneralSFMFactorCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                                gtsam::Point3>
+    GeneralSFMFactorCal3Bundler;
+
+template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
+virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
+  GeneralSFMFactor2(const gtsam::Point2& measured,
+                    const gtsam::noiseModel::Base* model, size_t poseKey,
+                    size_t landmarkKey, size_t calibKey);
+  gtsam::Point2 measured() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/slam/SmartProjectionFactor.h>
+
+/// Linearization mode: what factor to linearize to
+enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD };
+
+/// How to manage degeneracy
+enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY };
+
+class SmartProjectionParams {
+  SmartProjectionParams();
+  void setLinearizationMode(gtsam::LinearizationMode linMode);
+  void setDegeneracyMode(gtsam::DegeneracyMode degMode);
+  void setRankTolerance(double rankTol);
+  void setEnableEPI(bool enableEPI);
+  void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
+  void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
+};
+
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+template <CALIBRATION>
+virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+                            const CALIBRATION* K);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+                            const CALIBRATION* K,
+                            const gtsam::Pose3& body_P_sensor);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+                            const CALIBRATION* K,
+                            const gtsam::SmartProjectionParams& params);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+                            const CALIBRATION* K,
+                            const gtsam::Pose3& body_P_sensor,
+                            const gtsam::SmartProjectionParams& params);
+
+  void add(const gtsam::Point2& measured_i, size_t poseKey_i);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>
+    SmartProjectionPose3Factor;
+
+#include <gtsam/slam/StereoFactor.h>
+template <POSE, LANDMARK>
+virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
+  GenericStereoFactor(const gtsam::StereoPoint2& measured,
+                      const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
+                      size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
+  gtsam::StereoPoint2 measured() const;
+  gtsam::Cal3_S2Stereo* calibration() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>
+    GenericStereoFactor3D;
+
+#include <gtsam/slam/PoseTranslationPrior.h>
+template <POSE>
+virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
+  PoseTranslationPrior(size_t key, const POSE& pose_z,
+                       const gtsam::noiseModel::Base* noiseModel);
+};
+
+typedef gtsam::PoseTranslationPrior<gtsam::Pose2> PoseTranslationPrior2D;
+typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
+
+#include <gtsam/slam/PoseRotationPrior.h>
+template <POSE>
+virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
+  PoseRotationPrior(size_t key, const POSE& pose_z,
+                    const gtsam::noiseModel::Base* noiseModel);
+};
+
+typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
+typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
+
+#include <gtsam/slam/EssentialMatrixFactor.h>
+virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
+  EssentialMatrixFactor(size_t key, const gtsam::Point2& pA,
+                        const gtsam::Point2& pB,
+                        const gtsam::noiseModel::Base* noiseModel);
+};
+
+#include <gtsam/slam/dataset.h>
+
+class SfmTrack {
+  SfmTrack();
+  SfmTrack(const gtsam::Point3& pt);
+  const Point3& point3() const;
+
+  double r;
+  double g;
+  double b;
+
+  std::vector<pair<size_t, gtsam::Point2>> measurements;
+
+  size_t number_measurements() const;
+  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
+  pair<size_t, size_t> siftIndex(size_t idx) const;
+  void add_measurement(size_t idx, const gtsam::Point2& m);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::SfmTrack& expected, double tol) const;
+};
+
+class SfmData {
+  SfmData();
+  size_t number_cameras() const;
+  size_t number_tracks() const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::SfmTrack track(size_t idx) const;
+  void add_track(const gtsam::SfmTrack& t);
+  void add_camera(const gtsam::SfmCamera& cam);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::SfmData& expected, double tol) const;
+};
+
+gtsam::SfmData readBal(string filename);
+bool writeBAL(string filename, gtsam::SfmData& data);
+gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
+gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
+    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
+    bool addNoise, bool smart);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
+    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
+    bool addNoise);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
+    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
+    string filename, gtsam::noiseModel::Diagonal* model);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(
+    string filename, gtsam::noiseModel::Base* model, int maxIndex);
+void save2D(const gtsam::NonlinearFactorGraph& graph,
+            const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
+            string filename);
+
+// std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>
+// Ignored by pybind -> will be List[BetweenFactorPose2]
+class BetweenFactorPose2s {
+  BetweenFactorPose2s();
+  size_t size() const;
+  gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
+};
+gtsam::BetweenFactorPose2s parse2DFactors(string filename);
+
+// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
+// Ignored by pybind -> will be List[BetweenFactorPose3]
+class BetweenFactorPose3s {
+  BetweenFactorPose3s();
+  size_t size() const;
+  gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
+};
+gtsam::BetweenFactorPose3s parse3DFactors(string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename,
+                                                           bool is3D);
+void writeG2o(const gtsam::NonlinearFactorGraph& graph,
+              const gtsam::Values& estimate, string filename);
+
+#include <gtsam/slam/InitializePose3.h>
+class InitializePose3 {
+  static gtsam::Values computeOrientationsChordal(
+      const gtsam::NonlinearFactorGraph& pose3Graph);
+  static gtsam::Values computeOrientationsGradient(
+      const gtsam::NonlinearFactorGraph& pose3Graph,
+      const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
+  static gtsam::Values computeOrientationsGradient(
+      const gtsam::NonlinearFactorGraph& pose3Graph,
+      const gtsam::Values& givenGuess);
+  static gtsam::NonlinearFactorGraph buildPose3graph(
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::Values initializeOrientations(
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
+                                  const gtsam::Values& givenGuess,
+                                  bool useGradient);
+  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
+};
+
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
+template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
+               gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
+virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
+  KarcherMeanFactor(const gtsam::KeyVector& keys);
+};
+
+#include <gtsam/slam/FrobeniusFactor.h>
+gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
+                                                size_t d);
+
+template <T = {gtsam::SO3, gtsam::SO4}>
+virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
+  FrobeniusFactor(size_t key1, size_t key2);
+  FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
+
+  Vector evaluateError(const T& R1, const T& R2);
+};
+
+template <T = {gtsam::SO3, gtsam::SO4}>
+virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12,
+                         gtsam::noiseModel::Base* model);
+
+  Vector evaluateError(const T& R1, const T& R2);
+};
+
+}  // namespace gtsam
diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i
new file mode 100644
index 0000000000..4e7cca68ae
--- /dev/null
+++ b/gtsam/symbolic/symbolic.i
@@ -0,0 +1,201 @@
+//*************************************************************************
+// Symbolic
+//*************************************************************************
+namespace gtsam {
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+// ###################
+
+#include <gtsam/symbolic/SymbolicFactor.h>
+virtual class SymbolicFactor {
+  // Standard Constructors and Named Constructors
+  SymbolicFactor(const gtsam::SymbolicFactor& f);
+  SymbolicFactor();
+  SymbolicFactor(size_t j);
+  SymbolicFactor(size_t j1, size_t j2);
+  SymbolicFactor(size_t j1, size_t j2, size_t j3);
+  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4);
+  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5);
+  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5,
+                 size_t j6);
+  static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js);
+
+  // From Factor
+  size_t size() const;
+  void print(string s = "SymbolicFactor",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::SymbolicFactor& other, double tol) const;
+  gtsam::KeyVector keys();
+};
+
+#include <gtsam/symbolic/SymbolicFactorGraph.h>
+virtual class SymbolicFactorGraph {
+  SymbolicFactorGraph();
+  SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
+  SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
+
+  // From FactorGraph
+  void push_back(gtsam::SymbolicFactor* factor);
+  void print(string s = "SymbolicFactorGraph",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
+  size_t size() const;
+  bool exists(size_t idx) const;
+
+  // Standard interface
+  gtsam::KeySet keys() const;
+  void push_back(const gtsam::SymbolicFactorGraph& graph);
+  void push_back(const gtsam::SymbolicBayesNet& bayesNet);
+  void push_back(const gtsam::SymbolicBayesTree& bayesTree);
+
+  // Advanced Interface
+  void push_factor(size_t key);
+  void push_factor(size_t key1, size_t key2);
+  void push_factor(size_t key1, size_t key2, size_t key3);
+  void push_factor(size_t key1, size_t key2, size_t key3, size_t key4);
+
+  gtsam::SymbolicBayesNet* eliminateSequential();
+  gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
+  gtsam::SymbolicBayesTree* eliminateMultifrontal();
+  gtsam::SymbolicBayesTree* eliminateMultifrontal(
+      const gtsam::Ordering& ordering);
+  pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*>
+  eliminatePartialSequential(const gtsam::Ordering& ordering);
+  pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*>
+  eliminatePartialSequential(const gtsam::KeyVector& keys);
+  pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*>
+  eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
+  pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*>
+  eliminatePartialMultifrontal(const gtsam::KeyVector& keys);
+  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(
+      const gtsam::Ordering& ordering);
+  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(
+      const gtsam::KeyVector& key_vector);
+  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(
+      const gtsam::Ordering& ordering,
+      const gtsam::Ordering& marginalizedVariableOrdering);
+  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(
+      const gtsam::KeyVector& key_vector,
+      const gtsam::Ordering& marginalizedVariableOrdering);
+  gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector);
+};
+
+#include <gtsam/symbolic/SymbolicConditional.h>
+virtual class SymbolicConditional : gtsam::SymbolicFactor {
+  // Standard Constructors and Named Constructors
+  SymbolicConditional();
+  SymbolicConditional(const gtsam::SymbolicConditional& other);
+  SymbolicConditional(size_t key);
+  SymbolicConditional(size_t key, size_t parent);
+  SymbolicConditional(size_t key, size_t parent1, size_t parent2);
+  SymbolicConditional(size_t key, size_t parent1, size_t parent2,
+                      size_t parent3);
+  static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys,
+                                             size_t nrFrontals);
+
+  // Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::SymbolicConditional& other, double tol) const;
+
+  // Standard interface
+  size_t nrFrontals() const;
+  size_t nrParents() const;
+};
+
+#include <gtsam/symbolic/SymbolicBayesNet.h>
+class SymbolicBayesNet {
+  SymbolicBayesNet();
+  SymbolicBayesNet(const gtsam::SymbolicBayesNet& other);
+  // Testable
+  void print(string s = "SymbolicBayesNet",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::SymbolicBayesNet& other, double tol) const;
+
+  // Standard interface
+  size_t size() const;
+  void saveGraph(string s) const;
+  gtsam::SymbolicConditional* at(size_t idx) const;
+  gtsam::SymbolicConditional* front() const;
+  gtsam::SymbolicConditional* back() const;
+  void push_back(gtsam::SymbolicConditional* conditional);
+  void push_back(const gtsam::SymbolicBayesNet& bayesNet);
+};
+
+#include <gtsam/symbolic/SymbolicBayesTree.h>
+class SymbolicBayesTree {
+  // Constructors
+  SymbolicBayesTree();
+  SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
+
+  // Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter);
+  bool equals(const gtsam::SymbolicBayesTree& other, double tol) const;
+
+  // Standard Interface
+  // size_t findParentClique(const gtsam::IndexVector& parents) const;
+  size_t size();
+  void saveGraph(string s) const;
+  void clear();
+  void deleteCachedShortcuts();
+  size_t numCachedSeparatorMarginals() const;
+
+  gtsam::SymbolicConditional* marginalFactor(size_t key) const;
+  gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
+  gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
+};
+
+class SymbolicBayesTreeClique {
+  SymbolicBayesTreeClique();
+  // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional);
+
+  bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  size_t numCachedSeparatorMarginals() const;
+  // gtsam::sharedConditional* conditional() const;
+  bool isRoot() const;
+  size_t treeSize() const;
+  gtsam::SymbolicBayesTreeClique* parent() const;
+
+  //   // TODO: need wrapped versions graphs, BayesNet
+  //  BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function)
+  //  const; FactorGraph<FactorType> marginal(derived_ptr root, Eliminate
+  //  function) const; FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr
+  //  root, Eliminate function) const;
+  //
+  void deleteCachedShortcuts();
+};
+
+#include <gtsam/inference/VariableIndex.h>
+class VariableIndex {
+  // Standard Constructors and Named Constructors
+  VariableIndex();
+  // TODO: Templetize constructor when wrap supports it
+  // template<T = {gtsam::FactorGraph}>
+  // VariableIndex(const T& factorGraph, size_t nVariables);
+  // VariableIndex(const T& factorGraph);
+  VariableIndex(const gtsam::SymbolicFactorGraph& sfg);
+  VariableIndex(const gtsam::GaussianFactorGraph& gfg);
+  VariableIndex(const gtsam::NonlinearFactorGraph& fg);
+  VariableIndex(const gtsam::VariableIndex& other);
+
+  // Testable
+  bool equals(const gtsam::VariableIndex& other, double tol) const;
+  void print(string s = "VariableIndex: ",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+
+  // Standard interface
+  size_t size() const;
+  size_t nFactors() const;
+  size_t nEntries() const;
+};
+
+}  // namespace gtsam

From f33e6a801f5cb9a316dc29b0c96e6ac3f605d16c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:10:05 -0700
Subject: [PATCH 47/72] break up preamble and specializations so there are no
 duplicate includes

---
 python/gtsam/preamble.h                   | 30 -------------------
 python/gtsam/preamble/base.h              | 16 +++++++++++
 python/gtsam/preamble/geometry.h          | 21 ++++++++++++++
 python/gtsam/preamble/gtsam.h             | 17 +++++++++++
 python/gtsam/preamble/linear.h            | 12 ++++++++
 python/gtsam/preamble/navigation.h        | 18 ++++++++++++
 python/gtsam/preamble/nonlinear.h         | 12 ++++++++
 python/gtsam/preamble/sam.h               | 12 ++++++++
 python/gtsam/preamble/sfm.h               | 12 ++++++++
 python/gtsam/preamble/slam.h              | 17 +++++++++++
 python/gtsam/preamble/symbolic.h          | 12 ++++++++
 python/gtsam/specializations.h            | 35 -----------------------
 python/gtsam/specializations/base.h       | 17 +++++++++++
 python/gtsam/specializations/geometry.h   | 23 +++++++++++++++
 python/gtsam/specializations/gtsam.h      | 20 +++++++++++++
 python/gtsam/specializations/linear.h     | 12 ++++++++
 python/gtsam/specializations/navigation.h | 12 ++++++++
 python/gtsam/specializations/nonlinear.h  | 12 ++++++++
 python/gtsam/specializations/sam.h        | 12 ++++++++
 python/gtsam/specializations/sfm.h        | 16 +++++++++++
 python/gtsam/specializations/slam.h       | 19 ++++++++++++
 python/gtsam/specializations/symbolic.h   | 12 ++++++++
 22 files changed, 304 insertions(+), 65 deletions(-)
 delete mode 100644 python/gtsam/preamble.h
 create mode 100644 python/gtsam/preamble/base.h
 create mode 100644 python/gtsam/preamble/geometry.h
 create mode 100644 python/gtsam/preamble/gtsam.h
 create mode 100644 python/gtsam/preamble/linear.h
 create mode 100644 python/gtsam/preamble/navigation.h
 create mode 100644 python/gtsam/preamble/nonlinear.h
 create mode 100644 python/gtsam/preamble/sam.h
 create mode 100644 python/gtsam/preamble/sfm.h
 create mode 100644 python/gtsam/preamble/slam.h
 create mode 100644 python/gtsam/preamble/symbolic.h
 delete mode 100644 python/gtsam/specializations.h
 create mode 100644 python/gtsam/specializations/base.h
 create mode 100644 python/gtsam/specializations/geometry.h
 create mode 100644 python/gtsam/specializations/gtsam.h
 create mode 100644 python/gtsam/specializations/linear.h
 create mode 100644 python/gtsam/specializations/navigation.h
 create mode 100644 python/gtsam/specializations/nonlinear.h
 create mode 100644 python/gtsam/specializations/sam.h
 create mode 100644 python/gtsam/specializations/sfm.h
 create mode 100644 python/gtsam/specializations/slam.h
 create mode 100644 python/gtsam/specializations/symbolic.h

diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h
deleted file mode 100644
index 7294a6ac8e..0000000000
--- a/python/gtsam/preamble.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
- * These are required to save one copy operation on Python calls.
- *
- * NOTES
- * =================
- *
- * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding,
- * such that the raw objects can be accessed in Python. Without this they will be automatically
- * converted to a Python object, and all mutations on Python side will not be reflected on C++.
- */
-#ifdef GTSAM_ALLOCATOR_TBB
-PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
-#else
-PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
-#endif
-PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
-PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
-PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
-PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
-PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
-PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
-PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
-PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >);
-PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >);
-PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Matrix>); // JacobianVector
-
-// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this.
-namespace std {
-  using gtsam::operator<<;
-}
diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h
new file mode 100644
index 0000000000..626b47ae4a
--- /dev/null
+++ b/python/gtsam/preamble/base.h
@@ -0,0 +1,16 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
+
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
+
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Matrix>);  // JacobianVector
diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h
new file mode 100644
index 0000000000..498c7dc58f
--- /dev/null
+++ b/python/gtsam/preamble/geometry.h
@@ -0,0 +1,21 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
+PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
+PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
+PYBIND11_MAKE_OPAQUE(
+    gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
+PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);
diff --git a/python/gtsam/preamble/gtsam.h b/python/gtsam/preamble/gtsam.h
new file mode 100644
index 0000000000..ec39c410a8
--- /dev/null
+++ b/python/gtsam/preamble/gtsam.h
@@ -0,0 +1,17 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
+#ifdef GTSAM_ALLOCATOR_TBB
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
+#else
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
+#endif
diff --git a/python/gtsam/preamble/linear.h b/python/gtsam/preamble/linear.h
new file mode 100644
index 0000000000..a34e730580
--- /dev/null
+++ b/python/gtsam/preamble/linear.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
\ No newline at end of file
diff --git a/python/gtsam/preamble/navigation.h b/python/gtsam/preamble/navigation.h
new file mode 100644
index 0000000000..b647ef029a
--- /dev/null
+++ b/python/gtsam/preamble/navigation.h
@@ -0,0 +1,18 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
+
+// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair.
+// We should find a way to NOT do this.
+namespace std {
+using gtsam::operator<<;
+}
diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h
new file mode 100644
index 0000000000..a34e730580
--- /dev/null
+++ b/python/gtsam/preamble/nonlinear.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
\ No newline at end of file
diff --git a/python/gtsam/preamble/sam.h b/python/gtsam/preamble/sam.h
new file mode 100644
index 0000000000..a34e730580
--- /dev/null
+++ b/python/gtsam/preamble/sam.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
\ No newline at end of file
diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h
new file mode 100644
index 0000000000..a34e730580
--- /dev/null
+++ b/python/gtsam/preamble/sfm.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
\ No newline at end of file
diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h
new file mode 100644
index 0000000000..34dbb4b7ab
--- /dev/null
+++ b/python/gtsam/preamble/slam.h
@@ -0,0 +1,17 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
diff --git a/python/gtsam/preamble/symbolic.h b/python/gtsam/preamble/symbolic.h
new file mode 100644
index 0000000000..a34e730580
--- /dev/null
+++ b/python/gtsam/preamble/symbolic.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
\ No newline at end of file
diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h
deleted file mode 100644
index be8eb8a6cf..0000000000
--- a/python/gtsam/specializations.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
- * These are required to save one copy operation on Python calls.
- *
- * NOTES
- * =================
- *
- * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding,
- * such that the raw objects can be accessed in Python. Without this they will be automatically
- * converted to a Python object, and all mutations on Python side will not be reflected on C++.
- *
- * `py::bind_vector` and similar machinery gives the std container a Python-like interface, but
- * without the `<pybind11/stl.h>` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this
- * allows the types to be modified with Python, and saves one copy operation.
- */
-#ifdef GTSAM_ALLOCATOR_TBB
-py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
-py::implicitly_convertible<py::list, std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >();
-#else
-py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
-py::implicitly_convertible<py::list, std::vector<gtsam::Key> >();
-#endif
-
-py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
-py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "Point3Pairs");
-py::bind_vector<std::vector<gtsam::Pose3Pair> >(m_, "Pose3Pairs");
-py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
-py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
-py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");
-py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
-py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
-py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
-py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
-py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
-py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
-py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");
diff --git a/python/gtsam/specializations/base.h b/python/gtsam/specializations/base.h
new file mode 100644
index 0000000000..9eefdeca82
--- /dev/null
+++ b/python/gtsam/specializations/base.h
@@ -0,0 +1,17 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
+
+py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
+py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
+
+py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");
diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h
new file mode 100644
index 0000000000..3d22581d98
--- /dev/null
+++ b/python/gtsam/specializations/geometry.h
@@ -0,0 +1,23 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
+
+py::bind_vector<
+    std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>>(
+    m_, "Point2Vector");
+py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs");
+py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
+py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(
+    m_, "CameraSetCal3_S2");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>>(
+    m_, "CameraSetCal3Bundler");
diff --git a/python/gtsam/specializations/gtsam.h b/python/gtsam/specializations/gtsam.h
new file mode 100644
index 0000000000..490d719023
--- /dev/null
+++ b/python/gtsam/specializations/gtsam.h
@@ -0,0 +1,20 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
+
+#ifdef GTSAM_ALLOCATOR_TBB
+py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
+py::implicitly_convertible<py::list, std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >();
+#else
+py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
+py::implicitly_convertible<py::list, std::vector<gtsam::Key> >();
+#endif
diff --git a/python/gtsam/specializations/linear.h b/python/gtsam/specializations/linear.h
new file mode 100644
index 0000000000..d46ccdc668
--- /dev/null
+++ b/python/gtsam/specializations/linear.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
\ No newline at end of file
diff --git a/python/gtsam/specializations/navigation.h b/python/gtsam/specializations/navigation.h
new file mode 100644
index 0000000000..d46ccdc668
--- /dev/null
+++ b/python/gtsam/specializations/navigation.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
\ No newline at end of file
diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h
new file mode 100644
index 0000000000..d46ccdc668
--- /dev/null
+++ b/python/gtsam/specializations/nonlinear.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
\ No newline at end of file
diff --git a/python/gtsam/specializations/sam.h b/python/gtsam/specializations/sam.h
new file mode 100644
index 0000000000..d46ccdc668
--- /dev/null
+++ b/python/gtsam/specializations/sam.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
\ No newline at end of file
diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h
new file mode 100644
index 0000000000..6de15217fb
--- /dev/null
+++ b/python/gtsam/specializations/sfm.h
@@ -0,0 +1,16 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
+
+py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
+    m_, "BinaryMeasurementsUnit3");
+py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h
new file mode 100644
index 0000000000..198485a72e
--- /dev/null
+++ b/python/gtsam/specializations/slam.h
@@ -0,0 +1,19 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
+
+py::bind_vector<
+    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(
+    m_, "BetweenFactorPose3s");
+py::bind_vector<
+    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(
+    m_, "BetweenFactorPose2s");
diff --git a/python/gtsam/specializations/symbolic.h b/python/gtsam/specializations/symbolic.h
new file mode 100644
index 0000000000..d46ccdc668
--- /dev/null
+++ b/python/gtsam/specializations/symbolic.h
@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
\ No newline at end of file

From 54063934fab2d041cd54b60c2c0ba7ac624290b6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:10:06 -0700
Subject: [PATCH 48/72] update template for wrapper

---
 python/gtsam/gtsam.tpl | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl
index b800f7c35b..93e7ffbafd 100644
--- a/python/gtsam/gtsam.tpl
+++ b/python/gtsam/gtsam.tpl
@@ -18,7 +18,7 @@
 #include <pybind11/iostream.h>
 #include "gtsam/config.h"
 #include "gtsam/base/serialization.h"
-#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+#include "gtsam/base/utilities.h"  // for RedirectCout.
 
 // These are the included headers listed in `gtsam.i`
 {includes}
@@ -32,20 +32,24 @@
 
 // Preamble for STL classes
 // TODO(fan): make this automatic
-#include "python/gtsam/preamble.h"
+#include "python/gtsam/preamble/{module_name}.h"
 
 using namespace std;
 
 namespace py = pybind11;
 
-PYBIND11_MODULE({module_name}, m_) {{
+{submodules}
+
+{module_def} {{
     m_.doc() = "pybind11 wrapper of {module_name}";
 
+{submodules_init}
+
 {wrapped_namespace}
 
 // Specializations for STL classes
 // TODO(fan): make this automatic
-#include "python/gtsam/specializations.h"
+#include "python/gtsam/specializations/{module_name}.h"
 
 }}
 

From 86c47d52d571d427323288197a5b8a07209cbb7a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:10:08 -0700
Subject: [PATCH 49/72] move RedirectCout to base/utilities.h

---
 gtsam/base/utilities.h      | 29 +++++++++++++++++++++++++++++
 gtsam/nonlinear/utilities.h | 25 -------------------------
 2 files changed, 29 insertions(+), 25 deletions(-)
 create mode 100644 gtsam/base/utilities.h

diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h
new file mode 100644
index 0000000000..8eb5617a8e
--- /dev/null
+++ b/gtsam/base/utilities.h
@@ -0,0 +1,29 @@
+#pragma once
+
+namespace gtsam {
+/**
+ * For Python __str__().
+ * Redirect std cout to a string stream so we can return a string representation
+ * of an object when it prints to cout.
+ * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string
+ */
+struct RedirectCout {
+  /// constructor -- redirect stdout buffer to a stringstream buffer
+  RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {}
+
+  /// return the string
+  std::string str() const {
+    return ssBuffer_.str();
+  }
+
+  /// destructor -- redirect stdout buffer to its original buffer
+  ~RedirectCout() {
+    std::cout.rdbuf(coutBuffer_);
+  }
+
+private:
+  std::stringstream ssBuffer_;
+  std::streambuf* coutBuffer_;
+};
+
+}
diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h
index ffc279872d..4e79e20ffd 100644
--- a/gtsam/nonlinear/utilities.h
+++ b/gtsam/nonlinear/utilities.h
@@ -260,30 +260,5 @@ Values localToWorld(const Values& local, const Pose2& base,
 
 } // namespace utilities
 
-/**
- * For Python __str__().
- * Redirect std cout to a string stream so we can return a string representation
- * of an object when it prints to cout.
- * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string
- */
-struct RedirectCout {
-  /// constructor -- redirect stdout buffer to a stringstream buffer
-  RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {}
-
-  /// return the string
-  std::string str() const {
-    return ssBuffer_.str();
-  }
-
-  /// destructor -- redirect stdout buffer to its original buffer
-  ~RedirectCout() {
-    std::cout.rdbuf(coutBuffer_);
-  }
-
-private:
-  std::stringstream ssBuffer_;
-  std::streambuf* coutBuffer_;
-};
-
 }
 

From e8e3094556f2c28e55b03d3438c30fa36c37b8c4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:10:09 -0700
Subject: [PATCH 50/72] update CMake

---
 python/CMakeLists.txt | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 5f51368e6a..676479bd55 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -50,8 +50,22 @@ set(ignore
     gtsam::BinaryMeasurementsUnit3
     gtsam::KeyPairDoubleMap)
 
+set(interface_headers
+    ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i
+    ${PROJECT_SOURCE_DIR}/gtsam/base/base.i
+    ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i
+    ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i
+    ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i
+    ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i
+    ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i
+    ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i
+    ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i
+)
+
+
 pybind_wrap(gtsam_py # target
-            ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
+            "${interface_headers}" # interface_headers
             "gtsam.cpp" # generated_cpp
             "gtsam" # module_name
             "gtsam" # top_namespace

From fe95b8b9709db56d67790a294270be8a0f27e001 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:10:09 -0700
Subject: [PATCH 51/72] wrapper updates

---
 wrap/cmake/PybindWrap.cmake               | 157 +++++++++++-----------
 wrap/gtwrap/pybind_wrapper.py             |  75 +++++++++--
 wrap/scripts/pybind_wrap.py               |  11 +-
 wrap/templates/pybind_wrapper.tpl.example |   8 +-
 4 files changed, 157 insertions(+), 94 deletions(-)

diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake
index 331dfff8c4..f341c2f989 100644
--- a/wrap/cmake/PybindWrap.cmake
+++ b/wrap/cmake/PybindWrap.cmake
@@ -13,15 +13,14 @@ gtwrap_get_python_version(${WRAP_PYTHON_VERSION})
 message(STATUS "Setting Python version for wrapper")
 set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION})
 
-# User-friendly Pybind11 wrapping and installing function.
-# Builds a Pybind11 module from the provided interface_header.
-# For example, for the interface header gtsam.h, this will
-# build the wrap module 'gtsam_py.cc'.
+# User-friendly Pybind11 wrapping and installing function. Builds a Pybind11
+# module from the provided interface_headers. For example, for the interface
+# header gtsam.h, this will build the wrap module 'gtsam_py.cc'.
 #
 # Arguments:
 # ~~~
 # target: The Make target
-# interface_header:  The relative path to the wrapper interface definition file.
+# interface_headers:  List of paths to the wrapper interface definition files. The top level interface file should be first.
 # generated_cpp: The name of the cpp file which is generated from the tpl file.
 # module_name: The name of the Python module to use.
 # top_namespace: The C++ namespace under which the code to be wrapped exists.
@@ -31,16 +30,17 @@ set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION})
 # libs: Libraries to link with.
 # dependencies: Dependencies which need to be built before the wrapper.
 # use_boost (optional): Flag indicating whether to include Boost.
-function(pybind_wrap
-         target
-         interface_header
-         generated_cpp
-         module_name
-         top_namespace
-         ignore_classes
-         module_template
-         libs
-         dependencies)
+function(
+  pybind_wrap
+  target
+  interface_headers
+  generated_cpp
+  module_name
+  top_namespace
+  ignore_classes
+  module_template
+  libs
+  dependencies)
   set(ExtraMacroArgs ${ARGN})
   list(GET ExtraMacroArgs 0 USE_BOOST)
   if(USE_BOOST)
@@ -49,57 +49,62 @@ function(pybind_wrap
     set(_WRAP_BOOST_ARG "")
   endif(USE_BOOST)
 
-  if (UNIX)
+  if(UNIX)
     set(GTWRAP_PATH_SEPARATOR ":")
   else()
     set(GTWRAP_PATH_SEPARATOR ";")
   endif()
 
-  add_custom_command(OUTPUT ${generated_cpp}
-          COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
-          ${PYTHON_EXECUTABLE}
-                             ${PYBIND_WRAP_SCRIPT}
-                             --src
-                             ${interface_header}
-                             --out
-                             ${generated_cpp}
-                             --module_name
-                             ${module_name}
-                             --top_module_namespaces
-                             "${top_namespace}"
-                             --ignore
-                             ${ignore_classes}
-                             --template
-                             ${module_template}
-                             ${_WRAP_BOOST_ARG}
-                     DEPENDS ${interface_header} ${module_template}
-                     VERBATIM)
-  add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${generated_cpp})
+  # Convert .i file names to .cpp file names.
+  foreach(filepath ${interface_headers})
+    get_filename_component(interface ${filepath} NAME)
+    string(REPLACE ".i" ".cpp" cpp_file ${interface})
+    list(APPEND cpp_files ${cpp_file})
+  endforeach()
+
+  add_custom_command(
+    OUTPUT ${cpp_files}
+    COMMAND
+      ${CMAKE_COMMAND} -E env
+      "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
+      ${PYTHON_EXECUTABLE} ${PYBIND_WRAP_SCRIPT} --src "${interface_headers}"
+      --out "${generated_cpp}" --module_name ${module_name}
+      --top_module_namespaces "${top_namespace}" --ignore ${ignore_classes}
+      --template ${module_template} ${_WRAP_BOOST_ARG}
+    DEPENDS "${interface_headers}" ${module_template}
+    VERBATIM)
+
+  add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${cpp_files})
 
   # Late dependency injection, to make sure this gets called whenever the
   # interface header or the wrap library are updated.
   # ~~~
   # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
   # ~~~
-  add_custom_command(OUTPUT ${generated_cpp}
-                     DEPENDS ${interface_header}
-                            #  @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py
-                            #  @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py
-                            #  @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py
-                     APPEND)
+  add_custom_command(
+    OUTPUT ${cpp_files}
+    DEPENDS ${interface_headers}
+    # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py
+    # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py
+    # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py
+    APPEND)
 
-  pybind11_add_module(${target} ${generated_cpp})
+  pybind11_add_module(${target} "${cpp_files}")
 
   if(APPLE)
-    # `type_info` objects will become "weak private external" if the templated class is initialized implicitly even if we explicitly
-    # export them with `WRAP_EXPORT`. If that happens, the `type_info` for the same templated class will diverge between shared
-    # libraries, causing `dynamic_cast` to fail. This is mitigated by telling Clang to mimic the MSVC behavior.
-    # See https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2
+    # `type_info` objects will become "weak private external" if the templated
+    # class is initialized implicitly even if we explicitly export them with
+    # `WRAP_EXPORT`. If that happens, the `type_info` for the same templated
+    # class will diverge between shared libraries, causing `dynamic_cast` to
+    # fail. This is mitigated by telling Clang to mimic the MSVC behavior. See
+    # https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2
     # https://github.com/CppMicroServices/CppMicroServices/pull/82/files
     # https://www.russellmcc.com/posts/2013-08-03-rtti.html
     target_compile_options(${target} PRIVATE "-fvisibility-ms-compat")
   endif()
+
   add_dependencies(${target} pybind_wrap_${module_name})
+
   if(NOT "${libs}" STREQUAL "")
     target_link_libraries(${target} PRIVATE "${libs}")
   endif()
@@ -121,10 +126,7 @@ endfunction()
 # dest_directory: The destination directory to install to.
 # patterns: list of file patterns to install
 # ~~~
-function(install_python_scripts
-         source_directory
-         dest_directory
-         patterns)
+function(install_python_scripts source_directory dest_directory patterns)
   set(patterns_args "")
   set(exclude_patterns "")
 
@@ -144,17 +146,19 @@ function(install_python_scripts
       # there is one
       get_filename_component(location "${dest_directory}" PATH)
       get_filename_component(name "${dest_directory}" NAME)
-      install(DIRECTORY "${source_directory}"
-              DESTINATION "${location}/${name}${build_type_tag}"
-              CONFIGURATIONS "${build_type}"
-              FILES_MATCHING ${patterns_args}
-              PATTERN "${exclude_patterns}" EXCLUDE)
+      install(
+        DIRECTORY "${source_directory}"
+        DESTINATION "${location}/${name}${build_type_tag}"
+        CONFIGURATIONS "${build_type}"
+        FILES_MATCHING ${patterns_args}
+        PATTERN "${exclude_patterns}" EXCLUDE)
     endforeach()
   else()
-    install(DIRECTORY "${source_directory}"
-            DESTINATION "${dest_directory}"
-            FILES_MATCHING ${patterns_args}
-            PATTERN "${exclude_patterns}" EXCLUDE)
+    install(
+      DIRECTORY "${source_directory}"
+      DESTINATION "${dest_directory}"
+      FILES_MATCHING ${patterns_args}
+      PATTERN "${exclude_patterns}" EXCLUDE)
   endif()
 
 endfunction()
@@ -172,13 +176,14 @@ function(install_python_files source_files dest_directory)
     foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
       string(TOUPPER "${build_type}" build_type_upper)
       set(build_type_tag "")
-      # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if
-      # there is one
+      # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if there
+      # is one
       get_filename_component(location "${dest_directory}" PATH)
       get_filename_component(name "${dest_directory}" NAME)
-      install(FILES "${source_files}"
-              DESTINATION "${location}/${name}${build_type_tag}"
-              CONFIGURATIONS "${build_type}")
+      install(
+        FILES "${source_files}"
+        DESTINATION "${location}/${name}${build_type_tag}"
+        CONFIGURATIONS "${build_type}")
     endforeach()
   else()
     install(FILES "${source_files}" DESTINATION "${dest_directory}")
@@ -194,18 +199,19 @@ function(create_symlinks source_folder dest_folder)
     return()
   endif()
 
-  file(GLOB files
-       LIST_DIRECTORIES true
-       RELATIVE "${source_folder}"
-       "${source_folder}/*")
+  file(
+    GLOB files
+    LIST_DIRECTORIES true
+    RELATIVE "${source_folder}"
+    "${source_folder}/*")
   foreach(path_file ${files})
     get_filename_component(folder ${path_file} PATH)
     get_filename_component(ext ${path_file} EXT)
     set(ignored_ext ".tpl" ".h")
-    list (FIND ignored_ext "${ext}" _index)
-    if (${_index} GREATER -1)
+    list(FIND ignored_ext "${ext}" _index)
+    if(${_index} GREATER -1)
       continue()
-    endif ()
+    endif()
     # Create REAL folder
     file(MAKE_DIRECTORY "${dest_folder}")
 
@@ -224,9 +230,10 @@ function(create_symlinks source_folder dest_folder)
     endif()
     # cmake-format: on
 
-    execute_process(COMMAND ${command}
-                    RESULT_VARIABLE result
-                    ERROR_VARIABLE output)
+    execute_process(
+      COMMAND ${command}
+      RESULT_VARIABLE result
+      ERROR_VARIABLE output)
 
     if(NOT ${result} EQUAL 0)
       message(
diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py
index 0e1b3c7eaa..40571263aa 100755
--- a/wrap/gtwrap/pybind_wrapper.py
+++ b/wrap/gtwrap/pybind_wrapper.py
@@ -13,6 +13,7 @@
 # pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, line-too-long
 
 import re
+from pathlib import Path
 
 import gtwrap.interface_parser as parser
 import gtwrap.template_instantiator as instantiator
@@ -32,7 +33,7 @@ def __init__(self,
         self.top_module_namespaces = top_module_namespaces
         self.use_boost = use_boost
         self.ignore_classes = ignore_classes
-        self._serializing_classes = list()
+        self._serializing_classes = []
         self.module_template = module_template
         self.python_keywords = [
             'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import',
@@ -160,7 +161,7 @@ def _wrap_method(self,
                 'self->print',
                 'py::scoped_ostream_redirect output; self->print')
 
-            # Make __repr__() call print() internally
+            # Make __repr__() call .print() internally
             ret += '''{prefix}.def("__repr__",
                     [](const {cpp_class}& self{opt_comma}{args_signature_with_names}){{
                         gtsam::RedirectCout redirect;
@@ -557,8 +558,15 @@ def wrap_namespace(self, namespace):
             )
         return wrapped, includes
 
-    def wrap(self, content):
-        """Wrap the code in the interface file."""
+    def wrap_file(self, content, module_name=None, submodules=None):
+        """
+        Wrap the code in the interface file.
+
+        Args:
+            content: The contents of the interface file.
+            module_name: The name of the module.
+            submodules: List of other interface file names that should be linked to.
+        """
         # Parse the contents of the interface file
         module = parser.Module.parseString(content)
         # Instantiate all templates
@@ -574,23 +582,74 @@ def wrap(self, content):
             if ',' in cpp_class:
                 new_name = re.sub("[,:<> ]", "", cpp_class)
                 boost_class_export += "typedef {cpp_class} {new_name};\n".format(  # noqa
-                    cpp_class=cpp_class,
-                    new_name=new_name,
-                )
+                    cpp_class=cpp_class, new_name=new_name)
+
             boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format(
                 new_name=new_name, )
 
+        # Reset the serializing classes list
+        self._serializing_classes = []
+
         holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \
                       "{shared_ptr_type}::shared_ptr<TYPE_PLACEHOLDER_DONOTUSE>);"
         include_boost = "#include <boost/shared_ptr.hpp>" if self.use_boost else ""
 
+        submodules_init = []
+
+        if submodules is not None:
+            module_def = "PYBIND11_MODULE({0}, m_)".format(module_name)
+
+            for idx, submodule in enumerate(submodules):
+                submodules[idx] = "void {0}(py::module_ &);".format(submodule)
+                submodules_init.append("{0}(m_);".format(submodule))
+
+        else:
+            module_def = "void {0}(py::module_ &m_)".format(module_name)
+            submodules = []
+
         return self.module_template.format(
             include_boost=include_boost,
-            module_name=self.module_name,
+            module_def=module_def,
+            module_name=module_name,
             includes=includes,
             holder_type=holder_type.format(
                 shared_ptr_type=('boost' if self.use_boost else 'std'))
             if self.use_boost else "",
             wrapped_namespace=wrapped_namespace,
             boost_class_export=boost_class_export,
+            submodules="\n".join(submodules),
+            submodules_init="\n".join(submodules_init),
         )
+
+    def wrap(self, sources, main_output):
+        """
+        Wrap all the source interface files.
+
+        Args:
+            sources: List of all interface files.
+            main_output: The name for the main module.
+        """
+        main_module = sources[0]
+        submodules = []
+        for source in sources[1:]:
+            filename = Path(source).name
+            module_name = Path(source).stem
+            # Read in the complete interface (.i) file
+            with open(source, "r") as f:
+                content = f.read()
+            submodules.append(module_name)
+            cc_content = self.wrap_file(content, module_name=module_name)
+
+            # Generate the C++ code which Pybind11 will use.
+            with open(filename.replace(".i", ".cpp"), "w") as f:
+                f.write(cc_content)
+
+        with open(main_module, "r") as f:
+            content = f.read()
+        cc_content = self.wrap_file(content,
+                                    module_name=self.module_name,
+                                    submodules=submodules)
+
+        # Generate the C++ code which Pybind11 will use.
+        with open(main_output, "w") as f:
+            f.write(cc_content)
diff --git a/wrap/scripts/pybind_wrap.py b/wrap/scripts/pybind_wrap.py
index 7f2f8d4192..c82a1d24c0 100644
--- a/wrap/scripts/pybind_wrap.py
+++ b/wrap/scripts/pybind_wrap.py
@@ -67,10 +67,6 @@ def main():
     if top_module_namespaces[0]:
         top_module_namespaces = [''] + top_module_namespaces
 
-    # Read in the complete interface (.i) file
-    with open(args.src, "r") as f:
-        content = f.read()
-
     with open(args.template, "r") as f:
         template_content = f.read()
 
@@ -83,11 +79,8 @@ def main():
     )
 
     # Wrap the code and get back the cpp/cc code.
-    cc_content = wrapper.wrap(content)
-
-    # Generate the C++ code which Pybind11 will use.
-    with open(args.out, "w") as f:
-        f.write(cc_content)
+    sources = args.src.split(';')
+    wrapper.wrap(sources, args.out)
 
 
 if __name__ == "__main__":
diff --git a/wrap/templates/pybind_wrapper.tpl.example b/wrap/templates/pybind_wrapper.tpl.example
index bf5b334900..485aa8d00c 100644
--- a/wrap/templates/pybind_wrapper.tpl.example
+++ b/wrap/templates/pybind_wrapper.tpl.example
@@ -7,7 +7,7 @@
 #include <pybind11/iostream.h>
 #include <pybind11/functional.h>
 #include "gtsam/base/serialization.h"
-#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+#include "gtsam/base/utilities.h"  // for RedirectCout.
 
 {includes}
 #include <boost/serialization/export.hpp>
@@ -22,9 +22,13 @@ using namespace std;
 
 namespace py = pybind11;
 
-PYBIND11_MODULE({module_name}, m_) {{
+{submodules}
+
+{module_def} {{
     m_.doc() = "pybind11 wrapper of {module_name}";
 
+{submodules_init}
+
 {wrapped_namespace}
 
 #include "python/specializations.h"

From 0989aed0cfa2bfbec198dfa25d7bc7abcb6d9f7f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:10:10 -0700
Subject: [PATCH 52/72] enable CI builds

---
 .github/workflows/build-python.yml | 28 +++++++++++++---------------
 1 file changed, 13 insertions(+), 15 deletions(-)

diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml
index 1f87b5119a..addde8c64c 100644
--- a/.github/workflows/build-python.yml
+++ b/.github/workflows/build-python.yml
@@ -19,22 +19,20 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          # ubuntu-18.04-gcc-5,  #TODO Enable once the Python wrapper is optimized for memory
+          ubuntu-18.04-gcc-5,
           ubuntu-18.04-gcc-9,
           ubuntu-18.04-clang-9,
           macOS-10.15-xcode-11.3.1,
-          # ubuntu-18.04-gcc-5-tbb,
+          ubuntu-18.04-gcc-5-tbb,
         ]
 
-        #TODO update wrapper to prevent OOM
-        # build_type: [Debug, Release]
-        build_type: [Release]
+        build_type: [Debug, Release]
         python_version: [3]
         include:
-          # - name: ubuntu-18.04-gcc-5
-          #   os: ubuntu-18.04
-          #   compiler: gcc
-          #   version: "5"
+          - name: ubuntu-18.04-gcc-5
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "5"
 
           - name: ubuntu-18.04-gcc-9
             os: ubuntu-18.04
@@ -46,7 +44,7 @@ jobs:
             compiler: clang
             version: "9"
 
-          #NOTE temporarily added this as it is a required check.
+          # NOTE temporarily added this as it is a required check.
           - name: ubuntu-18.04-clang-9
             os: ubuntu-18.04
             compiler: clang
@@ -59,11 +57,11 @@ jobs:
             compiler: xcode
             version: "11.3.1"
 
-          # - name: ubuntu-18.04-gcc-5-tbb
-          #   os: ubuntu-18.04
-          #   compiler: gcc
-          #   version: "5"
-          #   flag: tbb
+          - name: ubuntu-18.04-gcc-5-tbb
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "5"
+            flag: tbb
 
     steps:
       - name: Checkout

From 17842dcea7ed5ae377a5b9f57fdfb45a3a2b0291 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:10:11 -0700
Subject: [PATCH 53/72] fixes

---
 gtsam/geometry/geometry.i                | 1 +
 python/gtsam_unstable/gtsam_unstable.tpl | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index a39c227973..6217d9e80a 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -459,6 +459,7 @@ class Pose3 {
   // Lie Group
   static gtsam::Pose3 Expmap(Vector v);
   static Vector Logmap(const gtsam::Pose3& pose);
+  gtsam::Pose3 expmap(Vector v);
   Vector logmap(const gtsam::Pose3& pose);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl
index c1033ba43c..aa7ac6bdb8 100644
--- a/python/gtsam_unstable/gtsam_unstable.tpl
+++ b/python/gtsam_unstable/gtsam_unstable.tpl
@@ -16,7 +16,7 @@
 #include <pybind11/functional.h>
 #include <pybind11/iostream.h>
 #include "gtsam/base/serialization.h"
-#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+#include "gtsam/base/utilities.h"  // for RedirectCout.
 
 // These are the included headers listed in `gtsam_unstable.i`
 {includes}

From 4c410fcd0ebe1869400f6ea601e21792da74f019 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Jul 2021 08:10:35 -0700
Subject: [PATCH 54/72] Squashed 'wrap/' changes from 07330d100..d9ae5ce03

d9ae5ce03 Merge pull request #118 from borglab/feature/matlab-multi-files
9adddf7dd update the main script for matlab wrapping
0b0398d46 remove debug statements since they aren't needed for now
df064a364 support for parsing mutiple interface files for Matlab wrapping
1929e197c add test for parsing multiple interface files
bac442056 Merge pull request #117 from borglab/fix/matlab-refactor
331f4a8ce update tests to remove redundant code
5426e3af4 generate all content from within the wrap function
f78612bf9 make directory check common
b7acd7a1f fixed import and test setup
88007b153 Merge pull request #116 from borglab/feature/matlab-refactor
a074896e6 utils -> mixins
414557e00 structure
187100439 update gitignore
adbc55aea don't use class attributes in matlab wrapper
f45ba5b2d broke down some large functions into smaller ones
7756f0548 add mixin for checks and call method to wrap global functions
a318e2a67 Merge pull request #115 from borglab/feature/multiple-modules
b02b74c3d convert matlab_wrapper to a submodule
be8641e83 improved function naming in tests
02ddbfbb0 update tests and docs
dfbded2c7 small fixes
e9ec5af07 update docs
d124e2cfb wrap multiple files
7c7342f86 update cmake to take in new changes for multiple modules
54850f724 Merge pull request #114 from borglab/fix/remove-py35
71ee98321 add mypy annotations
ccaea6294 remove support for python 3.5

git-subtree-dir: wrap
git-subtree-split: d9ae5ce036c4315db3c28b12db9c73eae246f314
---
 .github/workflows/linux-ci.yml                |   2 +-
 .github/workflows/macos-ci.yml                |   2 +-
 .gitignore                                    |   2 +-
 CMakeLists.txt                                |   2 +-
 DOCS.md                                       |   8 +-
 README.md                                     |   4 +-
 cmake/PybindWrap.cmake                        | 157 ++--
 gtwrap/interface_parser/__init__.py           |   2 +-
 gtwrap/interface_parser/classes.py            |  12 +-
 gtwrap/interface_parser/declaration.py        |   2 +-
 gtwrap/interface_parser/enum.py               |   2 +-
 gtwrap/interface_parser/function.py           |  12 +-
 gtwrap/interface_parser/module.py             |   3 +-
 gtwrap/interface_parser/namespace.py          |   6 +-
 gtwrap/interface_parser/template.py           |   4 +-
 gtwrap/interface_parser/tokens.py             |   6 +-
 gtwrap/interface_parser/type.py               |   5 +-
 gtwrap/interface_parser/variable.py           |   6 +-
 gtwrap/matlab_wrapper/__init__.py             |   3 +
 gtwrap/matlab_wrapper/mixins.py               | 222 +++++
 gtwrap/matlab_wrapper/templates.py            | 166 ++++
 .../wrapper.py}                               | 843 ++++++------------
 gtwrap/pybind_wrapper.py                      |  75 +-
 gtwrap/template_instantiator.py               |  23 +-
 scripts/matlab_wrap.py                        |  40 +-
 scripts/pybind_wrap.py                        |  11 +-
 templates/pybind_wrapper.tpl.example          |   8 +-
 tests/expected/matlab/+gtsam/Class1.m         |  36 +
 tests/expected/matlab/+gtsam/Class2.m         |  36 +
 tests/expected/matlab/+gtsam/ClassA.m         |  36 +
 tests/expected/matlab/class_wrapper.cpp       |  17 +-
 tests/expected/matlab/functions_wrapper.cpp   | 103 +--
 tests/expected/matlab/geometry_wrapper.cpp    | 103 +--
 tests/expected/matlab/inheritance_wrapper.cpp | 221 ++---
 .../matlab/multiple_files_wrapper.cpp         | 229 +++++
 tests/expected/matlab/namespaces_wrapper.cpp  | 165 +---
 .../expected/matlab/special_cases_wrapper.cpp | 226 +----
 tests/fixtures/part1.i                        |  11 +
 tests/fixtures/part2.i                        |   7 +
 tests/test_matlab_wrapper.py                  | 181 ++--
 tests/test_pybind_wrapper.py                  |  58 +-
 41 files changed, 1424 insertions(+), 1633 deletions(-)
 create mode 100644 gtwrap/matlab_wrapper/__init__.py
 create mode 100644 gtwrap/matlab_wrapper/mixins.py
 create mode 100644 gtwrap/matlab_wrapper/templates.py
 rename gtwrap/{matlab_wrapper.py => matlab_wrapper/wrapper.py} (68%)
 create mode 100644 tests/expected/matlab/+gtsam/Class1.m
 create mode 100644 tests/expected/matlab/+gtsam/Class2.m
 create mode 100644 tests/expected/matlab/+gtsam/ClassA.m
 create mode 100644 tests/expected/matlab/multiple_files_wrapper.cpp
 create mode 100644 tests/fixtures/part1.i
 create mode 100644 tests/fixtures/part2.i

diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml
index 0ca9ba8f5b..34623385ed 100644
--- a/.github/workflows/linux-ci.yml
+++ b/.github/workflows/linux-ci.yml
@@ -10,7 +10,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        python-version: [3.5, 3.6, 3.7, 3.8, 3.9]
+        python-version: [3.6, 3.7, 3.8, 3.9]
 
     steps:
       - name: Checkout
diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml
index b0ccb3fbe9..3910d28d8a 100644
--- a/.github/workflows/macos-ci.yml
+++ b/.github/workflows/macos-ci.yml
@@ -10,7 +10,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        python-version: [3.5, 3.6, 3.7, 3.8, 3.9]
+        python-version: [3.6, 3.7, 3.8, 3.9]
 
     steps:
       - name: Checkout
diff --git a/.gitignore b/.gitignore
index 8e2bafa7a0..9f79deafab 100644
--- a/.gitignore
+++ b/.gitignore
@@ -8,4 +8,4 @@ __pycache__/
 # Files related to code coverage stats
 **/.coverage
 
-gtwrap/matlab_wrapper.tpl
+gtwrap/matlab_wrapper/matlab_wrapper.tpl
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9e03da0607..2a11a760d4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,7 +58,7 @@ if(NOT DEFINED GTWRAP_INCLUDE_NAME)
 endif()
 
 configure_file(${PROJECT_SOURCE_DIR}/templates/matlab_wrapper.tpl.in
-               ${PROJECT_SOURCE_DIR}/gtwrap/matlab_wrapper.tpl)
+               ${PROJECT_SOURCE_DIR}/gtwrap/matlab_wrapper/matlab_wrapper.tpl)
 
 # Install the gtwrap python package as a directory so it can be found  by CMake
 # for wrapping.
diff --git a/DOCS.md b/DOCS.md
index 8537ddd276..c8285baeff 100644
--- a/DOCS.md
+++ b/DOCS.md
@@ -192,12 +192,14 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the
 
    - **DO NOT** re-define an overriden function already declared in the external (forward-declared) base class. This will cause an ambiguity problem in the Pybind header file.
 
+- Splitting wrapper over multiple files
+    - The Pybind11 wrapper supports splitting the wrapping code over multiple files.
+    - To be able to use classes from another module, simply import the C++ header file in that wrapper file.
+    - Unfortunately, this means that aliases can no longer be used.
+    - Similarly, there can be multiple `preamble.h` and `specializations.h` files. Each of these should match the module file name.
 
 ### TODO
-- Default values for arguments.
-    - WORKAROUND: make multiple versions of the same function for different configurations of default arguments.
 - Handle `gtsam::Rot3M` conversions to quaternions.
 - Parse return of const ref arguments.
 - Parse `std::string` variants and convert directly to special string.
-- Add enum support.
 - Add generalized serialization support via `boost.serialization` with hooks to MATLAB save/load.
diff --git a/README.md b/README.md
index 442fc2f934..a04a2ef2d0 100644
--- a/README.md
+++ b/README.md
@@ -29,8 +29,10 @@ Using `wrap` in your project is straightforward from here. In your `CMakeLists.t
 ```cmake
 find_package(gtwrap)
 
+set(interface_files ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h)
+
 pybind_wrap(${PROJECT_NAME}_py # target
-            ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h # interface header file
+            "${interface_files}" # list of interface header files
             "${PROJECT_NAME}.cpp" # the generated cpp
             "${PROJECT_NAME}" # module_name
             "${PROJECT_MODULE_NAME}" # top namespace in the cpp file e.g. gtsam
diff --git a/cmake/PybindWrap.cmake b/cmake/PybindWrap.cmake
index 331dfff8c4..f341c2f989 100644
--- a/cmake/PybindWrap.cmake
+++ b/cmake/PybindWrap.cmake
@@ -13,15 +13,14 @@ gtwrap_get_python_version(${WRAP_PYTHON_VERSION})
 message(STATUS "Setting Python version for wrapper")
 set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION})
 
-# User-friendly Pybind11 wrapping and installing function.
-# Builds a Pybind11 module from the provided interface_header.
-# For example, for the interface header gtsam.h, this will
-# build the wrap module 'gtsam_py.cc'.
+# User-friendly Pybind11 wrapping and installing function. Builds a Pybind11
+# module from the provided interface_headers. For example, for the interface
+# header gtsam.h, this will build the wrap module 'gtsam_py.cc'.
 #
 # Arguments:
 # ~~~
 # target: The Make target
-# interface_header:  The relative path to the wrapper interface definition file.
+# interface_headers:  List of paths to the wrapper interface definition files. The top level interface file should be first.
 # generated_cpp: The name of the cpp file which is generated from the tpl file.
 # module_name: The name of the Python module to use.
 # top_namespace: The C++ namespace under which the code to be wrapped exists.
@@ -31,16 +30,17 @@ set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION})
 # libs: Libraries to link with.
 # dependencies: Dependencies which need to be built before the wrapper.
 # use_boost (optional): Flag indicating whether to include Boost.
-function(pybind_wrap
-         target
-         interface_header
-         generated_cpp
-         module_name
-         top_namespace
-         ignore_classes
-         module_template
-         libs
-         dependencies)
+function(
+  pybind_wrap
+  target
+  interface_headers
+  generated_cpp
+  module_name
+  top_namespace
+  ignore_classes
+  module_template
+  libs
+  dependencies)
   set(ExtraMacroArgs ${ARGN})
   list(GET ExtraMacroArgs 0 USE_BOOST)
   if(USE_BOOST)
@@ -49,57 +49,62 @@ function(pybind_wrap
     set(_WRAP_BOOST_ARG "")
   endif(USE_BOOST)
 
-  if (UNIX)
+  if(UNIX)
     set(GTWRAP_PATH_SEPARATOR ":")
   else()
     set(GTWRAP_PATH_SEPARATOR ";")
   endif()
 
-  add_custom_command(OUTPUT ${generated_cpp}
-          COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
-          ${PYTHON_EXECUTABLE}
-                             ${PYBIND_WRAP_SCRIPT}
-                             --src
-                             ${interface_header}
-                             --out
-                             ${generated_cpp}
-                             --module_name
-                             ${module_name}
-                             --top_module_namespaces
-                             "${top_namespace}"
-                             --ignore
-                             ${ignore_classes}
-                             --template
-                             ${module_template}
-                             ${_WRAP_BOOST_ARG}
-                     DEPENDS ${interface_header} ${module_template}
-                     VERBATIM)
-  add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${generated_cpp})
+  # Convert .i file names to .cpp file names.
+  foreach(filepath ${interface_headers})
+    get_filename_component(interface ${filepath} NAME)
+    string(REPLACE ".i" ".cpp" cpp_file ${interface})
+    list(APPEND cpp_files ${cpp_file})
+  endforeach()
+
+  add_custom_command(
+    OUTPUT ${cpp_files}
+    COMMAND
+      ${CMAKE_COMMAND} -E env
+      "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
+      ${PYTHON_EXECUTABLE} ${PYBIND_WRAP_SCRIPT} --src "${interface_headers}"
+      --out "${generated_cpp}" --module_name ${module_name}
+      --top_module_namespaces "${top_namespace}" --ignore ${ignore_classes}
+      --template ${module_template} ${_WRAP_BOOST_ARG}
+    DEPENDS "${interface_headers}" ${module_template}
+    VERBATIM)
+
+  add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${cpp_files})
 
   # Late dependency injection, to make sure this gets called whenever the
   # interface header or the wrap library are updated.
   # ~~~
   # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
   # ~~~
-  add_custom_command(OUTPUT ${generated_cpp}
-                     DEPENDS ${interface_header}
-                            #  @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py
-                            #  @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py
-                            #  @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py
-                     APPEND)
+  add_custom_command(
+    OUTPUT ${cpp_files}
+    DEPENDS ${interface_headers}
+    # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py
+    # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py
+    # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py
+    APPEND)
 
-  pybind11_add_module(${target} ${generated_cpp})
+  pybind11_add_module(${target} "${cpp_files}")
 
   if(APPLE)
-    # `type_info` objects will become "weak private external" if the templated class is initialized implicitly even if we explicitly
-    # export them with `WRAP_EXPORT`. If that happens, the `type_info` for the same templated class will diverge between shared
-    # libraries, causing `dynamic_cast` to fail. This is mitigated by telling Clang to mimic the MSVC behavior.
-    # See https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2
+    # `type_info` objects will become "weak private external" if the templated
+    # class is initialized implicitly even if we explicitly export them with
+    # `WRAP_EXPORT`. If that happens, the `type_info` for the same templated
+    # class will diverge between shared libraries, causing `dynamic_cast` to
+    # fail. This is mitigated by telling Clang to mimic the MSVC behavior. See
+    # https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2
     # https://github.com/CppMicroServices/CppMicroServices/pull/82/files
     # https://www.russellmcc.com/posts/2013-08-03-rtti.html
     target_compile_options(${target} PRIVATE "-fvisibility-ms-compat")
   endif()
+
   add_dependencies(${target} pybind_wrap_${module_name})
+
   if(NOT "${libs}" STREQUAL "")
     target_link_libraries(${target} PRIVATE "${libs}")
   endif()
@@ -121,10 +126,7 @@ endfunction()
 # dest_directory: The destination directory to install to.
 # patterns: list of file patterns to install
 # ~~~
-function(install_python_scripts
-         source_directory
-         dest_directory
-         patterns)
+function(install_python_scripts source_directory dest_directory patterns)
   set(patterns_args "")
   set(exclude_patterns "")
 
@@ -144,17 +146,19 @@ function(install_python_scripts
       # there is one
       get_filename_component(location "${dest_directory}" PATH)
       get_filename_component(name "${dest_directory}" NAME)
-      install(DIRECTORY "${source_directory}"
-              DESTINATION "${location}/${name}${build_type_tag}"
-              CONFIGURATIONS "${build_type}"
-              FILES_MATCHING ${patterns_args}
-              PATTERN "${exclude_patterns}" EXCLUDE)
+      install(
+        DIRECTORY "${source_directory}"
+        DESTINATION "${location}/${name}${build_type_tag}"
+        CONFIGURATIONS "${build_type}"
+        FILES_MATCHING ${patterns_args}
+        PATTERN "${exclude_patterns}" EXCLUDE)
     endforeach()
   else()
-    install(DIRECTORY "${source_directory}"
-            DESTINATION "${dest_directory}"
-            FILES_MATCHING ${patterns_args}
-            PATTERN "${exclude_patterns}" EXCLUDE)
+    install(
+      DIRECTORY "${source_directory}"
+      DESTINATION "${dest_directory}"
+      FILES_MATCHING ${patterns_args}
+      PATTERN "${exclude_patterns}" EXCLUDE)
   endif()
 
 endfunction()
@@ -172,13 +176,14 @@ function(install_python_files source_files dest_directory)
     foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
       string(TOUPPER "${build_type}" build_type_upper)
       set(build_type_tag "")
-      # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if
-      # there is one
+      # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if there
+      # is one
       get_filename_component(location "${dest_directory}" PATH)
       get_filename_component(name "${dest_directory}" NAME)
-      install(FILES "${source_files}"
-              DESTINATION "${location}/${name}${build_type_tag}"
-              CONFIGURATIONS "${build_type}")
+      install(
+        FILES "${source_files}"
+        DESTINATION "${location}/${name}${build_type_tag}"
+        CONFIGURATIONS "${build_type}")
     endforeach()
   else()
     install(FILES "${source_files}" DESTINATION "${dest_directory}")
@@ -194,18 +199,19 @@ function(create_symlinks source_folder dest_folder)
     return()
   endif()
 
-  file(GLOB files
-       LIST_DIRECTORIES true
-       RELATIVE "${source_folder}"
-       "${source_folder}/*")
+  file(
+    GLOB files
+    LIST_DIRECTORIES true
+    RELATIVE "${source_folder}"
+    "${source_folder}/*")
   foreach(path_file ${files})
     get_filename_component(folder ${path_file} PATH)
     get_filename_component(ext ${path_file} EXT)
     set(ignored_ext ".tpl" ".h")
-    list (FIND ignored_ext "${ext}" _index)
-    if (${_index} GREATER -1)
+    list(FIND ignored_ext "${ext}" _index)
+    if(${_index} GREATER -1)
       continue()
-    endif ()
+    endif()
     # Create REAL folder
     file(MAKE_DIRECTORY "${dest_folder}")
 
@@ -224,9 +230,10 @@ function(create_symlinks source_folder dest_folder)
     endif()
     # cmake-format: on
 
-    execute_process(COMMAND ${command}
-                    RESULT_VARIABLE result
-                    ERROR_VARIABLE output)
+    execute_process(
+      COMMAND ${command}
+      RESULT_VARIABLE result
+      ERROR_VARIABLE output)
 
     if(NOT ${result} EQUAL 0)
       message(
diff --git a/gtwrap/interface_parser/__init__.py b/gtwrap/interface_parser/__init__.py
index 0f87eaaa9d..3be52d7d9f 100644
--- a/gtwrap/interface_parser/__init__.py
+++ b/gtwrap/interface_parser/__init__.py
@@ -12,7 +12,7 @@
 
 import sys
 
-import pyparsing
+import pyparsing  # type: ignore
 
 from .classes import *
 from .declaration import *
diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py
index ea7a3b3c38..3e6a0fc3c7 100644
--- a/gtwrap/interface_parser/classes.py
+++ b/gtwrap/interface_parser/classes.py
@@ -12,7 +12,7 @@
 
 from typing import Iterable, List, Union
 
-from pyparsing import Literal, Optional, ZeroOrMore
+from pyparsing import Literal, Optional, ZeroOrMore  # type: ignore
 
 from .enum import Enum
 from .function import ArgumentList, ReturnType
@@ -233,7 +233,7 @@ def __init__(self,
             self.static_methods = []
             self.properties = []
             self.operators = []
-            self.enums = []
+            self.enums: List[Enum] = []
             for m in members:
                 if isinstance(m, Constructor):
                     self.ctors.append(m)
@@ -274,7 +274,7 @@ def __init__(self,
 
     def __init__(
         self,
-        template: Template,
+        template: Union[Template, None],
         is_virtual: str,
         name: str,
         parent_class: list,
@@ -292,16 +292,16 @@ def __init__(
         if parent_class:
             # If it is in an iterable, extract the parent class.
             if isinstance(parent_class, Iterable):
-                parent_class = parent_class[0]
+                parent_class = parent_class[0]  # type: ignore
 
             # If the base class is a TemplatedType,
             # we want the instantiated Typename
             if isinstance(parent_class, TemplatedType):
-                parent_class = parent_class.typename
+                parent_class = parent_class.typename  # type: ignore
 
             self.parent_class = parent_class
         else:
-            self.parent_class = ''
+            self.parent_class = ''  # type: ignore
 
         self.ctors = ctors
         self.methods = methods
diff --git a/gtwrap/interface_parser/declaration.py b/gtwrap/interface_parser/declaration.py
index 292d6aeaa6..f47ee6e057 100644
--- a/gtwrap/interface_parser/declaration.py
+++ b/gtwrap/interface_parser/declaration.py
@@ -10,7 +10,7 @@
 Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
 """
 
-from pyparsing import CharsNotIn, Optional
+from pyparsing import CharsNotIn, Optional  # type: ignore
 
 from .tokens import (CLASS, COLON, INCLUDE, LOPBRACK, ROPBRACK, SEMI_COLON,
                      VIRTUAL)
diff --git a/gtwrap/interface_parser/enum.py b/gtwrap/interface_parser/enum.py
index fca7080ef2..265e1ad612 100644
--- a/gtwrap/interface_parser/enum.py
+++ b/gtwrap/interface_parser/enum.py
@@ -10,7 +10,7 @@
 Author: Varun Agrawal
 """
 
-from pyparsing import delimitedList
+from pyparsing import delimitedList  # type: ignore
 
 from .tokens import ENUM, IDENT, LBRACE, RBRACE, SEMI_COLON
 from .type import Typename
diff --git a/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py
index 3b9a5d4ada..995aba10e1 100644
--- a/gtwrap/interface_parser/function.py
+++ b/gtwrap/interface_parser/function.py
@@ -10,9 +10,9 @@
 Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
 """
 
-from typing import Iterable, List, Union
+from typing import Any, Iterable, List, Union
 
-from pyparsing import Optional, ParseResults, delimitedList
+from pyparsing import Optional, ParseResults, delimitedList  # type: ignore
 
 from .template import Template
 from .tokens import (COMMA, DEFAULT_ARG, EQUAL, IDENT, LOPBRACK, LPAREN, PAIR,
@@ -42,12 +42,12 @@ def __init__(self,
                  name: str,
                  default: ParseResults = None):
         if isinstance(ctype, Iterable):
-            self.ctype = ctype[0]
+            self.ctype = ctype[0]  # type: ignore
         else:
             self.ctype = ctype
         self.name = name
         self.default = default
-        self.parent = None  # type: Union[ArgumentList, None]
+        self.parent: Union[ArgumentList, None] = None
 
     def __repr__(self) -> str:
         return self.to_cpp()
@@ -70,7 +70,7 @@ def __init__(self, args_list: List[Argument]):
             arg.parent = self
         # The parent object which contains the argument list
         # E.g. Method, StaticMethod, Template, Constructor, GlobalFunction
-        self.parent = None
+        self.parent: Any = None
 
     @staticmethod
     def from_parse_result(parse_result: ParseResults):
@@ -123,7 +123,7 @@ def __init__(self, type1: Union[Type, TemplatedType], type2: Type):
         self.type2 = type2
         # The parent object which contains the return type
         # E.g. Method, StaticMethod, Template, Constructor, GlobalFunction
-        self.parent = None
+        self.parent: Any = None
 
     def is_void(self) -> bool:
         """
diff --git a/gtwrap/interface_parser/module.py b/gtwrap/interface_parser/module.py
index 6412098b8a..7912c40d5b 100644
--- a/gtwrap/interface_parser/module.py
+++ b/gtwrap/interface_parser/module.py
@@ -12,7 +12,8 @@
 
 # pylint: disable=unnecessary-lambda, unused-import, expression-not-assigned, no-else-return, protected-access, too-few-public-methods, too-many-arguments
 
-from pyparsing import ParseResults, ZeroOrMore, cppStyleComment, stringEnd
+from pyparsing import (ParseResults, ZeroOrMore,  # type: ignore
+                       cppStyleComment, stringEnd)
 
 from .classes import Class
 from .declaration import ForwardDeclaration, Include
diff --git a/gtwrap/interface_parser/namespace.py b/gtwrap/interface_parser/namespace.py
index 575d982371..9c135ffe8c 100644
--- a/gtwrap/interface_parser/namespace.py
+++ b/gtwrap/interface_parser/namespace.py
@@ -14,7 +14,7 @@
 
 from typing import List, Union
 
-from pyparsing import Forward, ParseResults, ZeroOrMore
+from pyparsing import Forward, ParseResults, ZeroOrMore  # type: ignore
 
 from .classes import Class, collect_namespaces
 from .declaration import ForwardDeclaration, Include
@@ -93,7 +93,7 @@ def from_parse_result(t: ParseResults):
         return Namespace(t.name, content)
 
     def find_class_or_function(
-            self, typename: Typename) -> Union[Class, GlobalFunction]:
+            self, typename: Typename) -> Union[Class, GlobalFunction, ForwardDeclaration]:
         """
         Find the Class or GlobalFunction object given its typename.
         We have to traverse the tree of namespaces.
@@ -115,7 +115,7 @@ def find_class_or_function(
             return res[0]
 
     def top_level(self) -> "Namespace":
-        """Return the top leve namespace."""
+        """Return the top level namespace."""
         if self.name == '' or self.parent == '':
             return self
         else:
diff --git a/gtwrap/interface_parser/template.py b/gtwrap/interface_parser/template.py
index dc9d0ce44f..fd9de830ae 100644
--- a/gtwrap/interface_parser/template.py
+++ b/gtwrap/interface_parser/template.py
@@ -12,11 +12,11 @@
 
 from typing import List
 
-from pyparsing import Optional, ParseResults, delimitedList
+from pyparsing import Optional, ParseResults, delimitedList  # type: ignore
 
 from .tokens import (EQUAL, IDENT, LBRACE, LOPBRACK, RBRACE, ROPBRACK,
                      SEMI_COLON, TEMPLATE, TYPEDEF)
-from .type import Typename, TemplatedType
+from .type import TemplatedType, Typename
 
 
 class Template:
diff --git a/gtwrap/interface_parser/tokens.py b/gtwrap/interface_parser/tokens.py
index 4eba95900a..0f8d38d868 100644
--- a/gtwrap/interface_parser/tokens.py
+++ b/gtwrap/interface_parser/tokens.py
@@ -10,9 +10,9 @@
 Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
 """
 
-from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress,
-                       Word, alphanums, alphas, nestedExpr, nums,
-                       originalTextFor, printables)
+from pyparsing import (Keyword, Literal, OneOrMore, Or,  # type: ignore
+                       QuotedString, Suppress, Word, alphanums, alphas,
+                       nestedExpr, nums, originalTextFor, printables)
 
 # rule for identifiers (e.g. variable names)
 IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums)
diff --git a/gtwrap/interface_parser/type.py b/gtwrap/interface_parser/type.py
index b9f2bd8f74..0b9be65017 100644
--- a/gtwrap/interface_parser/type.py
+++ b/gtwrap/interface_parser/type.py
@@ -14,7 +14,8 @@
 
 from typing import Iterable, List, Union
 
-from pyparsing import Forward, Optional, Or, ParseResults, delimitedList
+from pyparsing import (Forward, Optional, Or, ParseResults,  # type: ignore
+                       delimitedList)
 
 from .tokens import (BASIS_TYPES, CONST, IDENT, LOPBRACK, RAW_POINTER, REF,
                      ROPBRACK, SHARED_POINTER)
@@ -48,7 +49,7 @@ class Typename:
 
     def __init__(self,
                  t: ParseResults,
-                 instantiations: Union[tuple, list, str, ParseResults] = ()):
+                 instantiations: Iterable[ParseResults] = ()):
         self.name = t[-1]  # the name is the last element in this list
         self.namespaces = t[:-1]
 
diff --git a/gtwrap/interface_parser/variable.py b/gtwrap/interface_parser/variable.py
index fcb02666f7..3779cf74fa 100644
--- a/gtwrap/interface_parser/variable.py
+++ b/gtwrap/interface_parser/variable.py
@@ -10,7 +10,9 @@
 Author: Varun Agrawal, Gerry Chen
 """
 
-from pyparsing import Optional, ParseResults
+from typing import List
+
+from pyparsing import Optional, ParseResults  # type: ignore
 
 from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON
 from .type import TemplatedType, Type
@@ -40,7 +42,7 @@ class Hello {
                 t.default[0] if isinstance(t.default, ParseResults) else None))
 
     def __init__(self,
-                 ctype: Type,
+                 ctype: List[Type],
                  name: str,
                  default: ParseResults = None,
                  parent=''):
diff --git a/gtwrap/matlab_wrapper/__init__.py b/gtwrap/matlab_wrapper/__init__.py
new file mode 100644
index 0000000000..f10338c1c7
--- /dev/null
+++ b/gtwrap/matlab_wrapper/__init__.py
@@ -0,0 +1,3 @@
+"""Package to wrap C++ code to Matlab via MEX."""
+
+from .wrapper import MatlabWrapper
diff --git a/gtwrap/matlab_wrapper/mixins.py b/gtwrap/matlab_wrapper/mixins.py
new file mode 100644
index 0000000000..061cea2833
--- /dev/null
+++ b/gtwrap/matlab_wrapper/mixins.py
@@ -0,0 +1,222 @@
+"""Mixins for reducing the amount of boilerplate in the main wrapper class."""
+
+import gtwrap.interface_parser as parser
+import gtwrap.template_instantiator as instantiator
+
+
+class CheckMixin:
+    """Mixin to provide various checks."""
+    # Data types that are primitive types
+    not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
+    # Ignore the namespace for these datatypes
+    ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3']
+    # Methods that should be ignored
+    ignore_methods = ['pickle']
+    # Methods that should not be wrapped directly
+    whitelist = ['serializable', 'serialize']
+    # Datatypes that do not need to be checked in methods
+    not_check_type: list = []
+
+    def _has_serialization(self, cls):
+        for m in cls.methods:
+            if m.name in self.whitelist:
+                return True
+        return False
+
+    def is_shared_ptr(self, arg_type):
+        """
+        Determine if the `interface_parser.Type` should be treated as a
+        shared pointer in the wrapper.
+        """
+        return arg_type.is_shared_ptr or (
+            arg_type.typename.name not in self.not_ptr_type
+            and arg_type.typename.name not in self.ignore_namespace
+            and arg_type.typename.name != 'string')
+
+    def is_ptr(self, arg_type):
+        """
+        Determine if the `interface_parser.Type` should be treated as a
+        raw pointer in the wrapper.
+        """
+        return arg_type.is_ptr or (
+            arg_type.typename.name not in self.not_ptr_type
+            and arg_type.typename.name not in self.ignore_namespace
+            and arg_type.typename.name != 'string')
+
+    def is_ref(self, arg_type):
+        """
+        Determine if the `interface_parser.Type` should be treated as a
+        reference in the wrapper.
+        """
+        return arg_type.typename.name not in self.ignore_namespace and \
+               arg_type.typename.name not in self.not_ptr_type and \
+               arg_type.is_ref
+
+
+class FormatMixin:
+    """Mixin to provide formatting utilities."""
+    def _clean_class_name(self, instantiated_class):
+        """Reformatted the C++ class name to fit Matlab defined naming
+        standards
+        """
+        if len(instantiated_class.ctors) != 0:
+            return instantiated_class.ctors[0].name
+
+        return instantiated_class.name
+
+    def _format_type_name(self,
+                          type_name,
+                          separator='::',
+                          include_namespace=True,
+                          constructor=False,
+                          method=False):
+        """
+        Args:
+            type_name: an interface_parser.Typename to reformat
+            separator: the statement to add between namespaces and typename
+            include_namespace: whether to include namespaces when reformatting
+            constructor: if the typename will be in a constructor
+            method: if the typename will be in a method
+
+        Raises:
+            constructor and method cannot both be true
+        """
+        if constructor and method:
+            raise ValueError(
+                'Constructor and method parameters cannot both be True')
+
+        formatted_type_name = ''
+        name = type_name.name
+
+        if include_namespace:
+            for namespace in type_name.namespaces:
+                if name not in self.ignore_namespace and namespace != '':
+                    formatted_type_name += namespace + separator
+
+        if constructor:
+            formatted_type_name += self.data_type.get(name) or name
+        elif method:
+            formatted_type_name += self.data_type_param.get(name) or name
+        else:
+            formatted_type_name += name
+
+        if separator == "::":  # C++
+            templates = []
+            for idx in range(len(type_name.instantiations)):
+                template = '{}'.format(
+                    self._format_type_name(type_name.instantiations[idx],
+                                           include_namespace=include_namespace,
+                                           constructor=constructor,
+                                           method=method))
+                templates.append(template)
+
+            if len(templates) > 0:  # If there are no templates
+                formatted_type_name += '<{}>'.format(','.join(templates))
+
+        else:
+            for idx in range(len(type_name.instantiations)):
+                formatted_type_name += '{}'.format(
+                    self._format_type_name(type_name.instantiations[idx],
+                                           separator=separator,
+                                           include_namespace=False,
+                                           constructor=constructor,
+                                           method=method))
+
+        return formatted_type_name
+
+    def _format_return_type(self,
+                            return_type,
+                            include_namespace=False,
+                            separator="::"):
+        """Format return_type.
+
+        Args:
+            return_type: an interface_parser.ReturnType to reformat
+            include_namespace: whether to include namespaces when reformatting
+        """
+        return_wrap = ''
+
+        if self._return_count(return_type) == 1:
+            return_wrap = self._format_type_name(
+                return_type.type1.typename,
+                separator=separator,
+                include_namespace=include_namespace)
+        else:
+            return_wrap = 'pair< {type1}, {type2} >'.format(
+                type1=self._format_type_name(
+                    return_type.type1.typename,
+                    separator=separator,
+                    include_namespace=include_namespace),
+                type2=self._format_type_name(
+                    return_type.type2.typename,
+                    separator=separator,
+                    include_namespace=include_namespace))
+
+        return return_wrap
+
+    def _format_class_name(self, instantiated_class, separator=''):
+        """Format a template_instantiator.InstantiatedClass name."""
+        if instantiated_class.parent == '':
+            parent_full_ns = ['']
+        else:
+            parent_full_ns = instantiated_class.parent.full_namespaces()
+        # class_name = instantiated_class.parent.name
+        #
+        # if class_name != '':
+        #     class_name += separator
+        #
+        # class_name += instantiated_class.name
+        parentname = "".join([separator + x
+                              for x in parent_full_ns]) + separator
+
+        class_name = parentname[2 * len(separator):]
+
+        class_name += instantiated_class.name
+
+        return class_name
+
+    def _format_static_method(self, static_method, separator=''):
+        """Example:
+
+                gtsamPoint3.staticFunction
+        """
+        method = ''
+
+        if isinstance(static_method, parser.StaticMethod):
+            method += "".join([separator + x for x in static_method.parent.namespaces()]) + \
+                      separator + static_method.parent.name + separator
+
+        return method[2 * len(separator):]
+
+    def _format_instance_method(self, instance_method, separator=''):
+        """Example:
+
+                gtsamPoint3.staticFunction
+        """
+        method = ''
+
+        if isinstance(instance_method, instantiator.InstantiatedMethod):
+            method_list = [
+                separator + x
+                for x in instance_method.parent.parent.full_namespaces()
+            ]
+            method += "".join(method_list) + separator
+
+            method += instance_method.parent.name + separator
+            method += instance_method.original.name
+            method += "<" + instance_method.instantiations.to_cpp() + ">"
+
+        return method[2 * len(separator):]
+
+    def _format_global_method(self, static_method, separator=''):
+        """Example:
+
+                gtsamPoint3.staticFunction
+        """
+        method = ''
+
+        if isinstance(static_method, parser.GlobalFunction):
+            method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \
+                      separator
+
+        return method[2 * len(separator):]
diff --git a/gtwrap/matlab_wrapper/templates.py b/gtwrap/matlab_wrapper/templates.py
new file mode 100644
index 0000000000..7aaf8f487b
--- /dev/null
+++ b/gtwrap/matlab_wrapper/templates.py
@@ -0,0 +1,166 @@
+import textwrap
+
+
+class WrapperTemplate:
+    """Class to encapsulate string templates for use in wrapper generation"""
+    boost_headers = textwrap.dedent("""
+            #include <boost/archive/text_iarchive.hpp>
+            #include <boost/archive/text_oarchive.hpp>
+            #include <boost/serialization/export.hpp>
+        """)
+
+    typdef_collectors = textwrap.dedent('''\
+                typedef std::set<boost::shared_ptr<{class_name_sep}>*> Collector_{class_name};
+                static Collector_{class_name} collector_{class_name};
+            ''')
+
+    delete_obj = textwrap.indent(textwrap.dedent('''\
+                {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin();
+                    iter != collector_{class_name}.end(); ) {{
+                  delete *iter;
+                  collector_{class_name}.erase(iter++);
+                  anyDeleted = true;
+                }} }}
+            '''),
+                                 prefix='  ')
+
+    delete_all_objects = textwrap.dedent('''
+            void _deleteAllObjects()
+            {{
+              mstream mout;
+              std::streambuf *outbuf = std::cout.rdbuf(&mout);\n
+              bool anyDeleted = false;
+            {delete_objs}
+              if(anyDeleted)
+                cout <<
+                  "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\\n"
+                  "calling destructors, call \'clear all\' again if you plan to now recompile a wrap\\n"
+                  "module, so that your recompiled module is used instead of the old one." << endl;
+              std::cout.rdbuf(outbuf);
+            }}
+        ''')
+
+    rtti_register = textwrap.dedent('''\
+            void _{module_name}_RTTIRegister() {{
+              const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created");
+              if(!alreadyCreated) {{
+                std::map<std::string, std::string> types;
+
+            {rtti_classes}
+
+                mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
+                if(!registry)
+                  registry = mxCreateStructMatrix(1, 1, 0, NULL);
+                typedef std::pair<std::string, std::string> StringPair;
+                for(const StringPair& rtti_matlab: types) {{
+                  int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
+                  if(fieldId < 0) {{
+                    mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+                  }}
+                  mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
+                  mxSetFieldByNumber(registry, 0, fieldId, matlabName);
+                }}
+                if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {{
+                  mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+                }}
+                mxDestroyArray(registry);
+
+                mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
+                if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {{
+                  mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+                }}
+                mxDestroyArray(newAlreadyCreated);
+              }}
+            }}
+        ''')
+
+    collector_function_upcast_from_void = textwrap.dedent('''\
+            void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{
+              mexAtExit(&_deleteAllObjects);
+              typedef boost::shared_ptr<{cpp_name}> Shared;
+              boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
+              out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+              Shared *self = new Shared(boost::static_pointer_cast<{cpp_name}>(*asVoid));
+              *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
+            }}\n
+        ''')
+
+    class_serialize_method = textwrap.dedent('''\
+            function varargout = string_serialize(this, varargin)
+              % STRING_SERIALIZE usage: string_serialize() : returns string
+              % Doxygen can be found at https://gtsam.org/doxygen/
+              if length(varargin) == 0
+                varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}});
+              else
+                error('Arguments do not match any overload of function {class_name}.string_serialize');
+              end
+            end\n
+            function sobj = saveobj(obj)
+              % SAVEOBJ Saves the object to a matlab-readable format
+              sobj = obj.string_serialize();
+            end
+        ''')
+
+    collector_function_serialize = textwrap.indent(textwrap.dedent("""\
+            typedef boost::shared_ptr<{full_name}> Shared;
+            checkArguments("string_serialize",nargout,nargin-1,0);
+            Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}");
+            ostringstream out_archive_stream;
+            boost::archive::text_oarchive out_archive(out_archive_stream);
+            out_archive << *obj;
+            out[0] = wrap< string >(out_archive_stream.str());
+        """),
+                                                   prefix='  ')
+
+    collector_function_deserialize = textwrap.indent(textwrap.dedent("""\
+            typedef boost::shared_ptr<{full_name}> Shared;
+            checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1);
+            string serialized = unwrap< string >(in[0]);
+            istringstream in_archive_stream(serialized);
+            boost::archive::text_iarchive in_archive(in_archive_stream);
+            Shared output(new {full_name}());
+            in_archive >> *output;
+            out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false);
+        """),
+                                                     prefix='  ')
+
+    mex_function = textwrap.dedent('''
+            void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+            {{
+              mstream mout;
+              std::streambuf *outbuf = std::cout.rdbuf(&mout);\n
+              _{module_name}_RTTIRegister();\n
+              int id = unwrap<int>(in[0]);\n
+              try {{
+                switch(id) {{
+            {cases}    }}
+              }} catch(const std::exception& e) {{
+                mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str());
+              }}\n
+              std::cout.rdbuf(outbuf);
+            }}
+        ''')
+
+    collector_function_shared_return = textwrap.indent(textwrap.dedent('''\
+            {{
+            boost::shared_ptr<{name}> shared({shared_obj});
+            out[{id}] = wrap_shared_ptr(shared,"{name}");
+            }}{new_line}'''),
+                                                       prefix='  ')
+
+    matlab_deserialize = textwrap.indent(textwrap.dedent("""\
+                function varargout = string_deserialize(varargin)
+                  % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name}
+                  % Doxygen can be found at https://gtsam.org/doxygen/
+                  if length(varargin) == 1
+                    varargout{{1}} = {wrapper}({id}, varargin{{:}});
+                  else
+                    error('Arguments do not match any overload of function {class_name}.string_deserialize');
+                  end
+                end\n
+                function obj = loadobj(sobj)
+                  % LOADOBJ Saves the object to a matlab-readable format
+                  obj = {class_name}.string_deserialize(sobj);
+                end
+            """),
+                                         prefix='  ')
diff --git a/gtwrap/matlab_wrapper.py b/gtwrap/matlab_wrapper/wrapper.py
similarity index 68%
rename from gtwrap/matlab_wrapper.py
rename to gtwrap/matlab_wrapper/wrapper.py
index de6221bbcf..b040d27311 100755
--- a/gtwrap/matlab_wrapper.py
+++ b/gtwrap/matlab_wrapper/wrapper.py
@@ -7,16 +7,19 @@
 
 import os
 import os.path as osp
-import sys
 import textwrap
 from functools import partial, reduce
 from typing import Dict, Iterable, List, Union
 
+from loguru import logger
+
 import gtwrap.interface_parser as parser
 import gtwrap.template_instantiator as instantiator
+from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin
+from gtwrap.matlab_wrapper.templates import WrapperTemplate
 
 
-class MatlabWrapper(object):
+class MatlabWrapper(CheckMixin, FormatMixin):
     """ Wrap the given C++ code into Matlab.
 
     Attributes
@@ -25,89 +28,75 @@ class MatlabWrapper(object):
         top_module_namespace: C++ namespace for the top module (default '')
         ignore_classes: A list of classes to ignore (default [])
     """
-    # Map the data type to its Matlab class.
-    # Found in Argument.cpp in old wrapper
-    data_type = {
-        'string': 'char',
-        'char': 'char',
-        'unsigned char': 'unsigned char',
-        'Vector': 'double',
-        'Matrix': 'double',
-        'int': 'numeric',
-        'size_t': 'numeric',
-        'bool': 'logical'
-    }
-    # Map the data type into the type used in Matlab methods.
-    # Found in matlab.h in old wrapper
-    data_type_param = {
-        'string': 'char',
-        'char': 'char',
-        'unsigned char': 'unsigned char',
-        'size_t': 'int',
-        'int': 'int',
-        'double': 'double',
-        'Point2': 'double',
-        'Point3': 'double',
-        'Vector': 'double',
-        'Matrix': 'double',
-        'bool': 'bool'
-    }
-    # Methods that should not be wrapped directly
-    whitelist = ['serializable', 'serialize']
-    # Methods that should be ignored
-    ignore_methods = ['pickle']
-    # Datatypes that do not need to be checked in methods
-    not_check_type = []  # type: list
-    # Data types that are primitive types
-    not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
-    # Ignore the namespace for these datatypes
-    ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3']
-    # The amount of times the wrapper has created a call to geometry_wrapper
-    wrapper_id = 0
-    # Map each wrapper id to what its collector function namespace, class, type, and string format
-    wrapper_map = {}
-    # Set of all the includes in the namespace
-    includes = {}  # type: Dict[parser.Include, int]
-    # Set of all classes in the namespace
-    classes = [
-    ]  # type: List[Union[parser.Class, instantiator.InstantiatedClass]]
-    classes_elems = {
-    }  # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int]
-    # Id for ordering global functions in the wrapper
-    global_function_id = 0
-    # Files and their content
-    content = []  # type: List[str]
-
-    # Ensure the template file is always picked up from the correct directory.
-    dir_path = osp.dirname(osp.realpath(__file__))
-    with open(osp.join(dir_path, "matlab_wrapper.tpl")) as f:
-        wrapper_file_header = f.read()
-
     def __init__(self,
                  module_name,
                  top_module_namespace='',
                  ignore_classes=()):
+        super().__init__()
+
         self.module_name = module_name
         self.top_module_namespace = top_module_namespace
         self.ignore_classes = ignore_classes
         self.verbose = False
 
-    def _debug(self, message):
-        if not self.verbose:
-            return
-        print(message, file=sys.stderr)
-
-    def _add_include(self, include):
-        self.includes[include] = 0
-
-    def _add_class(self, instantiated_class):
+        # Map the data type to its Matlab class.
+        # Found in Argument.cpp in old wrapper
+        self.data_type = {
+            'string': 'char',
+            'char': 'char',
+            'unsigned char': 'unsigned char',
+            'Vector': 'double',
+            'Matrix': 'double',
+            'int': 'numeric',
+            'size_t': 'numeric',
+            'bool': 'logical'
+        }
+        # Map the data type into the type used in Matlab methods.
+        # Found in matlab.h in old wrapper
+        self.data_type_param = {
+            'string': 'char',
+            'char': 'char',
+            'unsigned char': 'unsigned char',
+            'size_t': 'int',
+            'int': 'int',
+            'double': 'double',
+            'Point2': 'double',
+            'Point3': 'double',
+            'Vector': 'double',
+            'Matrix': 'double',
+            'bool': 'bool'
+        }
+        # The amount of times the wrapper has created a call to geometry_wrapper
+        self.wrapper_id = 0
+        # Map each wrapper id to its collector function namespace, class, type, and string format
+        self.wrapper_map: Dict = {}
+        # Set of all the includes in the namespace
+        self.includes: List[parser.Include] = []
+        # Set of all classes in the namespace
+        self.classes: List[Union[parser.Class,
+                                 instantiator.InstantiatedClass]] = []
+        self.classes_elems: Dict[Union[parser.Class,
+                                       instantiator.InstantiatedClass],
+                                 int] = {}
+        # Id for ordering global functions in the wrapper
+        self.global_function_id = 0
+        # Files and their content
+        self.content: List[str] = []
+
+        # Ensure the template file is always picked up from the correct directory.
+        dir_path = osp.dirname(osp.realpath(__file__))
+        with open(osp.join(dir_path, "matlab_wrapper.tpl")) as f:
+            self.wrapper_file_headers = f.read()
+
+    def add_class(self, instantiated_class):
+        """Add `instantiated_class` to the list of classes."""
         if self.classes_elems.get(instantiated_class) is None:
             self.classes_elems[instantiated_class] = 0
             self.classes.append(instantiated_class)
 
     def _update_wrapper_id(self, collector_function=None, id_diff=0):
-        """Get and define wrapper ids.
-
+        """
+        Get and define wrapper ids.
         Generates the map of id -> collector function.
 
         Args:
@@ -150,34 +139,6 @@ def _insert_spaces(self, x, y):
         """
         return x + '\n' + ('' if y == '' else '  ') + y
 
-    def _is_shared_ptr(self, arg_type):
-        """
-        Determine if the `interface_parser.Type` should be treated as a
-        shared pointer in the wrapper.
-        """
-        return arg_type.is_shared_ptr or (
-            arg_type.typename.name not in self.not_ptr_type
-            and arg_type.typename.name not in self.ignore_namespace
-            and arg_type.typename.name != 'string')
-
-    def _is_ptr(self, arg_type):
-        """
-        Determine if the `interface_parser.Type` should be treated as a
-        raw pointer in the wrapper.
-        """
-        return arg_type.is_ptr or (
-            arg_type.typename.name not in self.not_ptr_type
-            and arg_type.typename.name not in self.ignore_namespace
-            and arg_type.typename.name != 'string')
-
-    def _is_ref(self, arg_type):
-        """Determine if the interface_parser.Type should be treated as a
-        reference in the wrapper.
-        """
-        return arg_type.typename.name not in self.ignore_namespace and \
-               arg_type.typename.name not in self.not_ptr_type and \
-               arg_type.is_ref
-
     def _group_methods(self, methods):
         """Group overloaded methods together"""
         method_map = {}
@@ -190,181 +151,10 @@ def _group_methods(self, methods):
                 method_map[method.name] = len(method_out)
                 method_out.append([method])
             else:
-                self._debug("[_group_methods] Merging {} with {}".format(
-                    method_index, method.name))
                 method_out[method_index].append(method)
 
         return method_out
 
-    def _clean_class_name(self, instantiated_class):
-        """Reformatted the C++ class name to fit Matlab defined naming
-        standards
-        """
-        if len(instantiated_class.ctors) != 0:
-            return instantiated_class.ctors[0].name
-
-        return instantiated_class.name
-
-    @classmethod
-    def _format_type_name(cls,
-                          type_name,
-                          separator='::',
-                          include_namespace=True,
-                          constructor=False,
-                          method=False):
-        """
-        Args:
-            type_name: an interface_parser.Typename to reformat
-            separator: the statement to add between namespaces and typename
-            include_namespace: whether to include namespaces when reformatting
-            constructor: if the typename will be in a constructor
-            method: if the typename will be in a method
-
-        Raises:
-            constructor and method cannot both be true
-        """
-        if constructor and method:
-            raise Exception(
-                'Constructor and method parameters cannot both be True')
-
-        formatted_type_name = ''
-        name = type_name.name
-
-        if include_namespace:
-            for namespace in type_name.namespaces:
-                if name not in cls.ignore_namespace and namespace != '':
-                    formatted_type_name += namespace + separator
-
-        #self._debug("formatted_ns: {}, ns: {}".format(formatted_type_name, type_name.namespaces))
-        if constructor:
-            formatted_type_name += cls.data_type.get(name) or name
-        elif method:
-            formatted_type_name += cls.data_type_param.get(name) or name
-        else:
-            formatted_type_name += name
-
-        if separator == "::":  # C++
-            templates = []
-            for idx in range(len(type_name.instantiations)):
-                template = '{}'.format(
-                    cls._format_type_name(type_name.instantiations[idx],
-                                          include_namespace=include_namespace,
-                                          constructor=constructor,
-                                          method=method))
-                templates.append(template)
-
-            if len(templates) > 0:  # If there are no templates
-                formatted_type_name += '<{}>'.format(','.join(templates))
-
-        else:
-            for idx in range(len(type_name.instantiations)):
-                formatted_type_name += '{}'.format(
-                    cls._format_type_name(type_name.instantiations[idx],
-                                          separator=separator,
-                                          include_namespace=False,
-                                          constructor=constructor,
-                                          method=method))
-
-        return formatted_type_name
-
-    @classmethod
-    def _format_return_type(cls,
-                            return_type,
-                            include_namespace=False,
-                            separator="::"):
-        """Format return_type.
-
-        Args:
-            return_type: an interface_parser.ReturnType to reformat
-            include_namespace: whether to include namespaces when reformatting
-        """
-        return_wrap = ''
-
-        if cls._return_count(return_type) == 1:
-            return_wrap = cls._format_type_name(
-                return_type.type1.typename,
-                separator=separator,
-                include_namespace=include_namespace)
-        else:
-            return_wrap = 'pair< {type1}, {type2} >'.format(
-                type1=cls._format_type_name(
-                    return_type.type1.typename,
-                    separator=separator,
-                    include_namespace=include_namespace),
-                type2=cls._format_type_name(
-                    return_type.type2.typename,
-                    separator=separator,
-                    include_namespace=include_namespace))
-
-        return return_wrap
-
-    def _format_class_name(self, instantiated_class, separator=''):
-        """Format a template_instantiator.InstantiatedClass name."""
-        if instantiated_class.parent == '':
-            parent_full_ns = ['']
-        else:
-            parent_full_ns = instantiated_class.parent.full_namespaces()
-        # class_name = instantiated_class.parent.name
-        #
-        # if class_name != '':
-        #     class_name += separator
-        #
-        # class_name += instantiated_class.name
-        parentname = "".join([separator + x
-                              for x in parent_full_ns]) + separator
-
-        class_name = parentname[2 * len(separator):]
-
-        class_name += instantiated_class.name
-
-        return class_name
-
-    def _format_static_method(self, static_method, separator=''):
-        """Example:
-
-                gtsamPoint3.staticFunction
-        """
-        method = ''
-
-        if isinstance(static_method, parser.StaticMethod):
-            method += "".join([separator + x for x in static_method.parent.namespaces()]) + \
-                      separator + static_method.parent.name + separator
-
-        return method[2 * len(separator):]
-
-    def _format_instance_method(self, instance_method, separator=''):
-        """Example:
-
-                gtsamPoint3.staticFunction
-        """
-        method = ''
-
-        if isinstance(instance_method, instantiator.InstantiatedMethod):
-            method_list = [
-                separator + x
-                for x in instance_method.parent.parent.full_namespaces()
-            ]
-            method += "".join(method_list) + separator
-
-            method += instance_method.parent.name + separator
-            method += instance_method.original.name
-            method += "<" + instance_method.instantiations.to_cpp() + ">"
-
-        return method[2 * len(separator):]
-
-    def _format_global_method(self, static_method, separator=''):
-        """Example:
-
-                gtsamPoint3.staticFunction
-        """
-        method = ''
-
-        if isinstance(static_method, parser.GlobalFunction):
-            method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \
-                      separator
-
-        return method[2 * len(separator):]
-
     def _wrap_args(self, args):
         """Wrap an interface_parser.ArgumentList into a list of arguments.
 
@@ -520,7 +310,7 @@ def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False):
             if params != '':
                 params += ','
 
-            if self._is_ref(arg.ctype):  # and not constructor:
+            if self.is_ref(arg.ctype):  # and not constructor:
                 ctype_camel = self._format_type_name(arg.ctype.typename,
                                                      separator='')
                 body_args += textwrap.indent(textwrap.dedent('''\
@@ -531,7 +321,7 @@ def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False):
                            id=arg_id)),
                                              prefix='  ')
 
-            elif (self._is_shared_ptr(arg.ctype) or self._is_ptr(arg.ctype)) and \
+            elif (self.is_shared_ptr(arg.ctype) or self.is_ptr(arg.ctype)) and \
                     arg.ctype.typename.name not in self.ignore_namespace:
                 if arg.ctype.is_shared_ptr:
                     call_type = arg.ctype.is_shared_ptr
@@ -665,22 +455,13 @@ def class_comment(self, instantiated_class):
 
         return comment
 
-    def generate_matlab_wrapper(self):
-        """Generate the C++ file for the wrapper."""
-        file_name = self._wrapper_name() + '.cpp'
-
-        wrapper_file = self.wrapper_file_header
-
-        return file_name, wrapper_file
-
     def wrap_method(self, methods):
-        """Wrap methods in the body of a class."""
+        """
+        Wrap methods in the body of a class.
+        """
         if not isinstance(methods, list):
             methods = [methods]
 
-        # for method in methods:
-        #     output = ''
-
         return ''
 
     def wrap_methods(self, methods, global_funcs=False, global_ns=None):
@@ -697,10 +478,6 @@ def wrap_methods(self, methods, global_funcs=False, global_ns=None):
                 continue
 
             if global_funcs:
-                self._debug("[wrap_methods] wrapping: {}..{}={}".format(
-                    method[0].parent.name, method[0].name,
-                    type(method[0].parent.name)))
-
                 method_text = self.wrap_global_function(method)
                 self.content.append(("".join([
                     '+' + x + '/' for x in global_ns.full_namespaces()[1:]
@@ -838,11 +615,6 @@ def wrap_class_constructors(self, namespace_name, inst_class, parent_name,
 
         base_obj = ''
 
-        if has_parent:
-            self._debug("class: {} ns: {}".format(
-                parent_name,
-                self._format_class_name(inst_class.parent, separator=".")))
-
         if has_parent:
             base_obj = '  obj = obj@{parent_name}(uint64(5139824614673773682), base_ptr);'.format(
                 parent_name=parent_name)
@@ -850,9 +622,6 @@ def wrap_class_constructors(self, namespace_name, inst_class, parent_name,
         if base_obj:
             base_obj = '\n' + base_obj
 
-        self._debug("class: {}, name: {}".format(
-            inst_class.name, self._format_class_name(inst_class,
-                                                     separator=".")))
         methods_wrap += textwrap.indent(textwrap.dedent('''\
               else
                 error('Arguments do not match any overload of {class_name_doc} constructor');
@@ -1101,27 +870,12 @@ def wrap_static_methods(self, namespace_name, instantiated_class,
                                            prefix="  ")
 
         if serialize:
-            method_text += textwrap.indent(textwrap.dedent("""\
-                function varargout = string_deserialize(varargin)
-                  % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name}
-                  % Doxygen can be found at https://gtsam.org/doxygen/
-                  if length(varargin) == 1
-                    varargout{{1}} = {wrapper}({id}, varargin{{:}});
-                  else
-                    error('Arguments do not match any overload of function {class_name}.string_deserialize');
-                  end
-                end\n
-                function obj = loadobj(sobj)
-                  % LOADOBJ Saves the object to a matlab-readable format
-                  obj = {class_name}.string_deserialize(sobj);
-                end
-            """).format(
+            method_text += WrapperTemplate.matlab_deserialize.format(
                 class_name=namespace_name + '.' + instantiated_class.name,
                 wrapper=self._wrapper_name(),
                 id=self._update_wrapper_id(
                     (namespace_name, instantiated_class, 'string_deserialize',
-                     'deserialize'))),
-                                           prefix='  ')
+                     'deserialize')))
 
         return method_text
 
@@ -1213,33 +967,32 @@ def wrap_instantiated_class(self, instantiated_class, namespace_name=''):
 
         return file_name + '.m', content_text
 
-    def wrap_namespace(self, namespace, parent=()):
+    def wrap_namespace(self, namespace):
         """Wrap a namespace by wrapping all of its components.
 
         Args:
             namespace: the interface_parser.namespace instance of the namespace
             parent: parent namespace
         """
-        test_output = ''
         namespaces = namespace.full_namespaces()
         inner_namespace = namespace.name != ''
         wrapped = []
-        self._debug("wrapping ns: {}, parent: {}".format(
-            namespace.full_namespaces(), parent))
 
-        matlab_wrapper = self.generate_matlab_wrapper()
-        self.content.append((matlab_wrapper[0], matlab_wrapper[1]))
+        cpp_filename = self._wrapper_name() + '.cpp'
+        self.content.append((cpp_filename, self.wrapper_file_headers))
 
         current_scope = []
         namespace_scope = []
 
         for element in namespace.content:
             if isinstance(element, parser.Include):
-                self._add_include(element)
+                self.includes.append(element)
+
             elif isinstance(element, parser.Namespace):
-                self.wrap_namespace(element, namespaces)
+                self.wrap_namespace(element)
+
             elif isinstance(element, instantiator.InstantiatedClass):
-                self._add_class(element)
+                self.add_class(element)
 
                 if inner_namespace:
                     class_text = self.wrap_instantiated_class(
@@ -1265,7 +1018,7 @@ def wrap_namespace(self, namespace, parent=()):
             if isinstance(func, parser.GlobalFunction)
         ]
 
-        test_output += self.wrap_methods(all_funcs, True, global_ns=namespace)
+        self.wrap_methods(all_funcs, True, global_ns=namespace)
 
         return wrapped
 
@@ -1277,16 +1030,12 @@ def wrap_collector_function_shared_return(self,
         """Wrap the collector function which returns a shared pointer."""
         new_line = '\n' if new_line else ''
 
-        return textwrap.indent(textwrap.dedent('''\
-            {{
-            boost::shared_ptr<{name}> shared({shared_obj});
-            out[{id}] = wrap_shared_ptr(shared,"{name}");
-            }}{new_line}''').format(name=self._format_type_name(
-            return_type_name, include_namespace=False),
-                                    shared_obj=shared_obj,
-                                    id=func_id,
-                                    new_line=new_line),
-                               prefix='  ')
+        return WrapperTemplate.collector_function_shared_return.format(
+            name=self._format_type_name(return_type_name,
+                                        include_namespace=False),
+            shared_obj=shared_obj,
+            id=func_id,
+            new_line=new_line)
 
     def wrap_collector_function_return_types(self, return_type, func_id):
         """
@@ -1296,7 +1045,7 @@ def wrap_collector_function_return_types(self, return_type, func_id):
         pair_value = 'first' if func_id == 0 else 'second'
         new_line = '\n' if func_id == 0 else ''
 
-        if self._is_shared_ptr(return_type) or self._is_ptr(return_type):
+        if self.is_shared_ptr(return_type) or self.is_ptr(return_type):
             shared_obj = 'pairResult.' + pair_value
 
             if not (return_type.is_shared_ptr or return_type.is_ptr):
@@ -1355,16 +1104,12 @@ def wrap_collector_function_return(self, method):
                 method_name = self._format_static_method(method, '::')
             method_name += method.name
 
-        if "MeasureRange" in method_name:
-            self._debug("method: {}, method: {}, inst: {}".format(
-                method_name, method.name, method.parent.to_cpp()))
-
         obj = '  ' if return_1_name == 'void' else ''
         obj += '{}{}({})'.format(obj_start, method_name, params)
 
         if return_1_name != 'void':
             if return_count == 1:
-                if self._is_shared_ptr(return_1) or self._is_ptr(return_1):
+                if self.is_shared_ptr(return_1) or self.is_ptr(return_1):
                     sep_method_name = partial(self._format_type_name,
                                               return_1.typename,
                                               include_namespace=True)
@@ -1377,12 +1122,6 @@ def wrap_collector_function_return(self, method):
                         shared_obj = '{obj},"{method_name_sep}"'.format(
                             obj=obj, method_name_sep=sep_method_name('.'))
                     else:
-                        self._debug("Non-PTR: {}, {}".format(
-                            return_1, type(return_1)))
-                        self._debug("Inner type is: {}, {}".format(
-                            return_1.typename.name, sep_method_name('.')))
-                        self._debug("Inner type instantiations: {}".format(
-                            return_1.typename.instantiations))
                         method_name_sep_dot = sep_method_name('.')
                         shared_obj_template = 'boost::make_shared<{method_name_sep_col}>({obj}),' \
                                               '"{method_name_sep_dot}"'
@@ -1417,16 +1156,8 @@ def wrap_collector_function_upcast_from_void(self, class_name, func_id,
         """
         Add function to upcast type from void type.
         """
-        return textwrap.dedent('''\
-            void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{
-              mexAtExit(&_deleteAllObjects);
-              typedef boost::shared_ptr<{cpp_name}> Shared;
-              boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
-              out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-              Shared *self = new Shared(boost::static_pointer_cast<{cpp_name}>(*asVoid));
-              *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
-            }}\n
-        ''').format(class_name=class_name, cpp_name=cpp_name, id=func_id)
+        return WrapperTemplate.collector_function_upcast_from_void.format(
+            class_name=class_name, cpp_name=cpp_name, id=func_id)
 
     def generate_collector_function(self, func_id):
         """
@@ -1610,158 +1341,109 @@ def mex_function(self):
             else:
                 next_case = None
 
-        mex_function = textwrap.dedent('''
-            void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-            {{
-              mstream mout;
-              std::streambuf *outbuf = std::cout.rdbuf(&mout);\n
-              _{module_name}_RTTIRegister();\n
-              int id = unwrap<int>(in[0]);\n
-              try {{
-                switch(id) {{
-            {cases}    }}
-              }} catch(const std::exception& e) {{
-                mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str());
-              }}\n
-              std::cout.rdbuf(outbuf);
-            }}
-        ''').format(module_name=self.module_name, cases=cases)
+        mex_function = WrapperTemplate.mex_function.format(
+            module_name=self.module_name, cases=cases)
 
         return mex_function
 
-    def generate_wrapper(self, namespace):
-        """Generate the c++ wrapper."""
-        # Includes
-        wrapper_file = self.wrapper_file_header + textwrap.dedent("""
-            #include <boost/archive/text_iarchive.hpp>
-            #include <boost/archive/text_oarchive.hpp>
-            #include <boost/serialization/export.hpp>\n
-        """)
-
-        assert namespace
+    def get_class_name(self, cls):
+        """Get the name of the class `cls` taking template instantiations into account."""
+        if cls.instantiations:
+            class_name_sep = cls.name
+        else:
+            class_name_sep = cls.to_cpp()
 
-        includes_list = sorted(list(self.includes.keys()),
-                               key=lambda include: include.header)
+        class_name = self._format_class_name(cls)
 
-        # Check the number of includes.
-        # If no includes, do nothing, if 1 then just append newline.
-        # if more than one, concatenate them with newlines.
-        if len(includes_list) == 0:
-            pass
-        elif len(includes_list) == 1:
-            wrapper_file += (str(includes_list[0]) + '\n')
-        else:
-            wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y),
-                                   includes_list)
-        wrapper_file += '\n'
+        return class_name, class_name_sep
 
-        typedef_instances = '\n'
-        typedef_collectors = ''
+    def generate_preamble(self):
+        """
+        Generate the preamble of the wrapper file, which includes
+        the Boost exports, typedefs for collectors, and
+        the _deleteAllObjects and _RTTIRegister functions.
+        """
+        delete_objs = ''
+        typedef_instances = []
         boost_class_export_guid = ''
-        delete_objs = textwrap.dedent('''\
-            void _deleteAllObjects()
-            {
-              mstream mout;
-              std::streambuf *outbuf = std::cout.rdbuf(&mout);\n
-              bool anyDeleted = false;
-        ''')
-        rtti_reg_start = textwrap.dedent('''\
-            void _{module_name}_RTTIRegister() {{
-              const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created");
-              if(!alreadyCreated) {{
-                std::map<std::string, std::string> types;
-        ''').format(module_name=self.module_name)
-        rtti_reg_mid = ''
-        rtti_reg_end = textwrap.indent(
-            textwrap.dedent('''
-                mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
-                if(!registry)
-                  registry = mxCreateStructMatrix(1, 1, 0, NULL);
-                typedef std::pair<std::string, std::string> StringPair;
-                for(const StringPair& rtti_matlab: types) {
-                  int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-                  if(fieldId < 0)
-                    mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-                  mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
-                  mxSetFieldByNumber(registry, 0, fieldId, matlabName);
-                }
-                if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
-                  mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-                mxDestroyArray(registry);
-        '''),
-            prefix='    ') + '    \n' + textwrap.dedent('''\
-                mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-                if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
-                  mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-                mxDestroyArray(newAlreadyCreated);
-              }
-            }
-        ''')
-        ptr_ctor_frag = ''
+        typedef_collectors = ''
+        rtti_classes = ''
 
         for cls in self.classes:
-            uninstantiated_name = "::".join(
-                cls.namespaces()[1:]) + "::" + cls.name
-            self._debug("Cls: {} -> {}".format(cls.name, uninstantiated_name))
-
+            # Check if class is in ignore list.
+            # If so, then skip
+            uninstantiated_name = "::".join(cls.namespaces()[1:] + [cls.name])
             if uninstantiated_name in self.ignore_classes:
-                self._debug("Ignoring: {} -> {}".format(
-                    cls.name, uninstantiated_name))
                 continue
 
-            def _has_serialization(cls):
-                for m in cls.methods:
-                    if m.name in self.whitelist:
-                        return True
-                return False
+            class_name, class_name_sep = self.get_class_name(cls)
 
+            # If a class has instantiations, then declare the typedef for each instance
             if cls.instantiations:
                 cls_insts = ''
-
                 for i, inst in enumerate(cls.instantiations):
                     if i != 0:
                         cls_insts += ', '
 
                     cls_insts += self._format_type_name(inst)
 
-                typedef_instances += 'typedef {original_class_name} {class_name_sep};\n' \
+                typedef_instances.append('typedef {original_class_name} {class_name_sep};' \
                     .format(original_class_name=cls.to_cpp(),
-                            class_name_sep=cls.name)
+                            class_name_sep=cls.name))
 
-                class_name_sep = cls.name
-                class_name = self._format_class_name(cls)
+            # Get the Boost exports for serialization
+            if cls.original.namespaces() and self._has_serialization(cls):
+                boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format(
+                    class_name_sep, class_name)
 
-                if len(cls.original.namespaces()) > 1 and _has_serialization(
-                        cls):
-                    boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format(
-                        class_name_sep, class_name)
-            else:
-                class_name_sep = cls.to_cpp()
-                class_name = self._format_class_name(cls)
-
-                if len(cls.original.namespaces()) > 1 and _has_serialization(
-                        cls):
-                    boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format(
-                        class_name_sep, class_name)
-
-            typedef_collectors += textwrap.dedent('''\
-                typedef std::set<boost::shared_ptr<{class_name_sep}>*> Collector_{class_name};
-                static Collector_{class_name} collector_{class_name};
-            ''').format(class_name_sep=class_name_sep, class_name=class_name)
-            delete_objs += textwrap.indent(textwrap.dedent('''\
-                {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin();
-                    iter != collector_{class_name}.end(); ) {{
-                  delete *iter;
-                  collector_{class_name}.erase(iter++);
-                  anyDeleted = true;
-                }} }}
-            ''').format(class_name=class_name),
-                                           prefix='  ')
+            # Typedef and declare the collector objects.
+            typedef_collectors += WrapperTemplate.typdef_collectors.format(
+                class_name_sep=class_name_sep, class_name=class_name)
+
+            # Generate the _deleteAllObjects method
+            delete_objs += WrapperTemplate.delete_obj.format(
+                class_name=class_name)
 
             if cls.is_virtual:
-                rtti_reg_mid += '    types.insert(std::make_pair(typeid({}).name(), "{}"));\n' \
+                class_name, class_name_sep = self.get_class_name(cls)
+                rtti_classes += '    types.insert(std::make_pair(typeid({}).name(), "{}"));\n' \
                     .format(class_name_sep, class_name)
 
+        # Generate the typedef instances string
+        typedef_instances = "\n".join(typedef_instances)
+
+        # Generate the full deleteAllObjects function
+        delete_all_objs = WrapperTemplate.delete_all_objects.format(
+            delete_objs=delete_objs)
+
+        # Generate the full RTTIRegister function
+        rtti_register = WrapperTemplate.rtti_register.format(
+            module_name=self.module_name, rtti_classes=rtti_classes)
+
+        return typedef_instances, boost_class_export_guid, \
+            typedef_collectors, delete_all_objs, rtti_register
+
+    def generate_wrapper(self, namespace):
+        """Generate the c++ wrapper."""
+        assert namespace, "Namespace if empty"
+
+        # Generate the header includes
+        includes_list = sorted(self.includes,
+                               key=lambda include: include.header)
+        includes = textwrap.dedent("""\
+            {wrapper_file_headers}
+            {boost_headers}
+            {includes_list}
+        """).format(wrapper_file_headers=self.wrapper_file_headers.strip(),
+                    boost_headers=WrapperTemplate.boost_headers,
+                    includes_list='\n'.join(map(str, includes_list)))
+
+        preamble = self.generate_preamble()
+        typedef_instances, boost_class_export_guid, \
+            typedef_collectors, delete_all_objs, \
+                rtti_register = preamble
+
+        ptr_ctor_frag = ''
         set_next_case = False
 
         for idx in range(self.wrapper_id):
@@ -1784,24 +1466,20 @@ def _has_serialization(cls):
                 ptr_ctor_frag += self.wrap_collector_function_upcast_from_void(
                     id_val[1].name, idx, id_val[1].to_cpp())
 
-        wrapper_file += textwrap.dedent('''\
+        wrapper_file = textwrap.dedent('''\
+            {includes}
             {typedef_instances}
             {boost_class_export_guid}
             {typedefs_collectors}
-            {delete_objs}  if(anyDeleted)
-                cout <<
-                  "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\\n"
-                  "calling destructors, call \'clear all\' again if you plan to now recompile a wrap\\n"
-                  "module, so that your recompiled module is used instead of the old one." << endl;
-              std::cout.rdbuf(outbuf);
-            }}\n
+            {delete_all_objs}
             {rtti_register}
             {pointer_constructor_fragment}{mex_function}''') \
-            .format(typedef_instances=typedef_instances,
+            .format(includes=includes,
+                    typedef_instances=typedef_instances,
                     boost_class_export_guid=boost_class_export_guid,
                     typedefs_collectors=typedef_collectors,
-                    delete_objs=delete_objs,
-                    rtti_register=rtti_reg_start + rtti_reg_mid + rtti_reg_end,
+                    delete_all_objs=delete_all_objs,
+                    rtti_register=rtti_register,
                     pointer_constructor_fragment=ptr_ctor_frag,
                     mex_function=self.mex_function())
 
@@ -1815,23 +1493,10 @@ def wrap_class_serialize_method(self, namespace_name, inst_class):
         wrapper_id = self._update_wrapper_id(
             (namespace_name, inst_class, 'string_serialize', 'serialize'))
 
-        return textwrap.dedent('''\
-            function varargout = string_serialize(this, varargin)
-              % STRING_SERIALIZE usage: string_serialize() : returns string
-              % Doxygen can be found at https://gtsam.org/doxygen/
-              if length(varargin) == 0
-                varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}});
-              else
-                error('Arguments do not match any overload of function {class_name}.string_serialize');
-              end
-            end\n
-            function sobj = saveobj(obj)
-              % SAVEOBJ Saves the object to a matlab-readable format
-              sobj = obj.string_serialize();
-            end
-        ''').format(wrapper=self._wrapper_name(),
-                    wrapper_id=wrapper_id,
-                    class_name=namespace_name + '.' + class_name)
+        return WrapperTemplate.class_serialize_method.format(
+            wrapper=self._wrapper_name(),
+            wrapper_id=wrapper_id,
+            class_name=namespace_name + '.' + class_name)
 
     def wrap_collector_function_serialize(self,
                                           class_name,
@@ -1840,18 +1505,8 @@ def wrap_collector_function_serialize(self,
         """
         Wrap the serizalize collector function.
         """
-        return textwrap.indent(textwrap.dedent("""\
-            typedef boost::shared_ptr<{full_name}> Shared;
-            checkArguments("string_serialize",nargout,nargin-1,0);
-            Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}");
-            ostringstream out_archive_stream;
-            boost::archive::text_oarchive out_archive(out_archive_stream);
-            out_archive << *obj;
-            out[0] = wrap< string >(out_archive_stream.str());
-        """).format(class_name=class_name,
-                    full_name=full_name,
-                    namespace=namespace),
-                               prefix='  ')
+        return WrapperTemplate.collector_function_serialize.format(
+            class_name=class_name, full_name=full_name, namespace=namespace)
 
     def wrap_collector_function_deserialize(self,
                                             class_name,
@@ -1860,87 +1515,85 @@ def wrap_collector_function_deserialize(self,
         """
         Wrap the deserizalize collector function.
         """
-        return textwrap.indent(textwrap.dedent("""\
-            typedef boost::shared_ptr<{full_name}> Shared;
-            checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1);
-            string serialized = unwrap< string >(in[0]);
-            istringstream in_archive_stream(serialized);
-            boost::archive::text_iarchive in_archive(in_archive_stream);
-            Shared output(new {full_name}());
-            in_archive >> *output;
-            out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false);
-        """).format(class_name=class_name,
-                    full_name=full_name,
-                    namespace=namespace),
-                               prefix='  ')
+        return WrapperTemplate.collector_function_deserialize.format(
+            class_name=class_name, full_name=full_name, namespace=namespace)
 
-    def wrap(self, content):
-        """High level function to wrap the project."""
-        # Parse the contents of the interface file
-        parsed_result = parser.Module.parseString(content)
-        # Instantiate the module
-        module = instantiator.instantiate_namespace(parsed_result)
-        self.wrap_namespace(module)
-        self.generate_wrapper(module)
+    def generate_content(self, cc_content, path):
+        """
+        Generate files and folders from matlab wrapper content.
 
-        return self.content
+        Args:
+            cc_content: The content to generate formatted as
+                (file_name, file_content) or
+                (folder_name, [(file_name, file_content)])
+            path: The path to the files parent folder within the main folder
+        """
+        for c in cc_content:
+            if isinstance(c, list):
+                if len(c) == 0:
+                    continue
 
+                path_to_folder = osp.join(path, c[0][0])
+
+                if not osp.isdir(path_to_folder):
+                    try:
+                        os.makedirs(path_to_folder, exist_ok=True)
+                    except OSError:
+                        pass
+
+                for sub_content in c:
+                    self.generate_content(sub_content[1], path_to_folder)
+
+            elif isinstance(c[1], list):
+                path_to_folder = osp.join(path, c[0])
+
+                if not osp.isdir(path_to_folder):
+                    try:
+                        os.makedirs(path_to_folder, exist_ok=True)
+                    except OSError:
+                        pass
+                for sub_content in c[1]:
+                    path_to_file = osp.join(path_to_folder, sub_content[0])
+                    with open(path_to_file, 'w') as f:
+                        f.write(sub_content[1])
+            else:
+                path_to_file = osp.join(path, c[0])
 
-def generate_content(cc_content, path, verbose=False):
-    """
-    Generate files and folders from matlab wrapper content.
+                if not osp.isdir(path_to_file):
+                    try:
+                        os.mkdir(path)
+                    except OSError:
+                        pass
 
-    Args:
-        cc_content: The content to generate formatted as
-            (file_name, file_content) or
-            (folder_name, [(file_name, file_content)])
-        path: The path to the files parent folder within the main folder
-    """
-    def _debug(message):
-        if not verbose:
-            return
-        print(message, file=sys.stderr)
-
-    for c in cc_content:
-        if isinstance(c, list):
-            if len(c) == 0:
-                continue
-            _debug("c object: {}".format(c[0][0]))
-            path_to_folder = osp.join(path, c[0][0])
-
-            if not os.path.isdir(path_to_folder):
-                try:
-                    os.makedirs(path_to_folder, exist_ok=True)
-                except OSError:
-                    pass
-
-            for sub_content in c:
-                _debug("sub object: {}".format(sub_content[1][0][0]))
-                generate_content(sub_content[1], path_to_folder)
-
-        elif isinstance(c[1], list):
-            path_to_folder = osp.join(path, c[0])
-
-            _debug("[generate_content_global]: {}".format(path_to_folder))
-            if not os.path.isdir(path_to_folder):
-                try:
-                    os.makedirs(path_to_folder, exist_ok=True)
-                except OSError:
-                    pass
-            for sub_content in c[1]:
-                path_to_file = osp.join(path_to_folder, sub_content[0])
-                _debug("[generate_global_method]: {}".format(path_to_file))
                 with open(path_to_file, 'w') as f:
-                    f.write(sub_content[1])
-        else:
-            path_to_file = osp.join(path, c[0])
+                    f.write(c[1])
 
-            _debug("[generate_content]: {}".format(path_to_file))
-            if not os.path.isdir(path_to_file):
-                try:
-                    os.mkdir(path)
-                except OSError:
-                    pass
+    def wrap(self, files, path):
+        """High level function to wrap the project."""
+        modules = {}
+        for file in files:
+            with open(file, 'r') as f:
+                content = f.read()
 
-            with open(path_to_file, 'w') as f:
-                f.write(c[1])
+            # Parse the contents of the interface file
+            parsed_result = parser.Module.parseString(content)
+            # print(parsed_result)
+
+            # Instantiate the module
+            module = instantiator.instantiate_namespace(parsed_result)
+
+            if module.name in modules:
+                modules[module.
+                        name].content[0].content += module.content[0].content
+            else:
+                modules[module.name] = module
+
+        for module in modules.values():
+            # Wrap the full namespace
+            self.wrap_namespace(module)
+            self.generate_wrapper(module)
+
+            # Generate the corresponding .m and .cpp files
+            self.generate_content(self.content, path)
+
+        return self.content
diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py
index 0e1b3c7eaa..40571263aa 100755
--- a/gtwrap/pybind_wrapper.py
+++ b/gtwrap/pybind_wrapper.py
@@ -13,6 +13,7 @@
 # pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, line-too-long
 
 import re
+from pathlib import Path
 
 import gtwrap.interface_parser as parser
 import gtwrap.template_instantiator as instantiator
@@ -32,7 +33,7 @@ def __init__(self,
         self.top_module_namespaces = top_module_namespaces
         self.use_boost = use_boost
         self.ignore_classes = ignore_classes
-        self._serializing_classes = list()
+        self._serializing_classes = []
         self.module_template = module_template
         self.python_keywords = [
             'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import',
@@ -160,7 +161,7 @@ def _wrap_method(self,
                 'self->print',
                 'py::scoped_ostream_redirect output; self->print')
 
-            # Make __repr__() call print() internally
+            # Make __repr__() call .print() internally
             ret += '''{prefix}.def("__repr__",
                     [](const {cpp_class}& self{opt_comma}{args_signature_with_names}){{
                         gtsam::RedirectCout redirect;
@@ -557,8 +558,15 @@ def wrap_namespace(self, namespace):
             )
         return wrapped, includes
 
-    def wrap(self, content):
-        """Wrap the code in the interface file."""
+    def wrap_file(self, content, module_name=None, submodules=None):
+        """
+        Wrap the code in the interface file.
+
+        Args:
+            content: The contents of the interface file.
+            module_name: The name of the module.
+            submodules: List of other interface file names that should be linked to.
+        """
         # Parse the contents of the interface file
         module = parser.Module.parseString(content)
         # Instantiate all templates
@@ -574,23 +582,74 @@ def wrap(self, content):
             if ',' in cpp_class:
                 new_name = re.sub("[,:<> ]", "", cpp_class)
                 boost_class_export += "typedef {cpp_class} {new_name};\n".format(  # noqa
-                    cpp_class=cpp_class,
-                    new_name=new_name,
-                )
+                    cpp_class=cpp_class, new_name=new_name)
+
             boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format(
                 new_name=new_name, )
 
+        # Reset the serializing classes list
+        self._serializing_classes = []
+
         holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \
                       "{shared_ptr_type}::shared_ptr<TYPE_PLACEHOLDER_DONOTUSE>);"
         include_boost = "#include <boost/shared_ptr.hpp>" if self.use_boost else ""
 
+        submodules_init = []
+
+        if submodules is not None:
+            module_def = "PYBIND11_MODULE({0}, m_)".format(module_name)
+
+            for idx, submodule in enumerate(submodules):
+                submodules[idx] = "void {0}(py::module_ &);".format(submodule)
+                submodules_init.append("{0}(m_);".format(submodule))
+
+        else:
+            module_def = "void {0}(py::module_ &m_)".format(module_name)
+            submodules = []
+
         return self.module_template.format(
             include_boost=include_boost,
-            module_name=self.module_name,
+            module_def=module_def,
+            module_name=module_name,
             includes=includes,
             holder_type=holder_type.format(
                 shared_ptr_type=('boost' if self.use_boost else 'std'))
             if self.use_boost else "",
             wrapped_namespace=wrapped_namespace,
             boost_class_export=boost_class_export,
+            submodules="\n".join(submodules),
+            submodules_init="\n".join(submodules_init),
         )
+
+    def wrap(self, sources, main_output):
+        """
+        Wrap all the source interface files.
+
+        Args:
+            sources: List of all interface files.
+            main_output: The name for the main module.
+        """
+        main_module = sources[0]
+        submodules = []
+        for source in sources[1:]:
+            filename = Path(source).name
+            module_name = Path(source).stem
+            # Read in the complete interface (.i) file
+            with open(source, "r") as f:
+                content = f.read()
+            submodules.append(module_name)
+            cc_content = self.wrap_file(content, module_name=module_name)
+
+            # Generate the C++ code which Pybind11 will use.
+            with open(filename.replace(".i", ".cpp"), "w") as f:
+                f.write(cc_content)
+
+        with open(main_module, "r") as f:
+            content = f.read()
+        cc_content = self.wrap_file(content,
+                                    module_name=self.module_name,
+                                    submodules=submodules)
+
+        # Generate the C++ code which Pybind11 will use.
+        with open(main_output, "w") as f:
+            f.write(cc_content)
diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py
index c474246489..87729cfa6d 100644
--- a/gtwrap/template_instantiator.py
+++ b/gtwrap/template_instantiator.py
@@ -4,7 +4,7 @@
 
 import itertools
 from copy import deepcopy
-from typing import List
+from typing import Iterable, List
 
 import gtwrap.interface_parser as parser
 
@@ -29,12 +29,13 @@ def instantiate_type(ctype: parser.Type,
     ctype = deepcopy(ctype)
 
     # Check if the return type has template parameters
-    if len(ctype.typename.instantiations) > 0:
+    if ctype.typename.instantiations:
         for idx, instantiation in enumerate(ctype.typename.instantiations):
             if instantiation.name in template_typenames:
                 template_idx = template_typenames.index(instantiation.name)
-                ctype.typename.instantiations[idx] = instantiations[
-                    template_idx]
+                ctype.typename.instantiations[
+                    idx] = instantiations[  # type: ignore
+                        template_idx]
 
         return ctype
 
@@ -212,7 +213,9 @@ class A {
         void func(X x, Y y);
     }
     """
-    def __init__(self, original, instantiations: List[parser.Typename] = ''):
+    def __init__(self,
+                 original,
+                 instantiations: Iterable[parser.Typename] = ()):
         self.original = original
         self.instantiations = instantiations
         self.template = ''
@@ -278,7 +281,7 @@ def __init__(self, original: parser.Class, instantiations=(), new_name=''):
         self.original = original
         self.instantiations = instantiations
 
-        self.template = ''
+        self.template = None
         self.is_virtual = original.is_virtual
         self.parent_class = original.parent_class
         self.parent = original.parent
@@ -318,7 +321,7 @@ def __init__(self, original: parser.Class, instantiations=(), new_name=''):
         self.methods = []
         for method in instantiated_methods:
             if not method.template:
-                self.methods.append(InstantiatedMethod(method, ''))
+                self.methods.append(InstantiatedMethod(method, ()))
             else:
                 instantiations = []
                 # Get all combinations of template parameters
@@ -342,9 +345,9 @@ def __init__(self, original: parser.Class, instantiations=(), new_name=''):
         )
 
     def __repr__(self):
-        return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\
-            "{ctors}\n{static_methods}\n{methods}".format(
-               virtual="virtual" if self.is_virtual else '',
+        return "{virtual}Class {cpp_class} : {parent_class}\n"\
+            "{ctors}\n{static_methods}\n{methods}\n{operators}".format(
+               virtual="virtual " if self.is_virtual else '',
                name=self.name,
                cpp_class=self.to_cpp(),
                parent_class=self.parent,
diff --git a/scripts/matlab_wrap.py b/scripts/matlab_wrap.py
index be6043947f..0f6664a635 100644
--- a/scripts/matlab_wrap.py
+++ b/scripts/matlab_wrap.py
@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-
 """
 Helper script to wrap C++ to Matlab.
 This script is installed via CMake to the user's binary directory
@@ -7,19 +6,24 @@
 """
 
 import argparse
-import os
 import sys
 
-from gtwrap.matlab_wrapper import MatlabWrapper, generate_content
+from gtwrap.matlab_wrapper import MatlabWrapper
 
 if __name__ == "__main__":
     arg_parser = argparse.ArgumentParser(
         formatter_class=argparse.ArgumentDefaultsHelpFormatter)
-    arg_parser.add_argument("--src", type=str, required=True,
+    arg_parser.add_argument("--src",
+                            type=str,
+                            required=True,
                             help="Input interface .h file.")
-    arg_parser.add_argument("--module_name", type=str, required=True,
+    arg_parser.add_argument("--module_name",
+                            type=str,
+                            required=True,
                             help="Name of the C++ class being wrapped.")
-    arg_parser.add_argument("--out", type=str, required=True,
+    arg_parser.add_argument("--out",
+                            type=str,
+                            required=True,
                             help="Name of the output folder.")
     arg_parser.add_argument(
         "--top_module_namespaces",
@@ -33,28 +37,22 @@
         "`<module_name>.Class` of the corresponding C++ `ns1::ns2::ns3::Class`"
         ", and `from <module_name> import ns4` gives you access to a Python "
         "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ")
-    arg_parser.add_argument("--ignore",
-                            nargs='*',
-                            type=str,
-                            help="A space-separated list of classes to ignore. "
-                                 "Class names must include their full namespaces.")
+    arg_parser.add_argument(
+        "--ignore",
+        nargs='*',
+        type=str,
+        help="A space-separated list of classes to ignore. "
+        "Class names must include their full namespaces.")
     args = arg_parser.parse_args()
 
     top_module_namespaces = args.top_module_namespaces.split("::")
     if top_module_namespaces[0]:
         top_module_namespaces = [''] + top_module_namespaces
 
-    with open(args.src, 'r') as f:
-        content = f.read()
-
-    if not os.path.exists(args.src):
-        os.mkdir(args.src)
-
-    print("Ignoring classes: {}".format(args.ignore), file=sys.stderr)
+    print("[MatlabWrapper] Ignoring classes: {}".format(args.ignore), file=sys.stderr)
     wrapper = MatlabWrapper(module_name=args.module_name,
                             top_module_namespace=top_module_namespaces,
                             ignore_classes=args.ignore)
 
-    cc_content = wrapper.wrap(content)
-
-    generate_content(cc_content, args.out)
+    sources = args.src.split(';')
+    cc_content = wrapper.wrap(sources, path=args.out)
diff --git a/scripts/pybind_wrap.py b/scripts/pybind_wrap.py
index 7f2f8d4192..c82a1d24c0 100644
--- a/scripts/pybind_wrap.py
+++ b/scripts/pybind_wrap.py
@@ -67,10 +67,6 @@ def main():
     if top_module_namespaces[0]:
         top_module_namespaces = [''] + top_module_namespaces
 
-    # Read in the complete interface (.i) file
-    with open(args.src, "r") as f:
-        content = f.read()
-
     with open(args.template, "r") as f:
         template_content = f.read()
 
@@ -83,11 +79,8 @@ def main():
     )
 
     # Wrap the code and get back the cpp/cc code.
-    cc_content = wrapper.wrap(content)
-
-    # Generate the C++ code which Pybind11 will use.
-    with open(args.out, "w") as f:
-        f.write(cc_content)
+    sources = args.src.split(';')
+    wrapper.wrap(sources, args.out)
 
 
 if __name__ == "__main__":
diff --git a/templates/pybind_wrapper.tpl.example b/templates/pybind_wrapper.tpl.example
index bf5b334900..485aa8d00c 100644
--- a/templates/pybind_wrapper.tpl.example
+++ b/templates/pybind_wrapper.tpl.example
@@ -7,7 +7,7 @@
 #include <pybind11/iostream.h>
 #include <pybind11/functional.h>
 #include "gtsam/base/serialization.h"
-#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+#include "gtsam/base/utilities.h"  // for RedirectCout.
 
 {includes}
 #include <boost/serialization/export.hpp>
@@ -22,9 +22,13 @@ using namespace std;
 
 namespace py = pybind11;
 
-PYBIND11_MODULE({module_name}, m_) {{
+{submodules}
+
+{module_def} {{
     m_.doc() = "pybind11 wrapper of {module_name}";
 
+{submodules_init}
+
 {wrapped_namespace}
 
 #include "python/specializations.h"
diff --git a/tests/expected/matlab/+gtsam/Class1.m b/tests/expected/matlab/+gtsam/Class1.m
new file mode 100644
index 0000000000..00dd5ca746
--- /dev/null
+++ b/tests/expected/matlab/+gtsam/Class1.m
@@ -0,0 +1,36 @@
+%class Class1, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%Class1()
+%
+classdef Class1 < handle
+  properties
+    ptr_gtsamClass1 = 0
+  end
+  methods
+    function obj = Class1(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        multiple_files_wrapper(0, my_ptr);
+      elseif nargin == 0
+        my_ptr = multiple_files_wrapper(1);
+      else
+        error('Arguments do not match any overload of gtsam.Class1 constructor');
+      end
+      obj.ptr_gtsamClass1 = my_ptr;
+    end
+
+    function delete(obj)
+      multiple_files_wrapper(2, obj.ptr_gtsamClass1);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
diff --git a/tests/expected/matlab/+gtsam/Class2.m b/tests/expected/matlab/+gtsam/Class2.m
new file mode 100644
index 0000000000..93279e1560
--- /dev/null
+++ b/tests/expected/matlab/+gtsam/Class2.m
@@ -0,0 +1,36 @@
+%class Class2, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%Class2()
+%
+classdef Class2 < handle
+  properties
+    ptr_gtsamClass2 = 0
+  end
+  methods
+    function obj = Class2(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        multiple_files_wrapper(3, my_ptr);
+      elseif nargin == 0
+        my_ptr = multiple_files_wrapper(4);
+      else
+        error('Arguments do not match any overload of gtsam.Class2 constructor');
+      end
+      obj.ptr_gtsamClass2 = my_ptr;
+    end
+
+    function delete(obj)
+      multiple_files_wrapper(5, obj.ptr_gtsamClass2);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
diff --git a/tests/expected/matlab/+gtsam/ClassA.m b/tests/expected/matlab/+gtsam/ClassA.m
new file mode 100644
index 0000000000..3210e93c60
--- /dev/null
+++ b/tests/expected/matlab/+gtsam/ClassA.m
@@ -0,0 +1,36 @@
+%class ClassA, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%ClassA()
+%
+classdef ClassA < handle
+  properties
+    ptr_gtsamClassA = 0
+  end
+  methods
+    function obj = ClassA(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        multiple_files_wrapper(6, my_ptr);
+      elseif nargin == 0
+        my_ptr = multiple_files_wrapper(7);
+      else
+        error('Arguments do not match any overload of gtsam.ClassA constructor');
+      end
+      obj.ptr_gtsamClassA = my_ptr;
+    end
+
+    function delete(obj)
+      multiple_files_wrapper(8, obj.ptr_gtsamClassA);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
diff --git a/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp
index e644ac00f2..fab9c14506 100644
--- a/tests/expected/matlab/class_wrapper.cpp
+++ b/tests/expected/matlab/class_wrapper.cpp
@@ -7,7 +7,6 @@
 
 #include <folder/path/to/Test.h>
 
-
 typedef Fun<double> FunDouble;
 typedef PrimitiveRef<double> PrimitiveRefDouble;
 typedef MyVector<3> MyVector3;
@@ -16,7 +15,6 @@ typedef MultipleTemplates<int, double> MultipleTemplatesIntDouble;
 typedef MultipleTemplates<int, float> MultipleTemplatesIntFloat;
 typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
 
-
 typedef std::set<boost::shared_ptr<FunRange>*> Collector_FunRange;
 static Collector_FunRange collector_FunRange;
 typedef std::set<boost::shared_ptr<FunDouble>*> Collector_FunDouble;
@@ -38,6 +36,7 @@ static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 
+
 void _deleteAllObjects()
 {
   mstream mout;
@@ -104,6 +103,7 @@ void _deleteAllObjects()
     collector_MyFactorPosePoint2.erase(iter++);
     anyDeleted = true;
   } }
+
   if(anyDeleted)
     cout <<
       "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
@@ -117,24 +117,29 @@ void _class_RTTIRegister() {
   if(!alreadyCreated) {
     std::map<std::string, std::string> types;
 
+
+
     mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
     if(!registry)
       registry = mxCreateStructMatrix(1, 1, 0, NULL);
     typedef std::pair<std::string, std::string> StringPair;
     for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-      if(fieldId < 0)
+      if(fieldId < 0) {
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+      }
       mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
       mxSetFieldByNumber(registry, 0, fieldId, matlabName);
     }
-    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
+    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(registry);
-    
+
     mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
+    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(newAlreadyCreated);
   }
 }
diff --git a/tests/expected/matlab/functions_wrapper.cpp b/tests/expected/matlab/functions_wrapper.cpp
index ae7f49c410..d0f0f8ca67 100644
--- a/tests/expected/matlab/functions_wrapper.cpp
+++ b/tests/expected/matlab/functions_wrapper.cpp
@@ -5,38 +5,11 @@
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/export.hpp>
 
-#include <folder/path/to/Test.h>
 
 
-typedef Fun<double> FunDouble;
-typedef PrimitiveRef<double> PrimitiveRefDouble;
-typedef MyVector<3> MyVector3;
-typedef MyVector<12> MyVector12;
-typedef MultipleTemplates<int, double> MultipleTemplatesIntDouble;
-typedef MultipleTemplates<int, float> MultipleTemplatesIntFloat;
-typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
 
 
-typedef std::set<boost::shared_ptr<FunRange>*> Collector_FunRange;
-static Collector_FunRange collector_FunRange;
-typedef std::set<boost::shared_ptr<FunDouble>*> Collector_FunDouble;
-static Collector_FunDouble collector_FunDouble;
-typedef std::set<boost::shared_ptr<Test>*> Collector_Test;
-static Collector_Test collector_Test;
-typedef std::set<boost::shared_ptr<PrimitiveRefDouble>*> Collector_PrimitiveRefDouble;
-static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble;
-typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
-static Collector_MyVector3 collector_MyVector3;
-typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
-static Collector_MyVector12 collector_MyVector12;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_MultipleTemplatesIntDouble;
-static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
-static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
-typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
-static Collector_ForwardKinematics collector_ForwardKinematics;
-typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
-static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
+
 
 void _deleteAllObjects()
 {
@@ -44,66 +17,7 @@ void _deleteAllObjects()
   std::streambuf *outbuf = std::cout.rdbuf(&mout);
 
   bool anyDeleted = false;
-  { for(Collector_FunRange::iterator iter = collector_FunRange.begin();
-      iter != collector_FunRange.end(); ) {
-    delete *iter;
-    collector_FunRange.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin();
-      iter != collector_FunDouble.end(); ) {
-    delete *iter;
-    collector_FunDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_Test::iterator iter = collector_Test.begin();
-      iter != collector_Test.end(); ) {
-    delete *iter;
-    collector_Test.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin();
-      iter != collector_PrimitiveRefDouble.end(); ) {
-    delete *iter;
-    collector_PrimitiveRefDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
-      iter != collector_MyVector3.end(); ) {
-    delete *iter;
-    collector_MyVector3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin();
-      iter != collector_MyVector12.end(); ) {
-    delete *iter;
-    collector_MyVector12.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin();
-      iter != collector_MultipleTemplatesIntDouble.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin();
-      iter != collector_MultipleTemplatesIntFloat.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntFloat.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
-      iter != collector_ForwardKinematics.end(); ) {
-    delete *iter;
-    collector_ForwardKinematics.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
-      iter != collector_MyFactorPosePoint2.end(); ) {
-    delete *iter;
-    collector_MyFactorPosePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
+
   if(anyDeleted)
     cout <<
       "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
@@ -117,24 +31,29 @@ void _functions_RTTIRegister() {
   if(!alreadyCreated) {
     std::map<std::string, std::string> types;
 
+
+
     mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
     if(!registry)
       registry = mxCreateStructMatrix(1, 1, 0, NULL);
     typedef std::pair<std::string, std::string> StringPair;
     for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-      if(fieldId < 0)
+      if(fieldId < 0) {
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+      }
       mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
       mxSetFieldByNumber(registry, 0, fieldId, matlabName);
     }
-    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
+    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(registry);
-    
+
     mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
+    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(newAlreadyCreated);
   }
 }
diff --git a/tests/expected/matlab/geometry_wrapper.cpp b/tests/expected/matlab/geometry_wrapper.cpp
index 4d8a7c7893..81631390c9 100644
--- a/tests/expected/matlab/geometry_wrapper.cpp
+++ b/tests/expected/matlab/geometry_wrapper.cpp
@@ -5,112 +5,25 @@
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/export.hpp>
 
-#include <folder/path/to/Test.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
 
-typedef Fun<double> FunDouble;
-typedef PrimitiveRef<double> PrimitiveRefDouble;
-typedef MyVector<3> MyVector3;
-typedef MyVector<12> MyVector12;
-typedef MultipleTemplates<int, double> MultipleTemplatesIntDouble;
-typedef MultipleTemplates<int, float> MultipleTemplatesIntFloat;
-typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
 
 BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2");
 BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3");
 
-typedef std::set<boost::shared_ptr<FunRange>*> Collector_FunRange;
-static Collector_FunRange collector_FunRange;
-typedef std::set<boost::shared_ptr<FunDouble>*> Collector_FunDouble;
-static Collector_FunDouble collector_FunDouble;
-typedef std::set<boost::shared_ptr<Test>*> Collector_Test;
-static Collector_Test collector_Test;
-typedef std::set<boost::shared_ptr<PrimitiveRefDouble>*> Collector_PrimitiveRefDouble;
-static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble;
-typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
-static Collector_MyVector3 collector_MyVector3;
-typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
-static Collector_MyVector12 collector_MyVector12;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_MultipleTemplatesIntDouble;
-static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
-static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
-typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
-static Collector_ForwardKinematics collector_ForwardKinematics;
-typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
-static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
 static Collector_gtsamPoint2 collector_gtsamPoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point3>*> Collector_gtsamPoint3;
 static Collector_gtsamPoint3 collector_gtsamPoint3;
 
+
 void _deleteAllObjects()
 {
   mstream mout;
   std::streambuf *outbuf = std::cout.rdbuf(&mout);
 
   bool anyDeleted = false;
-  { for(Collector_FunRange::iterator iter = collector_FunRange.begin();
-      iter != collector_FunRange.end(); ) {
-    delete *iter;
-    collector_FunRange.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin();
-      iter != collector_FunDouble.end(); ) {
-    delete *iter;
-    collector_FunDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_Test::iterator iter = collector_Test.begin();
-      iter != collector_Test.end(); ) {
-    delete *iter;
-    collector_Test.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin();
-      iter != collector_PrimitiveRefDouble.end(); ) {
-    delete *iter;
-    collector_PrimitiveRefDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
-      iter != collector_MyVector3.end(); ) {
-    delete *iter;
-    collector_MyVector3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin();
-      iter != collector_MyVector12.end(); ) {
-    delete *iter;
-    collector_MyVector12.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin();
-      iter != collector_MultipleTemplatesIntDouble.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin();
-      iter != collector_MultipleTemplatesIntFloat.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntFloat.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
-      iter != collector_ForwardKinematics.end(); ) {
-    delete *iter;
-    collector_ForwardKinematics.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
-      iter != collector_MyFactorPosePoint2.end(); ) {
-    delete *iter;
-    collector_MyFactorPosePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
   { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin();
       iter != collector_gtsamPoint2.end(); ) {
     delete *iter;
@@ -123,6 +36,7 @@ void _deleteAllObjects()
     collector_gtsamPoint3.erase(iter++);
     anyDeleted = true;
   } }
+
   if(anyDeleted)
     cout <<
       "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
@@ -136,24 +50,29 @@ void _geometry_RTTIRegister() {
   if(!alreadyCreated) {
     std::map<std::string, std::string> types;
 
+
+
     mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
     if(!registry)
       registry = mxCreateStructMatrix(1, 1, 0, NULL);
     typedef std::pair<std::string, std::string> StringPair;
     for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-      if(fieldId < 0)
+      if(fieldId < 0) {
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+      }
       mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
       mxSetFieldByNumber(registry, 0, fieldId, matlabName);
     }
-    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
+    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(registry);
-    
+
     mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
+    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(newAlreadyCreated);
   }
 }
diff --git a/tests/expected/matlab/inheritance_wrapper.cpp b/tests/expected/matlab/inheritance_wrapper.cpp
index 077df48305..8e61ac8c61 100644
--- a/tests/expected/matlab/inheritance_wrapper.cpp
+++ b/tests/expected/matlab/inheritance_wrapper.cpp
@@ -5,47 +5,11 @@
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/export.hpp>
 
-#include <folder/path/to/Test.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Point3.h>
-
-typedef Fun<double> FunDouble;
-typedef PrimitiveRef<double> PrimitiveRefDouble;
-typedef MyVector<3> MyVector3;
-typedef MyVector<12> MyVector12;
-typedef MultipleTemplates<int, double> MultipleTemplatesIntDouble;
-typedef MultipleTemplates<int, float> MultipleTemplatesIntFloat;
-typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
+
+
 typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
 typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
 
-BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2");
-BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3");
-
-typedef std::set<boost::shared_ptr<FunRange>*> Collector_FunRange;
-static Collector_FunRange collector_FunRange;
-typedef std::set<boost::shared_ptr<FunDouble>*> Collector_FunDouble;
-static Collector_FunDouble collector_FunDouble;
-typedef std::set<boost::shared_ptr<Test>*> Collector_Test;
-static Collector_Test collector_Test;
-typedef std::set<boost::shared_ptr<PrimitiveRefDouble>*> Collector_PrimitiveRefDouble;
-static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble;
-typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
-static Collector_MyVector3 collector_MyVector3;
-typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
-static Collector_MyVector12 collector_MyVector12;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_MultipleTemplatesIntDouble;
-static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
-static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
-typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
-static Collector_ForwardKinematics collector_ForwardKinematics;
-typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
-static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
-typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
-static Collector_gtsamPoint2 collector_gtsamPoint2;
-typedef std::set<boost::shared_ptr<gtsam::Point3>*> Collector_gtsamPoint3;
-static Collector_gtsamPoint3 collector_gtsamPoint3;
 typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase;
 static Collector_MyBase collector_MyBase;
 typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint2;
@@ -55,84 +19,13 @@ static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
 typedef std::set<boost::shared_ptr<ForwardKinematicsFactor>*> Collector_ForwardKinematicsFactor;
 static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor;
 
+
 void _deleteAllObjects()
 {
   mstream mout;
   std::streambuf *outbuf = std::cout.rdbuf(&mout);
 
   bool anyDeleted = false;
-  { for(Collector_FunRange::iterator iter = collector_FunRange.begin();
-      iter != collector_FunRange.end(); ) {
-    delete *iter;
-    collector_FunRange.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin();
-      iter != collector_FunDouble.end(); ) {
-    delete *iter;
-    collector_FunDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_Test::iterator iter = collector_Test.begin();
-      iter != collector_Test.end(); ) {
-    delete *iter;
-    collector_Test.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin();
-      iter != collector_PrimitiveRefDouble.end(); ) {
-    delete *iter;
-    collector_PrimitiveRefDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
-      iter != collector_MyVector3.end(); ) {
-    delete *iter;
-    collector_MyVector3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin();
-      iter != collector_MyVector12.end(); ) {
-    delete *iter;
-    collector_MyVector12.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin();
-      iter != collector_MultipleTemplatesIntDouble.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin();
-      iter != collector_MultipleTemplatesIntFloat.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntFloat.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
-      iter != collector_ForwardKinematics.end(); ) {
-    delete *iter;
-    collector_ForwardKinematics.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
-      iter != collector_MyFactorPosePoint2.end(); ) {
-    delete *iter;
-    collector_MyFactorPosePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin();
-      iter != collector_gtsamPoint2.end(); ) {
-    delete *iter;
-    collector_gtsamPoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin();
-      iter != collector_gtsamPoint3.end(); ) {
-    delete *iter;
-    collector_gtsamPoint3.erase(iter++);
-    anyDeleted = true;
-  } }
   { for(Collector_MyBase::iterator iter = collector_MyBase.begin();
       iter != collector_MyBase.end(); ) {
     delete *iter;
@@ -157,6 +50,7 @@ void _deleteAllObjects()
     collector_ForwardKinematicsFactor.erase(iter++);
     anyDeleted = true;
   } }
+
   if(anyDeleted)
     cout <<
       "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
@@ -169,49 +63,54 @@ void _inheritance_RTTIRegister() {
   const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_inheritance_rttiRegistry_created");
   if(!alreadyCreated) {
     std::map<std::string, std::string> types;
+
     types.insert(std::make_pair(typeid(MyBase).name(), "MyBase"));
     types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2"));
     types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix"));
     types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor"));
 
+
     mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
     if(!registry)
       registry = mxCreateStructMatrix(1, 1, 0, NULL);
     typedef std::pair<std::string, std::string> StringPair;
     for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-      if(fieldId < 0)
+      if(fieldId < 0) {
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+      }
       mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
       mxSetFieldByNumber(registry, 0, fieldId, matlabName);
     }
-    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
+    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(registry);
-    
+
     mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
+    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(newAlreadyCreated);
   }
 }
 
-void gtsamPoint2_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
+  typedef boost::shared_ptr<MyBase> Shared;
 
   Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_gtsamPoint2.insert(self);
+  collector_MyBase.insert(self);
 }
 
-void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
+void MyBase_upcastFromVoid_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyBase> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_MyBase.insert(self);
+  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  Shared *self = new Shared(boost::static_pointer_cast<MyBase>(*asVoid));
+  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
 void MyBase_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -227,19 +126,6 @@ void MyBase_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArr
   }
 }
 
-void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
-  checkArguments("delete_gtsamPoint2",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_gtsamPoint2::iterator item;
-  item = collector_gtsamPoint2.find(self);
-  if(item != collector_gtsamPoint2.end()) {
-    delete self;
-    collector_gtsamPoint2.erase(item);
-  }
-}
-
 void MyTemplatePoint2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
@@ -253,6 +139,15 @@ void MyTemplatePoint2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[],
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
+void MyTemplatePoint2_upcastFromVoid_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyTemplate<gtsam::Point2>> Shared;
+  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  Shared *self = new Shared(boost::static_pointer_cast<MyTemplate<gtsam::Point2>>(*asVoid));
+  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
+}
+
 void MyTemplatePoint2_constructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
@@ -399,20 +294,6 @@ void MyTemplatePoint2_Level_18(int nargout, mxArray *out[], int nargin, const mx
   out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Point2>>(MyTemplate<gtsam::Point2>::Level(K)),"MyTemplatePoint2", false);
 }
 
-void gtsamPoint3_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<gtsam::Point3> Shared;
-
-  double x = unwrap< double >(in[0]);
-  double y = unwrap< double >(in[1]);
-  double z = unwrap< double >(in[2]);
-  Shared *self = new Shared(new gtsam::Point3(x,y,z));
-  collector_gtsamPoint3.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
 void MyTemplateMatrix_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
@@ -426,6 +307,15 @@ void MyTemplateMatrix_collectorInsertAndMakeBase_19(int nargout, mxArray *out[],
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
+void MyTemplateMatrix_upcastFromVoid_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
+  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  Shared *self = new Shared(boost::static_pointer_cast<MyTemplate<gtsam::Matrix>>(*asVoid));
+  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
+}
+
 void MyTemplateMatrix_constructor_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
@@ -572,14 +462,6 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx
   out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
 }
 
-void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_vector2",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  Vector value = unwrap< Vector >(in[1]);
-  out[0] = wrap< Vector >(obj->return_vector2(value));
-}
-
 void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
@@ -593,6 +475,15 @@ void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
+void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
+  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  Shared *self = new Shared(boost::static_pointer_cast<ForwardKinematicsFactor>(*asVoid));
+  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
+}
+
 void ForwardKinematicsFactor_deconstructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
@@ -619,19 +510,19 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
   try {
     switch(id) {
     case 0:
-      gtsamPoint2_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1);
+      MyBase_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1);
       break;
     case 1:
-      MyBase_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1);
+      MyBase_upcastFromVoid_1(nargout, out, nargin-1, in+1);
       break;
     case 2:
       MyBase_deconstructor_2(nargout, out, nargin-1, in+1);
       break;
     case 3:
-      gtsamPoint2_deconstructor_3(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1);
       break;
     case 4:
-      MyTemplatePoint2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_upcastFromVoid_4(nargout, out, nargin-1, in+1);
       break;
     case 5:
       MyTemplatePoint2_constructor_5(nargout, out, nargin-1, in+1);
@@ -676,10 +567,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplatePoint2_Level_18(nargout, out, nargin-1, in+1);
       break;
     case 19:
-      gtsamPoint3_constructor_19(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1);
       break;
     case 20:
-      MyTemplateMatrix_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_upcastFromVoid_20(nargout, out, nargin-1, in+1);
       break;
     case 21:
       MyTemplateMatrix_constructor_21(nargout, out, nargin-1, in+1);
@@ -724,10 +615,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1);
       break;
     case 35:
-      Test_return_vector2_35(nargout, out, nargin-1, in+1);
+      ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
       break;
     case 36:
-      ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
+      ForwardKinematicsFactor_upcastFromVoid_36(nargout, out, nargin-1, in+1);
       break;
     case 37:
       ForwardKinematicsFactor_deconstructor_37(nargout, out, nargin-1, in+1);
diff --git a/tests/expected/matlab/multiple_files_wrapper.cpp b/tests/expected/matlab/multiple_files_wrapper.cpp
new file mode 100644
index 0000000000..66ab7ff73d
--- /dev/null
+++ b/tests/expected/matlab/multiple_files_wrapper.cpp
@@ -0,0 +1,229 @@
+#include <gtwrap/matlab.h>
+#include <map>
+
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/serialization/export.hpp>
+
+
+
+
+
+typedef std::set<boost::shared_ptr<gtsam::Class1>*> Collector_gtsamClass1;
+static Collector_gtsamClass1 collector_gtsamClass1;
+typedef std::set<boost::shared_ptr<gtsam::Class2>*> Collector_gtsamClass2;
+static Collector_gtsamClass2 collector_gtsamClass2;
+typedef std::set<boost::shared_ptr<gtsam::ClassA>*> Collector_gtsamClassA;
+static Collector_gtsamClassA collector_gtsamClassA;
+
+
+void _deleteAllObjects()
+{
+  mstream mout;
+  std::streambuf *outbuf = std::cout.rdbuf(&mout);
+
+  bool anyDeleted = false;
+  { for(Collector_gtsamClass1::iterator iter = collector_gtsamClass1.begin();
+      iter != collector_gtsamClass1.end(); ) {
+    delete *iter;
+    collector_gtsamClass1.erase(iter++);
+    anyDeleted = true;
+  } }
+  { for(Collector_gtsamClass2::iterator iter = collector_gtsamClass2.begin();
+      iter != collector_gtsamClass2.end(); ) {
+    delete *iter;
+    collector_gtsamClass2.erase(iter++);
+    anyDeleted = true;
+  } }
+  { for(Collector_gtsamClassA::iterator iter = collector_gtsamClassA.begin();
+      iter != collector_gtsamClassA.end(); ) {
+    delete *iter;
+    collector_gtsamClassA.erase(iter++);
+    anyDeleted = true;
+  } }
+
+  if(anyDeleted)
+    cout <<
+      "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
+      "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n"
+      "module, so that your recompiled module is used instead of the old one." << endl;
+  std::cout.rdbuf(outbuf);
+}
+
+void _multiple_files_RTTIRegister() {
+  const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_multiple_files_rttiRegistry_created");
+  if(!alreadyCreated) {
+    std::map<std::string, std::string> types;
+
+
+
+    mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
+    if(!registry)
+      registry = mxCreateStructMatrix(1, 1, 0, NULL);
+    typedef std::pair<std::string, std::string> StringPair;
+    for(const StringPair& rtti_matlab: types) {
+      int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
+      if(fieldId < 0) {
+        mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+      }
+      mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
+      mxSetFieldByNumber(registry, 0, fieldId, matlabName);
+    }
+    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {
+      mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
+    mxDestroyArray(registry);
+
+    mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
+    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {
+      mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
+    mxDestroyArray(newAlreadyCreated);
+  }
+}
+
+void gtsamClass1_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Class1> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_gtsamClass1.insert(self);
+}
+
+void gtsamClass1_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Class1> Shared;
+
+  Shared *self = new Shared(new gtsam::Class1());
+  collector_gtsamClass1.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void gtsamClass1_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<gtsam::Class1> Shared;
+  checkArguments("delete_gtsamClass1",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_gtsamClass1::iterator item;
+  item = collector_gtsamClass1.find(self);
+  if(item != collector_gtsamClass1.end()) {
+    delete self;
+    collector_gtsamClass1.erase(item);
+  }
+}
+
+void gtsamClass2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Class2> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_gtsamClass2.insert(self);
+}
+
+void gtsamClass2_constructor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Class2> Shared;
+
+  Shared *self = new Shared(new gtsam::Class2());
+  collector_gtsamClass2.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void gtsamClass2_deconstructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<gtsam::Class2> Shared;
+  checkArguments("delete_gtsamClass2",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_gtsamClass2::iterator item;
+  item = collector_gtsamClass2.find(self);
+  if(item != collector_gtsamClass2.end()) {
+    delete self;
+    collector_gtsamClass2.erase(item);
+  }
+}
+
+void gtsamClassA_collectorInsertAndMakeBase_6(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::ClassA> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_gtsamClassA.insert(self);
+}
+
+void gtsamClassA_constructor_7(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::ClassA> Shared;
+
+  Shared *self = new Shared(new gtsam::ClassA());
+  collector_gtsamClassA.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void gtsamClassA_deconstructor_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<gtsam::ClassA> Shared;
+  checkArguments("delete_gtsamClassA",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_gtsamClassA::iterator item;
+  item = collector_gtsamClassA.find(self);
+  if(item != collector_gtsamClassA.end()) {
+    delete self;
+    collector_gtsamClassA.erase(item);
+  }
+}
+
+
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mstream mout;
+  std::streambuf *outbuf = std::cout.rdbuf(&mout);
+
+  _multiple_files_RTTIRegister();
+
+  int id = unwrap<int>(in[0]);
+
+  try {
+    switch(id) {
+    case 0:
+      gtsamClass1_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1);
+      break;
+    case 1:
+      gtsamClass1_constructor_1(nargout, out, nargin-1, in+1);
+      break;
+    case 2:
+      gtsamClass1_deconstructor_2(nargout, out, nargin-1, in+1);
+      break;
+    case 3:
+      gtsamClass2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1);
+      break;
+    case 4:
+      gtsamClass2_constructor_4(nargout, out, nargin-1, in+1);
+      break;
+    case 5:
+      gtsamClass2_deconstructor_5(nargout, out, nargin-1, in+1);
+      break;
+    case 6:
+      gtsamClassA_collectorInsertAndMakeBase_6(nargout, out, nargin-1, in+1);
+      break;
+    case 7:
+      gtsamClassA_constructor_7(nargout, out, nargin-1, in+1);
+      break;
+    case 8:
+      gtsamClassA_deconstructor_8(nargout, out, nargin-1, in+1);
+      break;
+    }
+  } catch(const std::exception& e) {
+    mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str());
+  }
+
+  std::cout.rdbuf(outbuf);
+}
diff --git a/tests/expected/matlab/namespaces_wrapper.cpp b/tests/expected/matlab/namespaces_wrapper.cpp
index 8f6e415e2d..604ede5da5 100644
--- a/tests/expected/matlab/namespaces_wrapper.cpp
+++ b/tests/expected/matlab/namespaces_wrapper.cpp
@@ -5,9 +5,6 @@
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/export.hpp>
 
-#include <folder/path/to/Test.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/nonlinear/Values.h>
 #include <path/to/ns1.h>
 #include <path/to/ns1/ClassB.h>
@@ -15,51 +12,8 @@
 #include <path/to/ns2/ClassA.h>
 #include <path/to/ns3.h>
 
-typedef Fun<double> FunDouble;
-typedef PrimitiveRef<double> PrimitiveRefDouble;
-typedef MyVector<3> MyVector3;
-typedef MyVector<12> MyVector12;
-typedef MultipleTemplates<int, double> MultipleTemplatesIntDouble;
-typedef MultipleTemplates<int, float> MultipleTemplatesIntFloat;
-typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
-typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
-typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
-
-BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2");
-BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3");
-
-typedef std::set<boost::shared_ptr<FunRange>*> Collector_FunRange;
-static Collector_FunRange collector_FunRange;
-typedef std::set<boost::shared_ptr<FunDouble>*> Collector_FunDouble;
-static Collector_FunDouble collector_FunDouble;
-typedef std::set<boost::shared_ptr<Test>*> Collector_Test;
-static Collector_Test collector_Test;
-typedef std::set<boost::shared_ptr<PrimitiveRefDouble>*> Collector_PrimitiveRefDouble;
-static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble;
-typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
-static Collector_MyVector3 collector_MyVector3;
-typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
-static Collector_MyVector12 collector_MyVector12;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_MultipleTemplatesIntDouble;
-static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
-static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
-typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
-static Collector_ForwardKinematics collector_ForwardKinematics;
-typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
-static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
-typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
-static Collector_gtsamPoint2 collector_gtsamPoint2;
-typedef std::set<boost::shared_ptr<gtsam::Point3>*> Collector_gtsamPoint3;
-static Collector_gtsamPoint3 collector_gtsamPoint3;
-typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase;
-static Collector_MyBase collector_MyBase;
-typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint2;
-static Collector_MyTemplatePoint2 collector_MyTemplatePoint2;
-typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix;
-static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
-typedef std::set<boost::shared_ptr<ForwardKinematicsFactor>*> Collector_ForwardKinematicsFactor;
-static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor;
+
+
 typedef std::set<boost::shared_ptr<ns1::ClassA>*> Collector_ns1ClassA;
 static Collector_ns1ClassA collector_ns1ClassA;
 typedef std::set<boost::shared_ptr<ns1::ClassB>*> Collector_ns1ClassB;
@@ -75,108 +29,13 @@ static Collector_ClassD collector_ClassD;
 typedef std::set<boost::shared_ptr<gtsam::Values>*> Collector_gtsamValues;
 static Collector_gtsamValues collector_gtsamValues;
 
+
 void _deleteAllObjects()
 {
   mstream mout;
   std::streambuf *outbuf = std::cout.rdbuf(&mout);
 
   bool anyDeleted = false;
-  { for(Collector_FunRange::iterator iter = collector_FunRange.begin();
-      iter != collector_FunRange.end(); ) {
-    delete *iter;
-    collector_FunRange.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin();
-      iter != collector_FunDouble.end(); ) {
-    delete *iter;
-    collector_FunDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_Test::iterator iter = collector_Test.begin();
-      iter != collector_Test.end(); ) {
-    delete *iter;
-    collector_Test.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin();
-      iter != collector_PrimitiveRefDouble.end(); ) {
-    delete *iter;
-    collector_PrimitiveRefDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
-      iter != collector_MyVector3.end(); ) {
-    delete *iter;
-    collector_MyVector3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin();
-      iter != collector_MyVector12.end(); ) {
-    delete *iter;
-    collector_MyVector12.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin();
-      iter != collector_MultipleTemplatesIntDouble.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin();
-      iter != collector_MultipleTemplatesIntFloat.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntFloat.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
-      iter != collector_ForwardKinematics.end(); ) {
-    delete *iter;
-    collector_ForwardKinematics.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
-      iter != collector_MyFactorPosePoint2.end(); ) {
-    delete *iter;
-    collector_MyFactorPosePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin();
-      iter != collector_gtsamPoint2.end(); ) {
-    delete *iter;
-    collector_gtsamPoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin();
-      iter != collector_gtsamPoint3.end(); ) {
-    delete *iter;
-    collector_gtsamPoint3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyBase::iterator iter = collector_MyBase.begin();
-      iter != collector_MyBase.end(); ) {
-    delete *iter;
-    collector_MyBase.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin();
-      iter != collector_MyTemplatePoint2.end(); ) {
-    delete *iter;
-    collector_MyTemplatePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin();
-      iter != collector_MyTemplateMatrix.end(); ) {
-    delete *iter;
-    collector_MyTemplateMatrix.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin();
-      iter != collector_ForwardKinematicsFactor.end(); ) {
-    delete *iter;
-    collector_ForwardKinematicsFactor.erase(iter++);
-    anyDeleted = true;
-  } }
   { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin();
       iter != collector_ns1ClassA.end(); ) {
     delete *iter;
@@ -219,6 +78,7 @@ void _deleteAllObjects()
     collector_gtsamValues.erase(iter++);
     anyDeleted = true;
   } }
+
   if(anyDeleted)
     cout <<
       "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
@@ -231,10 +91,8 @@ void _namespaces_RTTIRegister() {
   const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_namespaces_rttiRegistry_created");
   if(!alreadyCreated) {
     std::map<std::string, std::string> types;
-    types.insert(std::make_pair(typeid(MyBase).name(), "MyBase"));
-    types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2"));
-    types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix"));
-    types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor"));
+
+
 
     mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
     if(!registry)
@@ -242,18 +100,21 @@ void _namespaces_RTTIRegister() {
     typedef std::pair<std::string, std::string> StringPair;
     for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-      if(fieldId < 0)
+      if(fieldId < 0) {
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+      }
       mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
       mxSetFieldByNumber(registry, 0, fieldId, matlabName);
     }
-    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
+    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(registry);
-    
+
     mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
+    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(newAlreadyCreated);
   }
 }
diff --git a/tests/expected/matlab/special_cases_wrapper.cpp b/tests/expected/matlab/special_cases_wrapper.cpp
index 056ce80973..69abbf73be 100644
--- a/tests/expected/matlab/special_cases_wrapper.cpp
+++ b/tests/expected/matlab/special_cases_wrapper.cpp
@@ -5,78 +5,11 @@
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/export.hpp>
 
-#include <folder/path/to/Test.h>
 #include <gtsam/geometry/Cal3Bundler.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Point3.h>
-#include <gtsam/nonlinear/Values.h>
-#include <path/to/ns1.h>
-#include <path/to/ns1/ClassB.h>
-#include <path/to/ns2.h>
-#include <path/to/ns2/ClassA.h>
-#include <path/to/ns3.h>
-
-typedef Fun<double> FunDouble;
-typedef PrimitiveRef<double> PrimitiveRefDouble;
-typedef MyVector<3> MyVector3;
-typedef MyVector<12> MyVector12;
-typedef MultipleTemplates<int, double> MultipleTemplatesIntDouble;
-typedef MultipleTemplates<int, float> MultipleTemplatesIntFloat;
-typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
-typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
-typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
+
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
 
-BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2");
-BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3");
-
-typedef std::set<boost::shared_ptr<FunRange>*> Collector_FunRange;
-static Collector_FunRange collector_FunRange;
-typedef std::set<boost::shared_ptr<FunDouble>*> Collector_FunDouble;
-static Collector_FunDouble collector_FunDouble;
-typedef std::set<boost::shared_ptr<Test>*> Collector_Test;
-static Collector_Test collector_Test;
-typedef std::set<boost::shared_ptr<PrimitiveRefDouble>*> Collector_PrimitiveRefDouble;
-static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble;
-typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
-static Collector_MyVector3 collector_MyVector3;
-typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
-static Collector_MyVector12 collector_MyVector12;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_MultipleTemplatesIntDouble;
-static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
-typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
-static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
-typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
-static Collector_ForwardKinematics collector_ForwardKinematics;
-typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
-static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
-typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
-static Collector_gtsamPoint2 collector_gtsamPoint2;
-typedef std::set<boost::shared_ptr<gtsam::Point3>*> Collector_gtsamPoint3;
-static Collector_gtsamPoint3 collector_gtsamPoint3;
-typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase;
-static Collector_MyBase collector_MyBase;
-typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint2;
-static Collector_MyTemplatePoint2 collector_MyTemplatePoint2;
-typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix;
-static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
-typedef std::set<boost::shared_ptr<ForwardKinematicsFactor>*> Collector_ForwardKinematicsFactor;
-static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor;
-typedef std::set<boost::shared_ptr<ns1::ClassA>*> Collector_ns1ClassA;
-static Collector_ns1ClassA collector_ns1ClassA;
-typedef std::set<boost::shared_ptr<ns1::ClassB>*> Collector_ns1ClassB;
-static Collector_ns1ClassB collector_ns1ClassB;
-typedef std::set<boost::shared_ptr<ns2::ClassA>*> Collector_ns2ClassA;
-static Collector_ns2ClassA collector_ns2ClassA;
-typedef std::set<boost::shared_ptr<ns2::ns3::ClassB>*> Collector_ns2ns3ClassB;
-static Collector_ns2ns3ClassB collector_ns2ns3ClassB;
-typedef std::set<boost::shared_ptr<ns2::ClassC>*> Collector_ns2ClassC;
-static Collector_ns2ClassC collector_ns2ClassC;
-typedef std::set<boost::shared_ptr<ClassD>*> Collector_ClassD;
-static Collector_ClassD collector_ClassD;
-typedef std::set<boost::shared_ptr<gtsam::Values>*> Collector_gtsamValues;
-static Collector_gtsamValues collector_gtsamValues;
 typedef std::set<boost::shared_ptr<gtsam::NonlinearFactorGraph>*> Collector_gtsamNonlinearFactorGraph;
 static Collector_gtsamNonlinearFactorGraph collector_gtsamNonlinearFactorGraph;
 typedef std::set<boost::shared_ptr<gtsam::SfmTrack>*> Collector_gtsamSfmTrack;
@@ -86,150 +19,13 @@ static Collector_gtsamPinholeCameraCal3Bundler collector_gtsamPinholeCameraCal3B
 typedef std::set<boost::shared_ptr<GeneralSFMFactorCal3Bundler>*> Collector_gtsamGeneralSFMFactorCal3Bundler;
 static Collector_gtsamGeneralSFMFactorCal3Bundler collector_gtsamGeneralSFMFactorCal3Bundler;
 
+
 void _deleteAllObjects()
 {
   mstream mout;
   std::streambuf *outbuf = std::cout.rdbuf(&mout);
 
   bool anyDeleted = false;
-  { for(Collector_FunRange::iterator iter = collector_FunRange.begin();
-      iter != collector_FunRange.end(); ) {
-    delete *iter;
-    collector_FunRange.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin();
-      iter != collector_FunDouble.end(); ) {
-    delete *iter;
-    collector_FunDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_Test::iterator iter = collector_Test.begin();
-      iter != collector_Test.end(); ) {
-    delete *iter;
-    collector_Test.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin();
-      iter != collector_PrimitiveRefDouble.end(); ) {
-    delete *iter;
-    collector_PrimitiveRefDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
-      iter != collector_MyVector3.end(); ) {
-    delete *iter;
-    collector_MyVector3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin();
-      iter != collector_MyVector12.end(); ) {
-    delete *iter;
-    collector_MyVector12.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin();
-      iter != collector_MultipleTemplatesIntDouble.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntDouble.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin();
-      iter != collector_MultipleTemplatesIntFloat.end(); ) {
-    delete *iter;
-    collector_MultipleTemplatesIntFloat.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
-      iter != collector_ForwardKinematics.end(); ) {
-    delete *iter;
-    collector_ForwardKinematics.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
-      iter != collector_MyFactorPosePoint2.end(); ) {
-    delete *iter;
-    collector_MyFactorPosePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin();
-      iter != collector_gtsamPoint2.end(); ) {
-    delete *iter;
-    collector_gtsamPoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin();
-      iter != collector_gtsamPoint3.end(); ) {
-    delete *iter;
-    collector_gtsamPoint3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyBase::iterator iter = collector_MyBase.begin();
-      iter != collector_MyBase.end(); ) {
-    delete *iter;
-    collector_MyBase.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin();
-      iter != collector_MyTemplatePoint2.end(); ) {
-    delete *iter;
-    collector_MyTemplatePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin();
-      iter != collector_MyTemplateMatrix.end(); ) {
-    delete *iter;
-    collector_MyTemplateMatrix.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin();
-      iter != collector_ForwardKinematicsFactor.end(); ) {
-    delete *iter;
-    collector_ForwardKinematicsFactor.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin();
-      iter != collector_ns1ClassA.end(); ) {
-    delete *iter;
-    collector_ns1ClassA.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin();
-      iter != collector_ns1ClassB.end(); ) {
-    delete *iter;
-    collector_ns1ClassB.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin();
-      iter != collector_ns2ClassA.end(); ) {
-    delete *iter;
-    collector_ns2ClassA.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin();
-      iter != collector_ns2ns3ClassB.end(); ) {
-    delete *iter;
-    collector_ns2ns3ClassB.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin();
-      iter != collector_ns2ClassC.end(); ) {
-    delete *iter;
-    collector_ns2ClassC.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ClassD::iterator iter = collector_ClassD.begin();
-      iter != collector_ClassD.end(); ) {
-    delete *iter;
-    collector_ClassD.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin();
-      iter != collector_gtsamValues.end(); ) {
-    delete *iter;
-    collector_gtsamValues.erase(iter++);
-    anyDeleted = true;
-  } }
   { for(Collector_gtsamNonlinearFactorGraph::iterator iter = collector_gtsamNonlinearFactorGraph.begin();
       iter != collector_gtsamNonlinearFactorGraph.end(); ) {
     delete *iter;
@@ -254,6 +50,7 @@ void _deleteAllObjects()
     collector_gtsamGeneralSFMFactorCal3Bundler.erase(iter++);
     anyDeleted = true;
   } }
+
   if(anyDeleted)
     cout <<
       "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
@@ -266,10 +63,8 @@ void _special_cases_RTTIRegister() {
   const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_special_cases_rttiRegistry_created");
   if(!alreadyCreated) {
     std::map<std::string, std::string> types;
-    types.insert(std::make_pair(typeid(MyBase).name(), "MyBase"));
-    types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2"));
-    types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix"));
-    types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor"));
+
+
 
     mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
     if(!registry)
@@ -277,18 +72,21 @@ void _special_cases_RTTIRegister() {
     typedef std::pair<std::string, std::string> StringPair;
     for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-      if(fieldId < 0)
+      if(fieldId < 0) {
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+      }
       mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
       mxSetFieldByNumber(registry, 0, fieldId, matlabName);
     }
-    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
+    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(registry);
-    
+
     mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
+    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {
       mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+    }
     mxDestroyArray(newAlreadyCreated);
   }
 }
diff --git a/tests/fixtures/part1.i b/tests/fixtures/part1.i
new file mode 100644
index 0000000000..b69850baff
--- /dev/null
+++ b/tests/fixtures/part1.i
@@ -0,0 +1,11 @@
+// First file to test for multi-file support.
+
+namespace gtsam {
+class Class1 {
+  Class1();
+};
+
+class Class2 {
+  Class2();
+};
+}  // namespace gtsam
\ No newline at end of file
diff --git a/tests/fixtures/part2.i b/tests/fixtures/part2.i
new file mode 100644
index 0000000000..29ad86a7f8
--- /dev/null
+++ b/tests/fixtures/part2.i
@@ -0,0 +1,7 @@
+// Second file to test for multi-file support.
+
+namespace gtsam {
+class ClassA {
+  ClassA();
+};
+}  // namespace gtsam
\ No newline at end of file
diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py
index b321c4e151..fad4de16a6 100644
--- a/tests/test_matlab_wrapper.py
+++ b/tests/test_matlab_wrapper.py
@@ -22,73 +22,31 @@ class TestWrap(unittest.TestCase):
     """
     Test the Matlab wrapper
     """
-    TEST_DIR = osp.dirname(osp.realpath(__file__))
-    INTERFACE_DIR = osp.join(TEST_DIR, "fixtures")
-    MATLAB_TEST_DIR = osp.join(TEST_DIR, "expected", "matlab")
-    MATLAB_ACTUAL_DIR = osp.join(TEST_DIR, "actual", "matlab")
-
-    # Create the `actual/matlab` directory
-    os.makedirs(MATLAB_ACTUAL_DIR, exist_ok=True)
-
-    # set the log level to INFO by default
-    logger.remove()  # remove the default sink
-    logger.add(sys.stderr, format="{time} {level} {message}", level="INFO")
-
-    def generate_content(self, cc_content, path=MATLAB_ACTUAL_DIR):
-        """Generate files and folders from matlab wrapper content.
-
-        Keyword arguments:
-        cc_content -- the content to generate formatted as
-            (file_name, file_content) or
-            (folder_name, [(file_name, file_content)])
-        path -- the path to the files parent folder within the main folder
-        """
-        for c in cc_content:
-            if isinstance(c, list):
-                if len(c) == 0:
-                    continue
-                logger.debug("c object: {}".format(c[0][0]))
-                path_to_folder = osp.join(path, c[0][0])
-
-                if not osp.isdir(path_to_folder):
-                    try:
-                        os.makedirs(path_to_folder, exist_ok=True)
-                    except OSError:
-                        pass
-
-                for sub_content in c:
-                    logger.debug("sub object: {}".format(sub_content[1][0][0]))
-                    self.generate_content(sub_content[1], path_to_folder)
-
-            elif isinstance(c[1], list):
-                path_to_folder = osp.join(path, c[0])
-
-                logger.debug(
-                    "[generate_content_global]: {}".format(path_to_folder))
-                if not osp.isdir(path_to_folder):
-                    try:
-                        os.makedirs(path_to_folder, exist_ok=True)
-                    except OSError:
-                        pass
-                for sub_content in c[1]:
-                    path_to_file = osp.join(path_to_folder, sub_content[0])
-                    logger.debug(
-                        "[generate_global_method]: {}".format(path_to_file))
-                    with open(path_to_file, 'w') as f:
-                        f.write(sub_content[1])
-
-            else:
-                path_to_file = osp.join(path, c[0])
-
-                logger.debug("[generate_content]: {}".format(path_to_file))
-                if not osp.isdir(path_to_file):
-                    try:
-                        os.mkdir(path)
-                    except OSError:
-                        pass
-
-                with open(path_to_file, 'w') as f:
-                    f.write(c[1])
+    def setUp(self) -> None:
+        super().setUp()
+
+        # Set up all the directories
+        self.TEST_DIR = osp.dirname(osp.realpath(__file__))
+        self.INTERFACE_DIR = osp.join(self.TEST_DIR, "fixtures")
+        self.MATLAB_TEST_DIR = osp.join(self.TEST_DIR, "expected", "matlab")
+        self.MATLAB_ACTUAL_DIR = osp.join(self.TEST_DIR, "actual", "matlab")
+
+        if not osp.exists(self.MATLAB_ACTUAL_DIR):
+            os.mkdir(self.MATLAB_ACTUAL_DIR)
+
+        # Generate the matlab.h file if it does not exist
+        template_file = osp.join(self.TEST_DIR, "..", "gtwrap",
+                                 "matlab_wrapper", "matlab_wrapper.tpl")
+        if not osp.exists(template_file):
+            with open(template_file, 'w') as tpl:
+                tpl.write("#include <gtwrap/matlab.h>\n#include <map>\n")
+
+        # Create the `actual/matlab` directory
+        os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True)
+
+        # set the log level to INFO by default
+        logger.remove()  # remove the default sink
+        logger.add(sys.stderr, format="{time} {level} {message}", level="INFO")
 
     def compare_and_diff(self, file):
         """
@@ -109,11 +67,7 @@ def test_geometry(self):
         python3 wrap/matlab_wrapper.py --src wrap/tests/geometry.h
             --module_name geometry --out wrap/tests/actual-matlab
         """
-        with open(osp.join(self.INTERFACE_DIR, 'geometry.i'), 'r') as f:
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
+        file = osp.join(self.INTERFACE_DIR, 'geometry.i')
 
         # Create MATLAB wrapper instance
         wrapper = MatlabWrapper(
@@ -122,24 +76,18 @@ def test_geometry(self):
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap(content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
-        self.generate_content(cc_content)
+        files = ['+gtsam/Point2.m', '+gtsam/Point3.m', 'geometry_wrapper.cpp']
 
         self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam')))
 
-        files = ['+gtsam/Point2.m', '+gtsam/Point3.m', 'geometry_wrapper.cpp']
-
         for file in files:
             self.compare_and_diff(file)
 
     def test_functions(self):
         """Test interface file with function info."""
-        with open(osp.join(self.INTERFACE_DIR, 'functions.i'), 'r') as f:
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
+        file = osp.join(self.INTERFACE_DIR, 'functions.i')
 
         wrapper = MatlabWrapper(
             module_name='functions',
@@ -147,9 +95,7 @@ def test_functions(self):
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap(content)
-
-        self.generate_content(cc_content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
             'functions_wrapper.cpp', 'aGlobalFunction.m', 'load2D.m',
@@ -163,11 +109,7 @@ def test_functions(self):
 
     def test_class(self):
         """Test interface file with only class info."""
-        with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f:
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
+        file = osp.join(self.INTERFACE_DIR, 'class.i')
 
         wrapper = MatlabWrapper(
             module_name='class',
@@ -175,9 +117,7 @@ def test_class(self):
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap(content)
-
-        self.generate_content(cc_content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
             'class_wrapper.cpp', 'FunDouble.m', 'FunRange.m',
@@ -191,21 +131,14 @@ def test_class(self):
 
     def test_inheritance(self):
         """Test interface file with class inheritance definitions."""
-        with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f:
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
+        file = osp.join(self.INTERFACE_DIR, 'inheritance.i')
 
         wrapper = MatlabWrapper(
             module_name='inheritance',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
-
-        cc_content = wrapper.wrap(content)
-
-        self.generate_content(cc_content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
             'inheritance_wrapper.cpp', 'MyBase.m', 'MyTemplateMatrix.m',
@@ -219,11 +152,7 @@ def test_namespaces(self):
         """
         Test interface file with full namespace definition.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f:
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
+        file = osp.join(self.INTERFACE_DIR, 'namespaces.i')
 
         wrapper = MatlabWrapper(
             module_name='namespaces',
@@ -231,9 +160,7 @@ def test_namespaces(self):
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap(content)
-
-        self.generate_content(cc_content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
             'namespaces_wrapper.cpp', '+ns1/aGlobalFunction.m',
@@ -249,21 +176,14 @@ def test_special_cases(self):
         """
         Tests for some unique, non-trivial features.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f:
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
+        file = osp.join(self.INTERFACE_DIR, 'special_cases.i')
 
         wrapper = MatlabWrapper(
             module_name='special_cases',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
-
-        cc_content = wrapper.wrap(content)
-
-        self.generate_content(cc_content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
             'special_cases_wrapper.cpp',
@@ -274,6 +194,31 @@ def test_special_cases(self):
         for file in files:
             self.compare_and_diff(file)
 
+    def test_multiple_files(self):
+        """
+        Test for when multiple interface files are specified.
+        """
+        file1 = osp.join(self.INTERFACE_DIR, 'part1.i')
+        file2 = osp.join(self.INTERFACE_DIR, 'part2.i')
+
+        wrapper = MatlabWrapper(
+            module_name='multiple_files',
+            top_module_namespace=['gtsam'],
+            ignore_classes=[''],
+        )
+
+        wrapper.wrap([file1, file2], path=self.MATLAB_ACTUAL_DIR)
+
+        files = [
+            'multiple_files_wrapper.cpp',
+            '+gtsam/Class1.m',
+            '+gtsam/Class2.m',
+            '+gtsam/ClassA.m',
+        ]
+
+        for file in files:
+            self.compare_and_diff(file)
+
 
 if __name__ == '__main__':
     unittest.main()
diff --git a/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py
index 77c884b622..67c637d146 100644
--- a/tests/test_pybind_wrapper.py
+++ b/tests/test_pybind_wrapper.py
@@ -31,9 +31,9 @@ class TestWrap(unittest.TestCase):
     # Create the `actual/python` directory
     os.makedirs(PYTHON_ACTUAL_DIR, exist_ok=True)
 
-    def wrap_content(self, content, module_name, output_dir):
+    def wrap_content(self, sources, module_name, output_dir):
         """
-        Common function to wrap content.
+        Common function to wrap content in `sources`.
         """
         with open(osp.join(self.TEST_DIR,
                            "pybind_wrapper.tpl")) as template_file:
@@ -46,15 +46,12 @@ def wrap_content(self, content, module_name, output_dir):
                                 ignore_classes=[''],
                                 module_template=module_template)
 
-        cc_content = wrapper.wrap(content)
-
         output = osp.join(self.TEST_DIR, output_dir, module_name + ".cpp")
 
         if not osp.exists(osp.join(self.TEST_DIR, output_dir)):
             os.mkdir(osp.join(self.TEST_DIR, output_dir))
 
-        with open(output, 'w') as f:
-            f.write(cc_content)
+        wrapper.wrap(sources, output)
 
         return output
 
@@ -76,39 +73,32 @@ def test_geometry(self):
         python3 ../pybind_wrapper.py --src geometry.h --module_name
             geometry_py --out output/geometry_py.cc
         """
-        with open(osp.join(self.INTERFACE_DIR, 'geometry.i'), 'r') as f:
-            content = f.read()
-
-        output = self.wrap_content(content, 'geometry_py',
+        source = osp.join(self.INTERFACE_DIR, 'geometry.i')
+        output = self.wrap_content([source], 'geometry_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('geometry_pybind.cpp', output)
 
     def test_functions(self):
         """Test interface file with function info."""
-        with open(osp.join(self.INTERFACE_DIR, 'functions.i'), 'r') as f:
-            content = f.read()
-
-        output = self.wrap_content(content, 'functions_py',
+        source = osp.join(self.INTERFACE_DIR, 'functions.i')
+        output = self.wrap_content([source], 'functions_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('functions_pybind.cpp', output)
 
     def test_class(self):
         """Test interface file with only class info."""
-        with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f:
-            content = f.read()
-
-        output = self.wrap_content(content, 'class_py', self.PYTHON_ACTUAL_DIR)
+        source = osp.join(self.INTERFACE_DIR, 'class.i')
+        output = self.wrap_content([source], 'class_py',
+                                   self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('class_pybind.cpp', output)
 
     def test_inheritance(self):
         """Test interface file with class inheritance definitions."""
-        with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f:
-            content = f.read()
-
-        output = self.wrap_content(content, 'inheritance_py',
+        source = osp.join(self.INTERFACE_DIR, 'inheritance.i')
+        output = self.wrap_content([source], 'inheritance_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('inheritance_pybind.cpp', output)
@@ -119,10 +109,8 @@ def test_namespaces(self):
         python3 ../pybind_wrapper.py --src namespaces.i --module_name
             namespaces_py --out output/namespaces_py.cpp
         """
-        with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f:
-            content = f.read()
-
-        output = self.wrap_content(content, 'namespaces_py',
+        source = osp.join(self.INTERFACE_DIR, 'namespaces.i')
+        output = self.wrap_content([source], 'namespaces_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('namespaces_pybind.cpp', output)
@@ -131,10 +119,8 @@ def test_operator_overload(self):
         """
         Tests for operator overloading.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'operator.i'), 'r') as f:
-            content = f.read()
-
-        output = self.wrap_content(content, 'operator_py',
+        source = osp.join(self.INTERFACE_DIR, 'operator.i')
+        output = self.wrap_content([source], 'operator_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('operator_pybind.cpp', output)
@@ -143,10 +129,8 @@ def test_special_cases(self):
         """
         Tests for some unique, non-trivial features.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f:
-            content = f.read()
-
-        output = self.wrap_content(content, 'special_cases_py',
+        source = osp.join(self.INTERFACE_DIR, 'special_cases.i')
+        output = self.wrap_content([source], 'special_cases_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('special_cases_pybind.cpp', output)
@@ -155,10 +139,8 @@ def test_enum(self):
         """
         Test if enum generation is correct.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f:
-            content = f.read()
-
-        output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR)
+        source = osp.join(self.INTERFACE_DIR, 'enum.i')
+        output = self.wrap_content([source], 'enum_py', self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('enum_pybind.cpp', output)
 

From 61c5e89de3bf628dccd98e95c0f955bf908ca8a9 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sun, 11 Jul 2021 22:46:54 -0400
Subject: [PATCH 55/72] try increasing pmax to pass test

---
 python/gtsam/tests/test_ShonanAveraging.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index 69e7055453..d07d840991 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -193,7 +193,8 @@ def test_constructorBetweenFactorPose2s(self) -> None:
         
         obj = ShonanAveraging2(between_factors, shonan_params)
         initial = obj.initializeRandomly()
-        result_values, _ = obj.run(initial, min_p=2, max_p=10)
+        min
+        result_values, _ = obj.run(initial, min_p=2, max_p=40)
         
         for i in range(num_images):
             wRi = result_values.atRot2(i)

From 690300124c5f7a162601298fca115c8f01cc531a Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sun, 11 Jul 2021 22:47:14 -0400
Subject: [PATCH 56/72] fix typo

---
 python/gtsam/tests/test_ShonanAveraging.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index d07d840991..c109ce6b0a 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -193,7 +193,6 @@ def test_constructorBetweenFactorPose2s(self) -> None:
         
         obj = ShonanAveraging2(between_factors, shonan_params)
         initial = obj.initializeRandomly()
-        min
         result_values, _ = obj.run(initial, min_p=2, max_p=40)
         
         for i in range(num_images):

From 36c2aa1e56a11b4a99acf855258413d0230b08aa Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 12 Jul 2021 11:46:31 -0400
Subject: [PATCH 57/72] matlab wrapper header update

---
 wrap/matlab.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/wrap/matlab.h b/wrap/matlab.h
index 4f3bfe96e3..bcdef3c8dd 100644
--- a/wrap/matlab.h
+++ b/wrap/matlab.h
@@ -26,6 +26,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/base/utilities.h>
 
 using gtsam::Vector;
 using gtsam::Matrix;

From cce952fbb3fc266e89bd2599ec58de7fb92622be Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Mon, 12 Jul 2021 19:35:34 -0400
Subject: [PATCH 58/72] use simple example for unit test

---
 python/gtsam/tests/test_ShonanAveraging.py | 84 +++++++++++-----------
 1 file changed, 43 insertions(+), 41 deletions(-)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index c109ce6b0a..08bbe5191d 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -22,12 +22,13 @@
     ShonanAveraging2,
     ShonanAveragingParameters2,
     ShonanAveraging3,
-    ShonanAveragingParameters3
+    ShonanAveragingParameters3,
 )
 from gtsam.utils.test_case import GtsamTestCase
 
 DEFAULT_PARAMS = ShonanAveragingParameters3(
-    gtsam.LevenbergMarquardtParams.CeresDefaults())
+    gtsam.LevenbergMarquardtParams.CeresDefaults()
+)
 
 
 def fromExampleName(
@@ -146,58 +147,59 @@ def test_PriorWeights(self):
         result, _lambdaMin = shonan.run(initial, 3, 3)
         self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
     
+
     def test_constructorBetweenFactorPose2s(self) -> None:
-        """Check if ShonanAveraging2 constructor works when not initialized from g2o file."""
-        num_images = 11
-        # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1
+        """Check if ShonanAveraging2 constructor works when not initialized from g2o file.
+
+        GT pose graph:
+
+           | cam 1 = (0,4)
+         --o
+           | .
+           .   .
+           .     .
+           |       |
+           o-- ... o--
+        cam 0       cam 2 = (4,0)
+          (0,0)
+        """
+        num_images = 3
+
+        wTi_list = [
+            Pose2(Rot2.fromDegrees(0), np.array([0, 0])),
+            Pose2(Rot2.fromDegrees(90), np.array([0, 4])),
+            Pose2(Rot2.fromDegrees(0), np.array([4, 0])),
+        ]
+
+        edges = [(0, 1), (1, 2), (0, 2)]
         i2Ri1_dict = {
-            (1, 2):  -43.86342,
-            (1, 5):  -135.06351,
-            (2, 4):  72.64527,
-            (3, 5):  117.75967,
-            (6, 7):  -31.73934,
-            (7, 10): 177.45901,
-            (6, 9):  -133.58713,
-            (7, 8):  -90.58668,
-            (0, 3):  127.02978,
-            (8, 10): -92.16361,
-            (4, 8):  51.63781,
-            (4, 6):  173.96384,
-            (4, 10): 139.59445,
-            (2, 3):  151.04022,
-            (3, 4):  -78.39495,
-            (1, 4):  28.78185,
-            (6, 8):  -122.32602,
-            (0, 2):  -24.01045,
-            (5, 7):  -53.93014,
-            (4, 5):  -163.84535,
-            (2, 5):  -91.20009,
-            (1, 3):  107.17680,
-            (7, 9):  -102.35615,
-            (0, 1):  19.85297,
-            (5, 8):  -144.51682,
-            (5, 6):  -22.19079,
-            (5, 10): -56.56016,
+            (i1, i2): wTi_list[i2].inverse().compose(wTi_list[i1]).rotation()
+            for (i1, i2) in edges
         }
-        i2Ri1_dict = {(i1,i2): Rot2.fromDegrees(theta_deg) for (i1,i2), theta_deg in i2Ri1_dict.items()}
+
         lm_params = LevenbergMarquardtParams.CeresDefaults()
         shonan_params = ShonanAveragingParameters2(lm_params)
         shonan_params.setUseHuber(False)
         shonan_params.setCertifyOptimality(True)
-        
+
         noise_model = gtsam.noiseModel.Unit.Create(3)
         between_factors = gtsam.BetweenFactorPose2s()
         for (i1, i2), i2Ri1 in i2Ri1_dict.items():
             i2Ti1 = Pose2(i2Ri1, np.zeros(2))
-            between_factors.append(BetweenFactorPose2(i2, i1, i2Ti1, noise_model))
-        
+            between_factors.append(
+                BetweenFactorPose2(i2, i1, i2Ti1, noise_model)
+            )
+
         obj = ShonanAveraging2(between_factors, shonan_params)
         initial = obj.initializeRandomly()
-        result_values, _ = obj.run(initial, min_p=2, max_p=40)
-        
-        for i in range(num_images):
-            wRi = result_values.atRot2(i)
+        result_values, _ = obj.run(initial, min_p=2, max_p=100)
+
+        wRi_list = [result_values.atRot2(i) for i in range(num_images)]
+        thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
+        thetas_deg -= max(thetas_deg)
+        expected_thetas_deg = np.array([0.0, -270.0, 0.0])
+        np.testing.assert_allclose(thetas_deg, expected_thetas_deg)
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     unittest.main()

From d7151ed28410e57526cf5286d820fc4ae089cf05 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Mon, 12 Jul 2021 20:52:36 -0400
Subject: [PATCH 59/72] use mod when comparing angles

---
 python/gtsam/tests/test_ShonanAveraging.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index 08bbe5191d..8c70df5dff 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -197,7 +197,9 @@ def test_constructorBetweenFactorPose2s(self) -> None:
         wRi_list = [result_values.atRot2(i) for i in range(num_images)]
         thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
         thetas_deg -= max(thetas_deg)
-        expected_thetas_deg = np.array([0.0, -270.0, 0.0])
+        # map all angles to [0,360)
+        thetas_deg = thetas_deg % 360
+        expected_thetas_deg = np.array([0.0, 90.0, 0.0])
         np.testing.assert_allclose(thetas_deg, expected_thetas_deg)
 
 

From c4a4e13196be2fdf738a173ca4e70b2a4eb43590 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 13 Jul 2021 00:16:24 -0600
Subject: [PATCH 60/72] fix assert on angles

---
 python/gtsam/tests/test_ShonanAveraging.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index 8c70df5dff..29a14b1b6c 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -196,11 +196,13 @@ def test_constructorBetweenFactorPose2s(self) -> None:
 
         wRi_list = [result_values.atRot2(i) for i in range(num_images)]
         thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
-        thetas_deg -= max(thetas_deg)
+        
         # map all angles to [0,360)
         thetas_deg = thetas_deg % 360
+        thetas_deg -= max(thetas_deg)
+        
         expected_thetas_deg = np.array([0.0, 90.0, 0.0])
-        np.testing.assert_allclose(thetas_deg, expected_thetas_deg)
+        np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
 
 
 if __name__ == "__main__":

From 54d34711214a438c89f173eb9878b43036876d22 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 13 Jul 2021 08:18:45 -0600
Subject: [PATCH 61/72] update logic in angular error comparison

---
 python/gtsam/tests/test_ShonanAveraging.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index 29a14b1b6c..19b9f8dc1a 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -199,7 +199,7 @@ def test_constructorBetweenFactorPose2s(self) -> None:
         
         # map all angles to [0,360)
         thetas_deg = thetas_deg % 360
-        thetas_deg -= max(thetas_deg)
+        thetas_deg -= thetas_deg[0]
         
         expected_thetas_deg = np.array([0.0, 90.0, 0.0])
         np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)

From 3402e46ad19cc6e2bef1c71cd777b167e09fe136 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Tue, 13 Jul 2021 22:00:21 +0200
Subject: [PATCH 62/72] Shared data for triangulation unit tests

---
 python/gtsam/tests/test_Cal3Fisheye.py | 46 ++++++++++----------------
 1 file changed, 18 insertions(+), 28 deletions(-)

diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py
index d731204ef8..756113b930 100644
--- a/python/gtsam/tests/test_Cal3Fisheye.py
+++ b/python/gtsam/tests/test_Cal3Fisheye.py
@@ -33,6 +33,19 @@ def setUpClass(cls):
         u, v = np.arctan2(x, z), 0.0
         cls.obj_point = np.array([x, y, z])
         cls.img_point = np.array([u, v])
+
+        p1 = [-1.0, 0.0, -1.0]
+        p2 = [ 1.0, 0.0, -1.0]
+        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        pose1 = gtsam.Pose3(q1, p1)
+        pose2 = gtsam.Pose3(q2, p2)
+        camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
+        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
+        cls.origin = np.array([0.0, 0.0, 0.0])
+        cls.poses = gtsam.Pose3Vector([pose1, pose2])
+        cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
+        cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras])
         
     def test_Cal3Fisheye(self):
         K = gtsam.Cal3Fisheye()
@@ -96,40 +109,17 @@ def test_sfm_factor2(self):
     @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
     def test_triangulation_skipped(self):
         """Estimate spatial point from image measurements"""
-        p1 = [-1.0, 0.0, -1.0]
-        p2 = [ 1.0, 0.0, -1.0]
-        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
-        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
-        obj_point = np.array([0.0, 0.0, 0.0])
-        pose1 = gtsam.Pose3(q1, p1)
-        pose2 = gtsam.Pose3(q2, p2)
-        camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
-        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
-        cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
-        measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
-        triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True)
+        triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True)
         self.gtsamAssertEquals(measurements[0], self.img_point)
-        self.gtsamAssertEquals(triangulated, obj_point)
+        self.gtsamAssertEquals(triangulated, self.origin)
 
     def test_triangulation_rectify(self):
         """Estimate spatial point from image measurements using rectification"""
-        p1 = [-1.0, 0.0, -1.0]
-        p2 = [ 1.0, 0.0, -1.0]
-        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
-        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
-        obj_point = np.array([0.0, 0.0, 0.0])
-        pose1 = gtsam.Pose3(q1, p1)
-        pose2 = gtsam.Pose3(q2, p2)
-        camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
-        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
-        cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
-        measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
-        rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)])
+        rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
         shared_cal = gtsam.Cal3_S2()
-        poses = gtsam.Pose3Vector([pose1, pose2])
-        triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
+        triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
         self.gtsamAssertEquals(measurements[0], self.img_point)
-        self.gtsamAssertEquals(triangulated, obj_point)
+        self.gtsamAssertEquals(triangulated, self.origin)
 
     def test_retract(self):
         expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,

From 17c37de7c4325bebdbfad1531ecd270f4ca80eb2 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Tue, 13 Jul 2021 22:07:19 +0200
Subject: [PATCH 63/72] Shared setup triangulation unit test

---
 python/gtsam/tests/test_Cal3Unified.py | 46 ++++++++++----------------
 1 file changed, 18 insertions(+), 28 deletions(-)

diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index 0b09fc3ae7..ca0959e44c 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -40,6 +40,19 @@ def setUpClass(cls):
         xi = 1
         cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
 
+        p1 = [-1.0, 0.0, -1.0]
+        p2 = [ 1.0, 0.0, -1.0]
+        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
+        pose1 = gtsam.Pose3(q1, p1)
+        pose2 = gtsam.Pose3(q2, p2)
+        camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic)
+        camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic)
+        cls.origin = np.array([0.0, 0.0, 0.0])
+        cls.poses = gtsam.Pose3Vector([pose1, pose2])
+        cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
+        cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras])
+
     def test_Cal3Unified(self):
         K = gtsam.Cal3Unified()
         self.assertEqual(K.fx(), 1.)
@@ -108,40 +121,17 @@ def test_sfm_factor2(self):
     @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
     def test_triangulation(self):
         """Estimate spatial point from image measurements"""
-        p1 = [-1.0, 0.0, -1.0]
-        p2 = [ 1.0, 0.0, -1.0]
-        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
-        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
-        obj_point = np.array([0.0, 0.0, 0.0])
-        pose1 = gtsam.Pose3(q1, p1)
-        pose2 = gtsam.Pose3(q2, p2)
-        camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic)
-        camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic)
-        cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
-        measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
-        triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True)
+        triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True)
         self.gtsamAssertEquals(measurements[0], self.img_point)
-        self.gtsamAssertEquals(triangulated, obj_point)
+        self.gtsamAssertEquals(triangulated, self.origin)
 
     def test_triangulation_rectify(self):
         """Estimate spatial point from image measurements using rectification"""
-        p1 = [-1.0, 0.0, -1.0]
-        p2 = [ 1.0, 0.0, -1.0]
-        q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
-        q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
-        obj_point = np.array([0.0, 0.0, 0.0])
-        pose1 = gtsam.Pose3(q1, p1)
-        pose2 = gtsam.Pose3(q2, p2)
-        camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic)
-        camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic)
-        cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
-        measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
-        rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)])
+        rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
         shared_cal = gtsam.Cal3_S2()
-        poses = gtsam.Pose3Vector([pose1, pose2])
-        triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
+        triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
         self.gtsamAssertEquals(measurements[0], self.img_point)
-        self.gtsamAssertEquals(triangulated, obj_point)
+        self.gtsamAssertEquals(triangulated, self.origin)
 
     def test_retract(self):
         expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,

From 3e41ece75a0ba8fc923ea6f59d912fd52f3914d3 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Tue, 13 Jul 2021 22:10:38 +0200
Subject: [PATCH 64/72] Minor fix test_Cal3Unified

---
 python/gtsam/tests/test_Cal3Unified.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index ca0959e44c..a402ae509f 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -122,7 +122,6 @@ def test_sfm_factor2(self):
     def test_triangulation(self):
         """Estimate spatial point from image measurements"""
         triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True)
-        self.gtsamAssertEquals(measurements[0], self.img_point)
         self.gtsamAssertEquals(triangulated, self.origin)
 
     def test_triangulation_rectify(self):
@@ -130,7 +129,6 @@ def test_triangulation_rectify(self):
         rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
         shared_cal = gtsam.Cal3_S2()
         triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
-        self.gtsamAssertEquals(measurements[0], self.img_point)
         self.gtsamAssertEquals(triangulated, self.origin)
 
     def test_retract(self):

From d130387a7db0b52080905c15ddcebfa22484d0de Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Tue, 13 Jul 2021 22:12:14 +0200
Subject: [PATCH 65/72] Minor fix test_Cal3Fisheye

---
 python/gtsam/tests/test_Cal3Fisheye.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py
index 756113b930..646b48881c 100644
--- a/python/gtsam/tests/test_Cal3Fisheye.py
+++ b/python/gtsam/tests/test_Cal3Fisheye.py
@@ -110,7 +110,6 @@ def test_sfm_factor2(self):
     def test_triangulation_skipped(self):
         """Estimate spatial point from image measurements"""
         triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True)
-        self.gtsamAssertEquals(measurements[0], self.img_point)
         self.gtsamAssertEquals(triangulated, self.origin)
 
     def test_triangulation_rectify(self):
@@ -118,7 +117,6 @@ def test_triangulation_rectify(self):
         rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
         shared_cal = gtsam.Cal3_S2()
         triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
-        self.gtsamAssertEquals(measurements[0], self.img_point)
         self.gtsamAssertEquals(triangulated, self.origin)
 
     def test_retract(self):

From 16cfc7fd381a9250bb573a4613037299be770d9f Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Wed, 14 Jul 2021 11:15:10 +0200
Subject: [PATCH 66/72] Remove commented out line

---
 python/gtsam/tests/test_Cal3Fisheye.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py
index 646b48881c..298c6e57b9 100644
--- a/python/gtsam/tests/test_Cal3Fisheye.py
+++ b/python/gtsam/tests/test_Cal3Fisheye.py
@@ -29,7 +29,6 @@ def setUpClass(cls):
         image plane and theta the incident angle of the object point.
         """
         x, y, z = 1.0, 0.0, 1.0
-        # x, y, z = 0.5, 0.0, 2.0
         u, v = np.arctan2(x, z), 0.0
         cls.obj_point = np.array([x, y, z])
         cls.img_point = np.array([u, v])

From c2bbe78e867bf1797ef33be444cc400bfe946d71 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Wed, 14 Jul 2021 11:16:00 +0200
Subject: [PATCH 67/72] Remove comment

---
 python/gtsam/tests/test_Cal3Unified.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index a402ae509f..dab1ae4466 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -31,7 +31,6 @@ def setUpClass(cls):
         x, y, z = 1.0, 0.0, 1.0
         r = np.linalg.norm([x, y, z])
         u, v = 2*x/(z+r), 0.0
-        #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0
         cls.obj_point = np.array([x, y, z])
         cls.img_point = np.array([u, v])
 

From a115788ea5d56df6cfdbc127ab24b6ebdbb9d157 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Wed, 14 Jul 2021 11:53:31 +0200
Subject: [PATCH 68/72] Remove spaces in empty line

---
 gtsam/geometry/SimpleCamera.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index 2c34bdfa73..119e9d1a3c 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -35,7 +35,7 @@ namespace gtsam {
   using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
   using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
   using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
-  
+
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /**
  * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x

From 6919ad927729385ba6583b7987099a628e708db8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 14 Jul 2021 21:53:20 -0400
Subject: [PATCH 69/72] update interface files with latest develop

---
 gtsam/base/base.i                       |  6 +-
 gtsam/geometry/SimpleCamera.h           |  2 +
 gtsam/geometry/geometry.i               | 58 ++++++++++++++++++
 gtsam/geometry/triangulation.h          | 10 ++-
 gtsam/linear/Coreset.h                  | 81 +++++++++++++++++++++++++
 gtsam/nonlinear/nonlinear.i             | 57 +++++++++++++----
 gtsam/sfm/sfm.i                         |  2 +
 gtsam/slam/slam.i                       | 15 ++++-
 python/CMakeLists.txt                   |  2 +
 python/gtsam/specializations/geometry.h |  4 ++
 10 files changed, 220 insertions(+), 17 deletions(-)
 create mode 100644 gtsam/linear/Coreset.h

diff --git a/gtsam/base/base.i b/gtsam/base/base.i
index 09278ff5b7..c24b04088a 100644
--- a/gtsam/base/base.i
+++ b/gtsam/base/base.i
@@ -6,6 +6,8 @@ namespace gtsam {
 
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/EssentialMatrix.h>
@@ -104,8 +106,8 @@ virtual class Value {
 template <T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2,
                gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2,
                gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
-               gtsam::EssentialMatrix, gtsam::CalibratedCamera,
-               gtsam::imuBias::ConstantBias}>
+               gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,
+               gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };
diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index aa00222c77..27b8bb549d 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -21,6 +21,7 @@
 #include <gtsam/geometry/BearingRange.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
@@ -33,6 +34,7 @@ namespace gtsam {
   using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
   using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
   using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
+  using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /**
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 6217d9e80a..4e56347d37 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -684,6 +684,57 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
   gtsam::Cal3Unified retract(Vector v) const;
   Vector localCoordinates(const gtsam::Cal3Unified& c) const;
 
+  // Action on Point2
+  // Note: the signature of this functions differ from the functions
+  // with equal name in the base class.
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
+};
+
+#include <gtsam/geometry/Cal3Fisheye.h>
+class Cal3Fisheye {
+  // Standard Constructors
+  Cal3Fisheye();
+  Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1,
+              double k2, double k3, double k4, double tol = 1e-5);
+  Cal3Fisheye(Vector v);
+
+  // Testable
+  void print(string s = "Cal3Fisheye") const;
+  bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3Fisheye retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Fisheye& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double k1() const;
+  double k2() const;
+  double k3() const;
+  double k4() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  Vector vector() const;
+  Vector k() const;
+  Matrix K() const;
+  Matrix inverse() const;
+
   // enabling serialization functionality
   void serialize() const;
 
@@ -860,6 +911,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
 typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
 
 template <T>
 class CameraSet {
@@ -924,6 +976,12 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize);
 
 #include <gtsam/geometry/BearingRange.h>
 template <POSE, POINT, BEARING, RANGE>
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 1df9efd22e..6f6ade6f70 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -18,14 +18,16 @@
 
 #pragma once
 
-#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/slam/TriangulationFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/TriangulationFactor.h>
 
 namespace gtsam {
 
@@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 // Vector of Cameras - used by the Python/MATLAB wrapper
 using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
+using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
+using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 
 } // \namespace gtsam
 
diff --git a/gtsam/linear/Coreset.h b/gtsam/linear/Coreset.h
new file mode 100644
index 0000000000..f622d9c81c
--- /dev/null
+++ b/gtsam/linear/Coreset.h
@@ -0,0 +1,81 @@
+#include <gtsam/linear/LossFunctions.h>
+
+#include <Eigen/unsupported/Eigen/CX11/Tensor>
+
+namespace gtsam {
+
+Vector FastCaratheodory(Matrix& P, Vector& weights, size_t coreset_size) {
+  size_t n = P.rows(), d = P.cols();
+  size_t m = 2 * d + 2;
+
+  if (n < d + 1) {
+    return weights;
+  }
+
+  Vector weights = weights / weights.sum();
+
+  size_t chunk_size = ceil(n / m);
+  size_t current_m = ceil(n / chunk_size);
+
+  size_t add_z = chunk_size - size_t(n % chunk_size);
+  Matrix u(weights.size(), 1);
+  u.col(0) = weights;
+
+  if (add_z != chunk_size) {
+    Matrix zeros = Matrix::Zero(add_z, d);
+    Matrix P_new = Matrix(P.rows() + zeros.rows(), P.cols() + zeros.cols());
+    P_new << P, zeros;
+    zeros = Matrix::Zero(add_z, u.cols());
+    Matrix u_new(u.rows() + zeros.rows(), u.cols() + zeros.cols());
+    u_new << u, zeros;
+  }
+
+  Vector idxarray = Vector::LinSpaced(n, 0, n - 1);
+  Eigen::Tensor p_groups;
+
+  // p_groups = P.reshape(current_m, chunk_size, P.shape[1])
+  // u_groups = u.reshape(current_m, chunk_size)
+  // idx_group = idxarray.reshape(current_m, chunk_size)
+  // u_nonzero = np.count_nonzero(u)
+
+  //   if not coreset_size:
+  //       coreset_size = d+1
+  //   while u_nonzero > coreset_size:
+
+  //       groups_means = np.einsum('ijk,ij->ik',p_groups, u_groups)
+  //       group_weigts = np.ones(groups_means.shape[0], dtype =
+  //       dtype)*1/current_m
+
+  //       Cara_u_idx = Caratheodory(groups_means , group_weigts,dtype = dtype )
+
+  //       IDX = np.nonzero(Cara_u_idx)
+
+  //       new_P = p_groups[IDX].reshape(-1,d)
+
+  //       subset_u = (current_m * u_groups[IDX] * Cara_u_idx[IDX][:,
+  //       np.newaxis]).reshape(-1, 1) new_idx_array =
+  //       idx_group[IDX].reshape(-1,1)
+  //       ##############################################################################3
+  //       u_nonzero = np.count_nonzero(subset_u)
+  //       chunk_size = math.ceil(new_P.shape[0]/ m)
+  //       current_m = math.ceil(new_P.shape[0]/ chunk_size)
+
+  //       add_z = chunk_size - int(new_P.shape[0] % chunk_size)
+  //       if add_z != chunk_size:
+  //           new_P = np.concatenate((new_P, np.zeros((add_z, new_P.shape[1]),
+  //           dtype = dtype))) subset_u = np.concatenate((subset_u,
+  //           np.zeros((add_z, subset_u.shape[1]),dtype = dtype)))
+  //           new_idx_array = np.concatenate((new_idx_array, np.zeros((add_z,
+  //           new_idx_array.shape[1]),dtype = dtype)))
+  //       p_groups = new_P.reshape(current_m, chunk_size, new_P.shape[1])
+  //       u_groups = subset_u.reshape(current_m, chunk_size)
+  //       idx_group = new_idx_array.reshape(current_m , chunk_size)
+  //       ###########################################################
+
+  //   new_u = np.zeros(n)
+  //   subset_u = subset_u[(new_idx_array < n)]
+  //   new_idx_array = new_idx_array[(new_idx_array <
+  //   n)].reshape(-1).astype(int) new_u[new_idx_array] = subset_u return u_sum
+  //   * new_u
+}
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index e22ac5954b..8071e8bc79 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -6,6 +6,8 @@ namespace gtsam {
 
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/EssentialMatrix.h>
@@ -151,11 +153,25 @@ class NonlinearFactorGraph {
   gtsam::KeySet keys() const;
   gtsam::KeyVector keyVector() const;
 
-  template <T = {double, Vector, gtsam::Point2, gtsam::StereoPoint2,
-                 gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4,
-                 gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,
-                 gtsam::CalibratedCamera, gtsam::PinholeCamera<gtsam::Cal3_S2>,
+  template <T = {double,
+                 Vector,
+                 gtsam::Point2,
+                 gtsam::StereoPoint2,
+                 gtsam::Point3,
+                 gtsam::Rot2,
+                 gtsam::SO3,
+                 gtsam::SO4,
+                 gtsam::Rot3,
+                 gtsam::Pose2,
+                 gtsam::Pose3,
+                 gtsam::Cal3_S2,
+                 gtsam::Cal3Fisheye,
+                 gtsam::Cal3Unified,
+                 gtsam::CalibratedCamera,
+                 gtsam::PinholeCamera<gtsam::Cal3_S2>,
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                 gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
+                 gtsam::PinholeCamera<gtsam::Cal3Unified>,
                  gtsam::imuBias::ConstantBias}>
   void addPrior(size_t key, const T& prior,
                 const gtsam::noiseModel::Base* noiseModel);
@@ -291,10 +307,13 @@ class Values {
   void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
   void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
+  void insert(size_t j, const gtsam::Cal3Unified& cal3unified);
   void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
-  void insert(size_t j,
-              const gtsam::PinholeCamera<gtsam::Cal3_S2>& simple_camera);
+  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
+  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
@@ -312,10 +331,13 @@ class Values {
   void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
   void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
+  void update(size_t j, const gtsam::Cal3Unified& cal3unified);
   void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
-  void update(size_t j,
-              const gtsam::PinholeCamera<gtsam::Cal3_S2>& simple_camera);
+  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
+  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
@@ -335,9 +357,13 @@ class Values {
                  gtsam::Cal3_S2,
                  gtsam::Cal3DS2,
                  gtsam::Cal3Bundler,
+                 gtsam::Cal3Fisheye,
+                 gtsam::Cal3Unified,
                  gtsam::EssentialMatrix,
                  gtsam::PinholeCamera<gtsam::Cal3_S2>,
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                 gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
+                 gtsam::PinholeCamera<gtsam::Cal3Unified>,
                  gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  Vector,
@@ -681,7 +707,9 @@ class ISAM2 {
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Cal3Bundler, gtsam::EssentialMatrix,
                      gtsam::PinholeCamera<gtsam::Cal3_S2>,
-                     gtsam::PinholeCamera<gtsam::Cal3Bundler>, Vector, Matrix}>
+                     gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                     gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
+                     gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
   gtsam::Values calculateBestEstimate() const;
   Matrix marginalCovariance(size_t key) const;
@@ -734,10 +762,14 @@ template <T = {double,
                gtsam::Cal3_S2,
                gtsam::Cal3DS2,
                gtsam::Cal3Bundler,
+               gtsam::Cal3Fisheye,
+               gtsam::Cal3Unified,
                gtsam::CalibratedCamera,
                gtsam::PinholeCamera<gtsam::Cal3_S2>,
-               gtsam::imuBias::ConstantBias,
-               gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
+               gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+               gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
+               gtsam::PinholeCamera<gtsam::Cal3Unified>,
+               gtsam::imuBias::ConstantBias}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior,
               const gtsam::noiseModel::Base* noiseModel);
@@ -755,6 +787,9 @@ template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
                gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
                gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
                gtsam::PinholeCamera<gtsam::Cal3_S2>,
+               gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+               gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
+               gtsam::PinholeCamera<gtsam::Cal3Unified>,
                gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index c86de8d88e..705892e60f 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -87,6 +87,8 @@ class ShonanAveraging2 {
   ShonanAveraging2(string g2oFile);
   ShonanAveraging2(string g2oFile,
                    const gtsam::ShonanAveragingParameters2& parameters);
+  ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
+                   const gtsam::ShonanAveragingParameters2 &parameters);
 
   // Query properties
   size_t nrUnknowns() const;
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index 457e907b92..1c04fd14c0 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -62,6 +62,12 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
 typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                        gtsam::Cal3DS2>
     GenericProjectionFactorCal3DS2;
+typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
+                                       gtsam::Cal3Fisheye>
+    GenericProjectionFactorCal3Fisheye;
+typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
+                                       gtsam::Cal3Unified>
+    GenericProjectionFactorCal3Unified;
 
 #include <gtsam/slam/GeneralSFMFactor.h>
 template <CAMERA, LANDMARK>
@@ -80,8 +86,15 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3DS2>,
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                                 gtsam::Point3>
     GeneralSFMFactorCal3Bundler;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
+                                gtsam::Point3>
+    GeneralSFMFactorCal3Fisheye;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
+                                gtsam::Point3>
+    GeneralSFMFactorCal3Unified;
 
-template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
+template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
+                         gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   GeneralSFMFactor2(const gtsam::Point2& measured,
                     const gtsam::noiseModel::Base* model, size_t poseKey,
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 676479bd55..7b8347da5c 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -116,6 +116,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::BinaryMeasurementsUnit3
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler
+            gtsam::CameraSetCal3Unified
+            gtsam::CameraSetCal3Fisheye
             gtsam::KeyPairDoubleMap)
             
     pybind_wrap(gtsam_unstable_py # target
diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h
index 3d22581d98..e11d3cc4fe 100644
--- a/python/gtsam/specializations/geometry.h
+++ b/python/gtsam/specializations/geometry.h
@@ -21,3 +21,7 @@ py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(
     m_, "CameraSetCal3_S2");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>>(
     m_, "CameraSetCal3Bundler");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified>>>(
+    m_, "CameraSetCal3Unified");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Fisheye>>>(
+    m_, "CameraSetCal3Fisheye");

From 6db646d80057555925845d7618a0f9284e77033f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 15 Jul 2021 00:25:40 -0400
Subject: [PATCH 70/72] remove extraneous file

---
 gtsam/linear/Coreset.h | 81 ------------------------------------------
 1 file changed, 81 deletions(-)
 delete mode 100644 gtsam/linear/Coreset.h

diff --git a/gtsam/linear/Coreset.h b/gtsam/linear/Coreset.h
deleted file mode 100644
index f622d9c81c..0000000000
--- a/gtsam/linear/Coreset.h
+++ /dev/null
@@ -1,81 +0,0 @@
-#include <gtsam/linear/LossFunctions.h>
-
-#include <Eigen/unsupported/Eigen/CX11/Tensor>
-
-namespace gtsam {
-
-Vector FastCaratheodory(Matrix& P, Vector& weights, size_t coreset_size) {
-  size_t n = P.rows(), d = P.cols();
-  size_t m = 2 * d + 2;
-
-  if (n < d + 1) {
-    return weights;
-  }
-
-  Vector weights = weights / weights.sum();
-
-  size_t chunk_size = ceil(n / m);
-  size_t current_m = ceil(n / chunk_size);
-
-  size_t add_z = chunk_size - size_t(n % chunk_size);
-  Matrix u(weights.size(), 1);
-  u.col(0) = weights;
-
-  if (add_z != chunk_size) {
-    Matrix zeros = Matrix::Zero(add_z, d);
-    Matrix P_new = Matrix(P.rows() + zeros.rows(), P.cols() + zeros.cols());
-    P_new << P, zeros;
-    zeros = Matrix::Zero(add_z, u.cols());
-    Matrix u_new(u.rows() + zeros.rows(), u.cols() + zeros.cols());
-    u_new << u, zeros;
-  }
-
-  Vector idxarray = Vector::LinSpaced(n, 0, n - 1);
-  Eigen::Tensor p_groups;
-
-  // p_groups = P.reshape(current_m, chunk_size, P.shape[1])
-  // u_groups = u.reshape(current_m, chunk_size)
-  // idx_group = idxarray.reshape(current_m, chunk_size)
-  // u_nonzero = np.count_nonzero(u)
-
-  //   if not coreset_size:
-  //       coreset_size = d+1
-  //   while u_nonzero > coreset_size:
-
-  //       groups_means = np.einsum('ijk,ij->ik',p_groups, u_groups)
-  //       group_weigts = np.ones(groups_means.shape[0], dtype =
-  //       dtype)*1/current_m
-
-  //       Cara_u_idx = Caratheodory(groups_means , group_weigts,dtype = dtype )
-
-  //       IDX = np.nonzero(Cara_u_idx)
-
-  //       new_P = p_groups[IDX].reshape(-1,d)
-
-  //       subset_u = (current_m * u_groups[IDX] * Cara_u_idx[IDX][:,
-  //       np.newaxis]).reshape(-1, 1) new_idx_array =
-  //       idx_group[IDX].reshape(-1,1)
-  //       ##############################################################################3
-  //       u_nonzero = np.count_nonzero(subset_u)
-  //       chunk_size = math.ceil(new_P.shape[0]/ m)
-  //       current_m = math.ceil(new_P.shape[0]/ chunk_size)
-
-  //       add_z = chunk_size - int(new_P.shape[0] % chunk_size)
-  //       if add_z != chunk_size:
-  //           new_P = np.concatenate((new_P, np.zeros((add_z, new_P.shape[1]),
-  //           dtype = dtype))) subset_u = np.concatenate((subset_u,
-  //           np.zeros((add_z, subset_u.shape[1]),dtype = dtype)))
-  //           new_idx_array = np.concatenate((new_idx_array, np.zeros((add_z,
-  //           new_idx_array.shape[1]),dtype = dtype)))
-  //       p_groups = new_P.reshape(current_m, chunk_size, new_P.shape[1])
-  //       u_groups = subset_u.reshape(current_m, chunk_size)
-  //       idx_group = new_idx_array.reshape(current_m , chunk_size)
-  //       ###########################################################
-
-  //   new_u = np.zeros(n)
-  //   subset_u = subset_u[(new_idx_array < n)]
-  //   new_idx_array = new_idx_array[(new_idx_array <
-  //   n)].reshape(-1).astype(int) new_u[new_idx_array] = subset_u return u_sum
-  //   * new_u
-}
-}  // namespace gtsam
\ No newline at end of file

From f819b1a03f74f289f50b96125610026618601a2f Mon Sep 17 00:00:00 2001
From: Akash Patel <17132214+acxz@users.noreply.github.com>
Date: Thu, 15 Jul 2021 15:01:56 -0400
Subject: [PATCH 71/72] Revert "replace deprecated tbb functionality"

---
 gtsam/base/treeTraversal-inst.h               |  5 +-
 .../treeTraversal/parallelTraversalTasks.h    | 70 ++++++++++++-------
 2 files changed, 46 insertions(+), 29 deletions(-)

diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h
index 30cec3b9a4..7a88f72eb6 100644
--- a/gtsam/base/treeTraversal-inst.h
+++ b/gtsam/base/treeTraversal-inst.h
@@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
   // Typedefs
   typedef typename FOREST::Node Node;
 
-  internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
-      visitorPost, problemSizeThreshold);
+  tbb::task::spawn_root_and_wait(
+      internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
+          visitorPost, problemSizeThreshold));
 #else
   DepthFirstForest(forest, rootData, visitorPre, visitorPost);
 #endif
diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h
index dc1b459066..87d5b0d4c3 100644
--- a/gtsam/base/treeTraversal/parallelTraversalTasks.h
+++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h
@@ -22,7 +22,7 @@
 #include <boost/make_shared.hpp>
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/task_group.h>         // tbb::task_group
+#include <tbb/task.h>               // tbb::task, tbb::task_list
 #include <tbb/scalable_allocator.h> // tbb::scalable_allocator
 
 namespace gtsam {
@@ -34,7 +34,7 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class PreOrderTask
+      class PreOrderTask : public tbb::task
       {
       public:
         const boost::shared_ptr<NODE>& treeNode;
@@ -42,30 +42,28 @@ namespace gtsam {
         VISITOR_PRE& visitorPre;
         VISITOR_POST& visitorPost;
         int problemSizeThreshold;
-        tbb::task_group& tg;
         bool makeNewTasks;
 
-        // Keep track of order phase across multiple calls to the same functor
-        mutable bool isPostOrderPhase;
+        bool isPostOrderPhase;
 
         PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData,
                      VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
-                     tbb::task_group& tg, bool makeNewTasks = true)
+                     bool makeNewTasks = true)
             : treeNode(treeNode),
               myData(myData),
               visitorPre(visitorPre),
               visitorPost(visitorPost),
               problemSizeThreshold(problemSizeThreshold),
-              tg(tg),
               makeNewTasks(makeNewTasks),
               isPostOrderPhase(false) {}
 
-        void operator()() const
+        tbb::task* execute() override
         {
           if(isPostOrderPhase)
           {
             // Run the post-order visitor since this task was recycled to run the post-order visitor
             (void) visitorPost(treeNode, *myData);
+            return nullptr;
           }
           else
           {
@@ -73,10 +71,14 @@ namespace gtsam {
             {
               if(!treeNode->children.empty())
               {
+                // Allocate post-order task as a continuation
+                isPostOrderPhase = true;
+                recycle_as_continuation();
+
                 bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold);
 
-                // If we have child tasks, start subtasks and wait for them to complete
-                tbb::task_group ctg;
+                tbb::task* firstChild = 0;
+                tbb::task_list childTasks;
                 for(const boost::shared_ptr<NODE>& child: treeNode->children)
                 {
                   // Process child in a subtask.  Important:  Run visitorPre before calling
@@ -84,30 +86,37 @@ namespace gtsam {
                   // allocated an extra child, this causes a TBB error.
                   boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>(
                       tbb::scalable_allocator<DATA>(), visitorPre(child, *myData));
-                  ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost,
-                      problemSizeThreshold, ctg, overThreshold));
+                  tbb::task* childTask =
+                      new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost,
+                                                          problemSizeThreshold, overThreshold);
+                  if (firstChild)
+                    childTasks.push_back(*childTask);
+                  else
+                    firstChild = childTask;
                 }
-                ctg.wait();
 
-                // Allocate post-order task as a continuation
-                isPostOrderPhase = true;
-                tg.run(*this);
+                // If we have child tasks, start subtasks and wait for them to complete
+                set_ref_count((int)treeNode->children.size());
+                spawn(childTasks);
+                return firstChild;
               }
               else
               {
                 // Run the post-order visitor in this task if we have no children
                 (void) visitorPost(treeNode, *myData);
+                return nullptr;
               }
             }
             else
             {
               // Process this node and its children in this task
               processNodeRecursively(treeNode, *myData);
+              return nullptr;
             }
           }
         }
 
-        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData) const
+        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
         {
           for(const boost::shared_ptr<NODE>& child: node->children)
           {
@@ -122,7 +131,7 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class RootTask
+      class RootTask : public tbb::task
       {
       public:
         const ROOTS& roots;
@@ -130,31 +139,38 @@ namespace gtsam {
         VISITOR_PRE& visitorPre;
         VISITOR_POST& visitorPost;
         int problemSizeThreshold;
-        tbb::task_group& tg;
         RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
-          int problemSizeThreshold, tbb::task_group& tg) :
+          int problemSizeThreshold) :
           roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
-          problemSizeThreshold(problemSizeThreshold), tg(tg) {}
+          problemSizeThreshold(problemSizeThreshold) {}
 
-        void operator()() const
+        tbb::task* execute() override
         {
           typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
           // Create data and tasks for our children
+          tbb::task_list tasks;
           for(const boost::shared_ptr<NODE>& root: roots)
           {
             boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
-            tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
+            tasks.push_back(*new(allocate_child())
+              PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold));
           }
+          // Set TBB ref count
+          set_ref_count(1 + (int) roots.size());
+          // Spawn tasks
+          spawn_and_wait_for_all(tasks);
+          // Return nullptr
+          return nullptr;
         }
       };
 
       template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
+      RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
+        CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
       {
           typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
-          tbb::task_group tg;
-          tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
-      }
+          return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold);
+        }
 
     }
 

From 1d75a0738a9a9cb187b61cdedfb13c4ae5e9019e Mon Sep 17 00:00:00 2001
From: Scott <scott.t.mcmichael@nasa.gov>
Date: Sat, 17 Jul 2021 18:35:58 -0700
Subject: [PATCH 72/72] Try macOS fix

---
 gtsam/nonlinear/tests/testSerializationNonlinear.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
index 0375826545..f4bb5f4f69 100644
--- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
+++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
@@ -126,7 +126,8 @@ TEST(Serialization, ISAM2) {
   graph.add(gtsam::PriorFactor<gtsam::Pose3>(symbol0, pose0, noiseModel));
   initialValues.insert(symbol0, pose0);
 
-  solver.update(graph, initialValues, gtsam::FastVector<size_t>());
+  solver.update(graph, initialValues,
+                gtsam::FastVector<gtsam::FactorIndex>());
 
   std::string binaryPath = "saved_solver.dat";
   try {