diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 93979bba1..b3a12a8d5 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -70,7 +70,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
   string dot(
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
       bool showZero = true) const;
-  std::vector<std::pair<DiscreteValues, double>> enumerate() const;
+  std::vector<std::pair<gtsam::DiscreteValues, double>> enumerate() const;
   string markdown(const gtsam::KeyFormatter& keyFormatter =
                       gtsam::DefaultKeyFormatter) const;
   string markdown(const gtsam::KeyFormatter& keyFormatter,
@@ -97,7 +97,7 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
                       const gtsam::Ordering& orderedKeys);
   gtsam::DiscreteConditional operator*(
       const gtsam::DiscreteConditional& other) const;
-  DiscreteConditional marginal(gtsam::Key key) const;
+  gtsam::DiscreteConditional marginal(gtsam::Key key) const;
   void print(string s = "Discrete Conditional\n",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
@@ -269,16 +269,16 @@ class DiscreteFactorGraph {
   gtsam::DiscreteLookupDAG maxProduct(gtsam::Ordering::OrderingType type);
   gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering);
 
-  gtsam::DiscreteBayesNet eliminateSequential();
-  gtsam::DiscreteBayesNet eliminateSequential(gtsam::Ordering::OrderingType type);
-  gtsam::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering);
-  std::pair<gtsam::DiscreteBayesNet, gtsam::DiscreteFactorGraph>
+  gtsam::DiscreteBayesNet* eliminateSequential();
+  gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
+  gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
+  pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
       eliminatePartialSequential(const gtsam::Ordering& ordering);
 
-  gtsam::DiscreteBayesTree eliminateMultifrontal();
-  gtsam::DiscreteBayesTree eliminateMultifrontal(gtsam::Ordering::OrderingType type);  
-  gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);  
-  std::pair<gtsam::DiscreteBayesTree, gtsam::DiscreteFactorGraph>
+  gtsam::DiscreteBayesTree* eliminateMultifrontal();
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);  
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);  
+  pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
       eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
 
   string dot(
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index b240603fc..82b5ec91d 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -41,6 +41,9 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
  public:
   enum { dimension = 3 };
 
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3Bundler>;
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index f756cba5e..affce0819 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -37,6 +37,9 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
  public:
   enum { dimension = 9 };
 
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3DS2>;
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index a9b09cf46..b583234fd 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -47,6 +47,9 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
  public:
   enum { dimension = 9 };
 
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3DS2_Base>;
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index f07ca0a54..e93d313c8 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -52,6 +52,9 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
  public:
   enum { dimension = 10 };
 
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3Unified>;
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h
index c6e1001c4..d88afd505 100644
--- a/gtsam/nonlinear/ISAM2Params.h
+++ b/gtsam/nonlinear/ISAM2Params.h
@@ -300,18 +300,10 @@ struct GTSAM_EXPORT ISAM2Params {
   RelinearizationThreshold getRelinearizeThreshold() const {
     return relinearizeThreshold;
   }
-  int getRelinearizeSkip() const { return relinearizeSkip; }
-  bool isEnableRelinearization() const { return enableRelinearization; }
-  bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
   std::string getFactorization() const {
     return factorizationTranslator(factorization);
   }
-  bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
   KeyFormatter getKeyFormatter() const { return keyFormatter; }
-  bool isEnableDetailedResults() const { return enableDetailedResults; }
-  bool isEnablePartialRelinearizationCheck() const {
-    return enablePartialRelinearizationCheck;
-  }
 
   void setOptimizationParams(OptimizationParams optimizationParams) {
     this->optimizationParams = optimizationParams;
@@ -319,31 +311,12 @@ struct GTSAM_EXPORT ISAM2Params {
   void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
     this->relinearizeThreshold = relinearizeThreshold;
   }
-  void setRelinearizeSkip(int relinearizeSkip) {
-    this->relinearizeSkip = relinearizeSkip;
-  }
-  void setEnableRelinearization(bool enableRelinearization) {
-    this->enableRelinearization = enableRelinearization;
-  }
-  void setEvaluateNonlinearError(bool evaluateNonlinearError) {
-    this->evaluateNonlinearError = evaluateNonlinearError;
-  }
   void setFactorization(const std::string& factorization) {
     this->factorization = factorizationTranslator(factorization);
   }
-  void setCacheLinearizedFactors(bool cacheLinearizedFactors) {
-    this->cacheLinearizedFactors = cacheLinearizedFactors;
-  }
   void setKeyFormatter(KeyFormatter keyFormatter) {
     this->keyFormatter = keyFormatter;
   }
-  void setEnableDetailedResults(bool enableDetailedResults) {
-    this->enableDetailedResults = enableDetailedResults;
-  }
-  void setEnablePartialRelinearizationCheck(
-      bool enablePartialRelinearizationCheck) {
-    this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck;
-  }
 
   GaussianFactorGraph::Eliminate getEliminationFunction() const {
     return factorization == CHOLESKY
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 055fbd75b..da5e8b29c 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -588,21 +588,19 @@ class ISAM2Params {
   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);
+
+  int relinearizeSkip;
+  bool enableRelinearization;
+  bool evaluateNonlinearError;
+  bool cacheLinearizedFactors;
+  bool enableDetailedResults;
+  bool enablePartialRelinearizationCheck;
+  bool findUnusedFactorSlots;
+
+  enum Factorization { CHOLESKY, QR };
+  Factorization factorization;
 };
 
 class ISAM2Clique {
diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h
index ba4d12a25..20f12dbce 100644
--- a/gtsam/slam/PoseRotationPrior.h
+++ b/gtsam/slam/PoseRotationPrior.h
@@ -39,6 +39,9 @@ protected:
 
 public:
 
+  /** default constructor - only use for serialization */
+  PoseRotationPrior() {}
+
   /** standard constructor */
   PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
   : Base(model, key), measured_(rot_z) {}
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index 2785a3fb3..4e943253e 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -223,12 +223,12 @@ enum KernelFunctionType {
   KernelFunctionTypeTUKEY
 };
 
-std::pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
     string filename, gtsam::noiseModel::Diagonal* model = nullptr,
     size_t maxIndex = 0, bool addNoise = false, bool smart = true,
-    gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormatAUTO,
+    gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormat::NoiseFormatAUTO,
     gtsam::KernelFunctionType kernelFunctionType =
-        gtsam::KernelFunctionTypeNONE);
+        gtsam::KernelFunctionType::KernelFunctionTypeNONE);
 
 void save2D(const gtsam::NonlinearFactorGraph& graph,
             const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
@@ -259,7 +259,7 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(
     string filename, const bool is3D = false,
     gtsam::KernelFunctionType kernelFunctionType =
-        gtsam::KernelFunctionTypeNONE);
+        gtsam::KernelFunctionType::KernelFunctionTypeNONE);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,
               const gtsam::Values& estimate, string filename);
 
diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i
index 8c9345147..dd66e7a73 100644
--- a/gtsam_unstable/gtsam_unstable.i
+++ b/gtsam_unstable/gtsam_unstable.i
@@ -566,7 +566,13 @@ virtual class FixedLagSmoother {
   gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
   double smootherLag() const;
 
-  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
+                                       const gtsam::Values &newTheta,
+                                       const gtsam::FixedLagSmootherKeyTimestampMap &timestamps);
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
+                                       const gtsam::Values &newTheta,
+                                       const gtsam::FixedLagSmootherKeyTimestampMap &timestamps,
+                                       const gtsam::FactorIndices &factorsToRemove);
   gtsam::Values calculateEstimate() const;
 };
 
@@ -576,6 +582,8 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
   BatchFixedLagSmoother(double smootherLag);
   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
 
+  void print(string s = "BatchFixedLagSmoother:\n") const;
+
   gtsam::LevenbergMarquardtParams params() const;
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
@@ -589,7 +597,12 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
   IncrementalFixedLagSmoother(double smootherLag);
   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
 
+  void print(string s = "IncrementalFixedLagSmoother:\n") const;
+
   gtsam::ISAM2Params params() const;
+
+  gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::ISAM2 getISAM2() const;
 };
 
 #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
index 79c05a01a..4079dbb23 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
@@ -113,6 +113,9 @@ public:
   /// Get results of latest isam2 update
   const ISAM2Result& getISAM2Result() const{ return isamResult_; }
 
+  /// Get the iSAM2 object which is used for the inference internally
+  const ISAM2& getISAM2() const { return isam_; }
+
 protected:
 
   /** Create default parameters */
diff --git a/matlab/+gtsam/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m
index 9b834e3e1..560503345 100644
--- a/matlab/+gtsam/VisualISAMInitialize.m
+++ b/matlab/+gtsam/VisualISAMInitialize.m
@@ -7,7 +7,7 @@ import gtsam.*
 %% Initialize iSAM
 params = gtsam.ISAM2Params;
 if options.alwaysRelinearize
-    params.setRelinearizeSkip(1);
+    params.relinearizeSkip = 1;
 end
 isam = ISAM2(params);
 
diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt
index 749ad870a..1755e2075 100644
--- a/matlab/CMakeLists.txt
+++ b/matlab/CMakeLists.txt
@@ -68,6 +68,8 @@ set(interface_files
     ${GTSAM_SOURCE_DIR}/gtsam/gtsam.i
     ${GTSAM_SOURCE_DIR}/gtsam/base/base.i
     ${GTSAM_SOURCE_DIR}/gtsam/basis/basis.i
+    ${PROJECT_SOURCE_DIR}/gtsam/inference/inference.i
+    ${PROJECT_SOURCE_DIR}/gtsam/discrete/discrete.i
     ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
     ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
     ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m
index 530c3382c..b350618e5 100755
--- a/matlab/gtsam_examples/IMUKittiExampleGPS.m
+++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m
@@ -52,7 +52,7 @@ IMU_params.setOmegaCoriolis(w_coriolis);
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;
diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m
index ccc975d84..b753916c6 100644
--- a/matlab/unstable_examples/+imuSimulator/IMUComparison.m
+++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m
@@ -46,7 +46,7 @@ posesIMUbody(1).R = poses(1).R;
 
 %% Solver object
 isamParams = ISAM2Params;
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 
 initialValues = Values;
diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
index 6adc8e9dc..689d8a3f5 100644
--- a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
+++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
@@ -34,7 +34,7 @@ poses(1).R = currentPoseGlobal.rotation.matrix;
 
 %% Solver object
 isamParams = ISAM2Params;
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 
 sigma_init_x = 1.0;
diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m
index 61dc78d96..dd276e2c1 100644
--- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m
+++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m
@@ -119,7 +119,7 @@ h = figure;
 % Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;
diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m
index cb13eacee..b09764ec0 100644
--- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m
+++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m
@@ -82,7 +82,7 @@ w_coriolis = [0;0;0];
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('QR');
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;
diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m
index f35d36512..4183e439a 100644
--- a/matlab/unstable_examples/IMUKittiExampleVO.m
+++ b/matlab/unstable_examples/IMUKittiExampleVO.m
@@ -58,7 +58,7 @@ w_coriolis = [0;0;0];
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;
diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py
index 8b6b4fdf0..d00f633ba 100644
--- a/python/gtsam/examples/IMUKittiExampleGPS.py
+++ b/python/gtsam/examples/IMUKittiExampleGPS.py
@@ -203,7 +203,7 @@ def optimize(gps_measurements: List[GpsMeasurement],
     # Set ISAM2 parameters and create ISAM2 solver object
     isam_params = gtsam.ISAM2Params()
     isam_params.setFactorization("CHOLESKY")
-    isam_params.setRelinearizeSkip(10)
+    isam_params.relinearizeSkip = 10
 
     isam = gtsam.ISAM2(isam_params)
 
diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py
index c70dbfa6f..3a8de0317 100644
--- a/python/gtsam/examples/Pose2ISAM2Example.py
+++ b/python/gtsam/examples/Pose2ISAM2Example.py
@@ -111,7 +111,7 @@ def Pose2SLAM_ISAM2_example():
     # update calls are required to perform the relinearization.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.1)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create the ground truth odometry measurements of the robot during the trajectory.
diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py
index 59b9fb2ac..cb71813c5 100644
--- a/python/gtsam/examples/Pose3ISAM2Example.py
+++ b/python/gtsam/examples/Pose3ISAM2Example.py
@@ -140,7 +140,7 @@ def Pose3_ISAM2_example():
     # update calls are required to perform the relinearization.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.1)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create the ground truth poses of the robot trajectory.
diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py
index bacf510ec..4b480fab7 100644
--- a/python/gtsam/examples/VisualISAM2Example.py
+++ b/python/gtsam/examples/VisualISAM2Example.py
@@ -81,7 +81,7 @@ def visual_ISAM2_example():
     # will approach the batch result.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.01)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create a Factor Graph and Values to hold the new data
diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h
index 320e0ac71..d07a75f6f 100644
--- a/python/gtsam/preamble/inference.h
+++ b/python/gtsam/preamble/inference.h
@@ -10,6 +10,3 @@
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
  */
-
-#include <pybind11/stl.h>
-
diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py
index 1e4fba8b6..18c9ef722 100644
--- a/python/gtsam/tests/test_SfmData.py
+++ b/python/gtsam/tests/test_SfmData.py
@@ -14,10 +14,9 @@ from __future__ import print_function
 
 import unittest
 
-import numpy as np
-
 import gtsam
-from gtsam import SfmData, SfmTrack, Point2, Point3
+import numpy as np
+from gtsam import Point2, Point3, SfmData, SfmTrack
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -34,40 +33,39 @@ class TestSfmData(GtsamTestCase):
         """Test functions in SfmTrack"""
         # measurement is of format (camera_idx, imgPoint)
         # create arbitrary camera indices for two cameras
-        i1, i2 = 4,5
+        i1, i2 = 4, 5
 
         # create arbitrary image measurements for cameras i1 and i2
         uv_i1 = Point2(12.6, 82)
-        
+
         # translating point uv_i1 along X-axis
         uv_i2 = Point2(24.88, 82)
-        
+
         # add measurements to the track
         self.tracks.addMeasurement(i1, uv_i1)
         self.tracks.addMeasurement(i2, uv_i2)
-        
+
         # Number of measurements in the track is 2
         self.assertEqual(self.tracks.numberMeasurements(), 2)
-        
+
         # camera_idx in the first measurement of the track corresponds to i1
         cam_idx, img_measurement = self.tracks.measurement(0)
         self.assertEqual(cam_idx, i1)
         np.testing.assert_array_almost_equal(
-            Point3(0.,0.,0.), 
+            Point3(0., 0., 0.),
             self.tracks.point3()
         )
 
-
     def test_data(self):
         """Test functions in SfmData"""
         # Create new track with 3 measurements
-        i1, i2, i3 = 3,5,6
+        i1, i2, i3 = 3, 5, 6
         uv_i1 = Point2(21.23, 45.64)
-        
+
         # translating along X-axis
         uv_i2 = Point2(45.7, 45.64)
         uv_i3 = Point2(68.35, 45.64)
-        
+
         # add measurements and arbitrary point to the track
         measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
         pt = Point3(1.0, 6.0, 2.0)
@@ -80,14 +78,17 @@ class TestSfmData(GtsamTestCase):
 
         # Number of tracks in SfmData is 2
         self.assertEqual(self.data.numberTracks(), 2)
-        
+
         # camera idx of first measurement of second track corresponds to i1
         cam_idx, img_measurement = self.data.track(1).measurement(0)
         self.assertEqual(cam_idx, i1)
 
     def test_Balbianello(self):
+        """ Check that we can successfully read a bundler file and create a 
+            factor graph from it
+        """
         # The structure where we will save the SfM data
-        filename = gtsam.findExampleDataFile("Balbianello")
+        filename = gtsam.findExampleDataFile("Balbianello.out")
         sfm_data = SfmData.FromBundlerFile(filename)
 
         # Check number of things
@@ -104,7 +105,8 @@ class TestSfmData(GtsamTestCase):
         self.gtsamAssertEquals(expected, actual, 1)
 
         # We share *one* noiseModel between all projection factors
-        model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)  # one pixel in u and v
+        model = gtsam.noiseModel.Isotropic.Sigma(
+            2, 1.0)  # one pixel in u and v
 
         # Convert to NonlinearFactorGraph
         graph = sfm_data.sfmFactorGraph(model)
diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py
index b2a7e2d29..972f25477 100644
--- a/python/gtsam/utils/visual_data_generator.py
+++ b/python/gtsam/utils/visual_data_generator.py
@@ -1,12 +1,12 @@
 from __future__ import print_function
-from typing import Tuple
 
 import math
-import numpy as np
 from math import pi
+from typing import Tuple
 
 import gtsam
-from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
+import numpy as np
+from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3
 
 
 class Options:
@@ -36,7 +36,7 @@ class GroundTruth:
         self.cameras = [Pose3()] * nrCameras
         self.points = [Point3(0, 0, 0)] * nrPoints
 
-    def print(self, s="") -> None:
+    def print(self, s: str = "") -> None:
         print(s)
         print("K = ", self.K)
         print("Cameras: ", len(self.cameras))
@@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]:
         r = 10
         for j in range(len(truth.points)):
             theta = j * 2 * pi / nrPoints
-            truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
+            truth.points[j] = Point3(
+                r * math.cos(theta), r * math.sin(theta), 0)
     else:  # 3D landmarks as vertices of a cube
         truth.points = [
             Point3(10, 10, 10), Point3(-10, 10, 10),
diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py
index a8fed4b23..4ebd8accd 100644
--- a/python/gtsam/utils/visual_isam.py
+++ b/python/gtsam/utils/visual_isam.py
@@ -17,7 +17,7 @@ def initialize(data, truth, options):
     # Initialize iSAM
     params = gtsam.ISAM2Params()
     if options.alwaysRelinearize:
-        params.setRelinearizeSkip(1)
+        params.relinearizeSkip = 1
     isam = gtsam.ISAM2(params=params)
 
     # Add constraints/priors
diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py
index f4a7988fd..4c2b005b7 100644
--- a/wrap/gtwrap/matlab_wrapper/mixins.py
+++ b/wrap/gtwrap/matlab_wrapper/mixins.py
@@ -26,25 +26,30 @@ class CheckMixin:
                 return True
         return False
 
+    def can_be_pointer(self, arg_type: parser.Type):
+        """
+        Determine if the `arg_type` can have a pointer to it.
+
+        E.g. `Pose3` can have `Pose3*` but 
+        `Matrix` should not have `Matrix*`.
+        """
+        return (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_shared_ptr(self, arg_type: parser.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')
+        return arg_type.is_shared_ptr
 
     def is_ptr(self, arg_type: parser.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')
+        return arg_type.is_ptr
 
     def is_ref(self, arg_type: parser.Type):
         """
diff --git a/wrap/gtwrap/matlab_wrapper/wrapper.py b/wrap/gtwrap/matlab_wrapper/wrapper.py
index 42610999d..e690cd213 100755
--- a/wrap/gtwrap/matlab_wrapper/wrapper.py
+++ b/wrap/gtwrap/matlab_wrapper/wrapper.py
@@ -147,11 +147,13 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         """
         def args_copy(args):
             return ArgumentList([copy.copy(arg) for arg in args.list()])
+
         def method_copy(method):
             method2 = copy.copy(method)
             method2.args = args_copy(method.args)
             method2.args.backup = method.args.backup
             return method2
+
         if save_backup:
             method.args.backup = args_copy(method.args)
         method = method_copy(method)
@@ -162,7 +164,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                 method.args.list().remove(arg)
                 return [
                     methodWithArg,
-                    *MatlabWrapper._expand_default_arguments(method, save_backup=False)
+                    *MatlabWrapper._expand_default_arguments(method,
+                                                             save_backup=False)
                 ]
             break
         assert all(arg.default is None for arg in method.args.list()), \
@@ -180,9 +183,12 @@ class MatlabWrapper(CheckMixin, FormatMixin):
 
             if method_index is None:
                 method_map[method.name] = len(method_out)
-                method_out.append(MatlabWrapper._expand_default_arguments(method))
+                method_out.append(
+                    MatlabWrapper._expand_default_arguments(method))
             else:
-                method_out[method_index] += MatlabWrapper._expand_default_arguments(method)
+                method_out[
+                    method_index] += MatlabWrapper._expand_default_arguments(
+                        method)
 
         return method_out
 
@@ -337,43 +343,42 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         body_args = ''
 
         for arg in args.list():
+            ctype_camel = self._format_type_name(arg.ctype.typename,
+                                                 separator='')
+            ctype_sep = self._format_type_name(arg.ctype.typename)
+
             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('''\
-                   {ctype}& {name} = *unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");
-                '''.format(ctype=self._format_type_name(arg.ctype.typename),
-                           ctype_camel=ctype_camel,
-                           name=arg.name,
-                           id=arg_id)),
-                                             prefix='  ')
+                arg_type = "{ctype}&".format(ctype=ctype_sep)
+                unwrap = '*unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");'.format(
+                    ctype=ctype_sep, ctype_camel=ctype_camel, id=arg_id)
 
-            elif (self.is_shared_ptr(arg.ctype) or self.is_ptr(arg.ctype)) and \
+            elif 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
-                else:
-                    call_type = arg.ctype.is_ptr
 
-                body_args += textwrap.indent(textwrap.dedent('''\
-                    {std_boost}::shared_ptr<{ctype_sep}> {name} = unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");
-                '''.format(std_boost='boost' if constructor else 'boost',
-                           ctype_sep=self._format_type_name(
-                               arg.ctype.typename),
-                           ctype=self._format_type_name(arg.ctype.typename,
-                                                        separator=''),
-                           name=arg.name,
-                           id=arg_id)),
-                                             prefix='  ')
+                arg_type = "{ctype_sep}*".format(ctype_sep=ctype_sep)
+                unwrap = 'unwrap_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");'.format(
+                    ctype_sep=ctype_sep, ctype=ctype_camel, id=arg_id)
+
+            elif (self.is_shared_ptr(arg.ctype) or self.can_be_pointer(arg.ctype)) and \
+                    arg.ctype.typename.name not in self.ignore_namespace:
+                call_type = arg.ctype.is_shared_ptr
+
+                arg_type = "{std_boost}::shared_ptr<{ctype_sep}>".format(
+                    std_boost='boost' if constructor else 'boost',
+                    ctype_sep=ctype_sep)
+                unwrap = 'unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");'.format(
+                    ctype_sep=ctype_sep, ctype=ctype_camel, id=arg_id)
 
             else:
-                body_args += textwrap.indent(textwrap.dedent('''\
-                    {ctype} {name} = unwrap< {ctype} >(in[{id}]);
-                '''.format(ctype=arg.ctype.typename.name,
-                           name=arg.name,
-                           id=arg_id)),
-                                             prefix='  ')
+                arg_type = "{ctype}".format(ctype=arg.ctype.typename.name)
+                unwrap = 'unwrap< {ctype} >(in[{id}]);'.format(
+                    ctype=arg.ctype.typename.name, id=arg_id)
 
+            body_args += textwrap.indent(textwrap.dedent('''\
+                    {arg_type} {name} = {unwrap}
+                    '''.format(arg_type=arg_type, name=arg.name,
+                               unwrap=unwrap)),
+                                         prefix='  ')
             arg_id += 1
 
         params = ''
@@ -383,12 +388,14 @@ class MatlabWrapper(CheckMixin, FormatMixin):
             if params != '':
                 params += ','
 
-            if (arg.default is not None) and (arg.name not in explicit_arg_names):
+            if (arg.default is not None) and (arg.name
+                                              not in explicit_arg_names):
                 params += arg.default
                 continue
 
-            if (not self.is_ref(arg.ctype)) and (self.is_shared_ptr(arg.ctype)) and (self.is_ptr(
-                    arg.ctype)) and (arg.ctype.typename.name not in self.ignore_namespace):
+            if not self.is_ref(arg.ctype) and (self.is_shared_ptr(arg.ctype) or \
+                self.is_ptr(arg.ctype) or self.can_be_pointer(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
                 else:
@@ -601,7 +608,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         if not isinstance(ctors, Iterable):
             ctors = [ctors]
 
-        ctors = sum((MatlabWrapper._expand_default_arguments(ctor) for ctor in ctors), [])
+        ctors = sum((MatlabWrapper._expand_default_arguments(ctor)
+                     for ctor in ctors), [])
 
         methods_wrap = textwrap.indent(textwrap.dedent("""\
             methods
@@ -885,10 +893,10 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                     wrapper=self._wrapper_name(),
                     id=self._update_wrapper_id(
                         (namespace_name, instantiated_class,
-                        static_overload.name, static_overload)),
+                         static_overload.name, static_overload)),
                     class_name=instantiated_class.name,
                     end_statement=end_statement),
-                                            prefix='    ')
+                                               prefix='    ')
 
             # If the arguments don't match any of the checks above,
             # throw an error with the class and method name.
@@ -1079,7 +1087,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         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) or \
+            self.can_be_pointer(return_type):
             shared_obj = 'pairResult.' + pair_value
 
             if not (return_type.is_shared_ptr or return_type.is_ptr):
@@ -1145,7 +1154,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
 
         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) or \
+                    self.can_be_pointer(return_1):
                     sep_method_name = partial(self._format_type_name,
                                               return_1.typename,
                                               include_namespace=True)
diff --git a/wrap/matlab.h b/wrap/matlab.h
index bcdef3c8d..fbed0b2e2 100644
--- a/wrap/matlab.h
+++ b/wrap/matlab.h
@@ -477,6 +477,14 @@ boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& pro
   return *spp;
 }
 
+template <typename Class>
+Class* unwrap_ptr(const mxArray* obj, const string& propertyName) {
+
+  mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str());
+  Class* x = reinterpret_cast<Class*> (mxGetData(mxh));
+  return x;
+}
+
 //// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector
 //template <>
 //Vector unwrap_shared_ptr<Vector>(const mxArray* obj, const string& propertyName) {
diff --git a/wrap/tests/expected/matlab/ForwardKinematicsFactor.m b/wrap/tests/expected/matlab/ForwardKinematicsFactor.m
index 46aa41392..e5efdce19 100644
--- a/wrap/tests/expected/matlab/ForwardKinematicsFactor.m
+++ b/wrap/tests/expected/matlab/ForwardKinematicsFactor.m
@@ -11,9 +11,9 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3>
         if nargin == 2
           my_ptr = varargin{2};
         else
-          my_ptr = inheritance_wrapper(36, varargin{2});
+          my_ptr = inheritance_wrapper(52, varargin{2});
         end
-        base_ptr = inheritance_wrapper(35, my_ptr);
+        base_ptr = inheritance_wrapper(51, my_ptr);
       else
         error('Arguments do not match any overload of ForwardKinematicsFactor constructor');
       end
@@ -22,7 +22,7 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3>
     end
 
     function delete(obj)
-      inheritance_wrapper(37, obj.ptr_ForwardKinematicsFactor);
+      inheritance_wrapper(53, obj.ptr_ForwardKinematicsFactor);
     end
 
     function display(obj), obj.print(''); end
diff --git a/wrap/tests/expected/matlab/functions_wrapper.cpp b/wrap/tests/expected/matlab/functions_wrapper.cpp
index 17b5fb494..61286d84f 100644
--- a/wrap/tests/expected/matlab/functions_wrapper.cpp
+++ b/wrap/tests/expected/matlab/functions_wrapper.cpp
@@ -86,7 +86,7 @@ void load2D_2(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("load2D",nargout,nargin,2);
   string filename = unwrap< string >(in[0]);
-  boost::shared_ptr<gtsam::noiseModel::Diagonal> model = unwrap_shared_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal");
+  gtsam::noiseModel::Diagonal* model = unwrap_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal");
   auto pairResult = load2D(filename,model);
   out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);
diff --git a/wrap/tests/expected/matlab/geometry_wrapper.cpp b/wrap/tests/expected/matlab/geometry_wrapper.cpp
index ee1f04359..4a3ad1d68 100644
--- a/wrap/tests/expected/matlab/geometry_wrapper.cpp
+++ b/wrap/tests/expected/matlab/geometry_wrapper.cpp
@@ -151,7 +151,7 @@ void gtsamPoint2_argChar_7(int nargout, mxArray *out[], int nargin, const mxArra
 {
   checkArguments("argChar",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  boost::shared_ptr<char> a = unwrap_shared_ptr< char >(in[1], "ptr_char");
+  char* a = unwrap_ptr< char >(in[1], "ptr_char");
   obj->argChar(a);
 }
 
@@ -175,7 +175,7 @@ void gtsamPoint2_argChar_10(int nargout, mxArray *out[], int nargin, const mxArr
 {
   checkArguments("argChar",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  boost::shared_ptr<char> a = unwrap_shared_ptr< char >(in[1], "ptr_char");
+  char* a = unwrap_ptr< char >(in[1], "ptr_char");
   obj->argChar(a);
 }
 
diff --git a/wrap/tests/expected/matlab/inheritance_wrapper.cpp b/wrap/tests/expected/matlab/inheritance_wrapper.cpp
index 0cf17eedd..9c45ca55f 100644
--- a/wrap/tests/expected/matlab/inheritance_wrapper.cpp
+++ b/wrap/tests/expected/matlab/inheritance_wrapper.cpp
@@ -9,6 +9,7 @@
 
 typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
 typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
+typedef MyTemplate<A> MyTemplateA;
 
 typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase;
 static Collector_MyBase collector_MyBase;
@@ -16,6 +17,8 @@ typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint
 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<MyTemplateA>*> Collector_MyTemplateA;
+static Collector_MyTemplateA collector_MyTemplateA;
 typedef std::set<boost::shared_ptr<ForwardKinematicsFactor>*> Collector_ForwardKinematicsFactor;
 static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor;
 
@@ -44,6 +47,12 @@ void _deleteAllObjects()
     collector_MyTemplateMatrix.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_MyTemplateA::iterator iter = collector_MyTemplateA.begin();
+      iter != collector_MyTemplateA.end(); ) {
+    delete *iter;
+    collector_MyTemplateA.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin();
       iter != collector_ForwardKinematicsFactor.end(); ) {
     delete *iter;
@@ -67,6 +76,7 @@ void _inheritance_RTTIRegister() {
     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(MyTemplateA).name(), "MyTemplateA"));
     types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor"));
 
 
@@ -462,7 +472,157 @@ 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 ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateA_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyTemplate<A>> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_MyTemplateA.insert(self);
+
+  typedef boost::shared_ptr<MyBase> SharedBase;
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
+}
+
+void MyTemplateA_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyTemplate<A>> 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<A>>(*asVoid));
+  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
+}
+
+void MyTemplateA_constructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyTemplate<A>> Shared;
+
+  Shared *self = new Shared(new MyTemplate<A>());
+  collector_MyTemplateA.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+
+  typedef boost::shared_ptr<MyBase> SharedBase;
+  out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
+}
+
+void MyTemplateA_deconstructor_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MyTemplate<A>> Shared;
+  checkArguments("delete_MyTemplateA",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_MyTemplateA::iterator item;
+  item = collector_MyTemplateA.find(self);
+  if(item != collector_MyTemplateA.end()) {
+    collector_MyTemplateA.erase(item);
+  }
+  delete self;
+}
+
+void MyTemplateA_accept_T_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("accept_T",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  A& value = *unwrap_shared_ptr< A >(in[1], "ptr_A");
+  obj->accept_T(value);
+}
+
+void MyTemplateA_accept_Tptr_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("accept_Tptr",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  boost::shared_ptr<A> value = unwrap_shared_ptr< A >(in[1], "ptr_A");
+  obj->accept_Tptr(value);
+}
+
+void MyTemplateA_create_MixedPtrs_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("create_MixedPtrs",nargout,nargin-1,0);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  auto pairResult = obj->create_MixedPtrs();
+  out[0] = wrap_shared_ptr(boost::make_shared<A>(pairResult.first),"A", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"A", false);
+}
+
+void MyTemplateA_create_ptrs_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("create_ptrs",nargout,nargin-1,0);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  auto pairResult = obj->create_ptrs();
+  out[0] = wrap_shared_ptr(pairResult.first,"A", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"A", false);
+}
+
+void MyTemplateA_return_T_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("return_T",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  A* value = unwrap_ptr< A >(in[1], "ptr_A");
+  out[0] = wrap_shared_ptr(boost::make_shared<A>(obj->return_T(value)),"A", false);
+}
+
+void MyTemplateA_return_Tptr_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("return_Tptr",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  boost::shared_ptr<A> value = unwrap_shared_ptr< A >(in[1], "ptr_A");
+  out[0] = wrap_shared_ptr(obj->return_Tptr(value),"A", false);
+}
+
+void MyTemplateA_return_ptrs_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("return_ptrs",nargout,nargin-1,2);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  boost::shared_ptr<A> p1 = unwrap_shared_ptr< A >(in[1], "ptr_A");
+  boost::shared_ptr<A> p2 = unwrap_shared_ptr< A >(in[2], "ptr_A");
+  auto pairResult = obj->return_ptrs(p1,p2);
+  out[0] = wrap_shared_ptr(pairResult.first,"A", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"A", false);
+}
+
+void MyTemplateA_templatedMethod_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  Matrix t = unwrap< Matrix >(in[1]);
+  out[0] = wrap< Matrix >(obj->templatedMethod<gtsam::Matrix>(t));
+}
+
+void MyTemplateA_templatedMethod_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  Point2 t = unwrap< Point2 >(in[1]);
+  out[0] = wrap< Point2 >(obj->templatedMethod<gtsam::Point2>(t));
+}
+
+void MyTemplateA_templatedMethod_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  Point3 t = unwrap< Point3 >(in[1]);
+  out[0] = wrap< Point3 >(obj->templatedMethod<gtsam::Point3>(t));
+}
+
+void MyTemplateA_templatedMethod_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodVector",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  Vector t = unwrap< Vector >(in[1]);
+  out[0] = wrap< Vector >(obj->templatedMethod<gtsam::Vector>(t));
+}
+
+void MyTemplateA_Level_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("MyTemplate<A>.Level",nargout,nargin,1);
+  A& K = *unwrap_shared_ptr< A >(in[0], "ptr_A");
+  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<A>>(MyTemplate<A>::Level(K)),"MyTemplateA", false);
+}
+
+void ForwardKinematicsFactor_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
@@ -475,7 +635,7 @@ 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[]) {
+void ForwardKinematicsFactor_upcastFromVoid_52(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]));
@@ -484,7 +644,7 @@ void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int
   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
-void ForwardKinematicsFactor_deconstructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematicsFactor_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
   checkArguments("delete_ForwardKinematicsFactor",nargout,nargin,1);
@@ -615,13 +775,61 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1);
       break;
     case 35:
-      ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
+      MyTemplateA_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
       break;
     case 36:
-      ForwardKinematicsFactor_upcastFromVoid_36(nargout, out, nargin-1, in+1);
+      MyTemplateA_upcastFromVoid_36(nargout, out, nargin-1, in+1);
       break;
     case 37:
-      ForwardKinematicsFactor_deconstructor_37(nargout, out, nargin-1, in+1);
+      MyTemplateA_constructor_37(nargout, out, nargin-1, in+1);
+      break;
+    case 38:
+      MyTemplateA_deconstructor_38(nargout, out, nargin-1, in+1);
+      break;
+    case 39:
+      MyTemplateA_accept_T_39(nargout, out, nargin-1, in+1);
+      break;
+    case 40:
+      MyTemplateA_accept_Tptr_40(nargout, out, nargin-1, in+1);
+      break;
+    case 41:
+      MyTemplateA_create_MixedPtrs_41(nargout, out, nargin-1, in+1);
+      break;
+    case 42:
+      MyTemplateA_create_ptrs_42(nargout, out, nargin-1, in+1);
+      break;
+    case 43:
+      MyTemplateA_return_T_43(nargout, out, nargin-1, in+1);
+      break;
+    case 44:
+      MyTemplateA_return_Tptr_44(nargout, out, nargin-1, in+1);
+      break;
+    case 45:
+      MyTemplateA_return_ptrs_45(nargout, out, nargin-1, in+1);
+      break;
+    case 46:
+      MyTemplateA_templatedMethod_46(nargout, out, nargin-1, in+1);
+      break;
+    case 47:
+      MyTemplateA_templatedMethod_47(nargout, out, nargin-1, in+1);
+      break;
+    case 48:
+      MyTemplateA_templatedMethod_48(nargout, out, nargin-1, in+1);
+      break;
+    case 49:
+      MyTemplateA_templatedMethod_49(nargout, out, nargin-1, in+1);
+      break;
+    case 50:
+      MyTemplateA_Level_50(nargout, out, nargin-1, in+1);
+      break;
+    case 51:
+      ForwardKinematicsFactor_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1);
+      break;
+    case 52:
+      ForwardKinematicsFactor_upcastFromVoid_52(nargout, out, nargin-1, in+1);
+      break;
+    case 53:
+      ForwardKinematicsFactor_deconstructor_53(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {
diff --git a/wrap/tests/expected/python/inheritance_pybind.cpp b/wrap/tests/expected/python/inheritance_pybind.cpp
index d6cd77ca0..fdb29b5ce 100644
--- a/wrap/tests/expected/python/inheritance_pybind.cpp
+++ b/wrap/tests/expected/python/inheritance_pybind.cpp
@@ -54,6 +54,21 @@ PYBIND11_MODULE(inheritance_py, m_) {
         .def("return_ptrs",[](MyTemplate<gtsam::Matrix>* self, const std::shared_ptr<gtsam::Matrix> p1, const std::shared_ptr<gtsam::Matrix> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
         .def_static("Level",[](const gtsam::Matrix& K){return MyTemplate<gtsam::Matrix>::Level(K);}, py::arg("K"));
 
+    py::class_<MyTemplate<A>, MyBase, std::shared_ptr<MyTemplate<A>>>(m_, "MyTemplateA")
+        .def(py::init<>())
+        .def("templatedMethodPoint2",[](MyTemplate<A>* self, const gtsam::Point2& t){return self->templatedMethod<gtsam::Point2>(t);}, py::arg("t"))
+        .def("templatedMethodPoint3",[](MyTemplate<A>* self, const gtsam::Point3& t){return self->templatedMethod<gtsam::Point3>(t);}, py::arg("t"))
+        .def("templatedMethodVector",[](MyTemplate<A>* self, const gtsam::Vector& t){return self->templatedMethod<gtsam::Vector>(t);}, py::arg("t"))
+        .def("templatedMethodMatrix",[](MyTemplate<A>* self, const gtsam::Matrix& t){return self->templatedMethod<gtsam::Matrix>(t);}, py::arg("t"))
+        .def("accept_T",[](MyTemplate<A>* self, const A& value){ self->accept_T(value);}, py::arg("value"))
+        .def("accept_Tptr",[](MyTemplate<A>* self, std::shared_ptr<A> value){ self->accept_Tptr(value);}, py::arg("value"))
+        .def("return_Tptr",[](MyTemplate<A>* self, std::shared_ptr<A> value){return self->return_Tptr(value);}, py::arg("value"))
+        .def("return_T",[](MyTemplate<A>* self, A* value){return self->return_T(value);}, py::arg("value"))
+        .def("create_ptrs",[](MyTemplate<A>* self){return self->create_ptrs();})
+        .def("create_MixedPtrs",[](MyTemplate<A>* self){return self->create_MixedPtrs();})
+        .def("return_ptrs",[](MyTemplate<A>* self, std::shared_ptr<A> p1, std::shared_ptr<A> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
+        .def_static("Level",[](const A& K){return MyTemplate<A>::Level(K);}, py::arg("K"));
+
     py::class_<ForwardKinematicsFactor, gtsam::BetweenFactor<gtsam::Pose3>, std::shared_ptr<ForwardKinematicsFactor>>(m_, "ForwardKinematicsFactor");
 
 
diff --git a/wrap/tests/fixtures/inheritance.i b/wrap/tests/fixtures/inheritance.i
index ddf9745df..e63f8e689 100644
--- a/wrap/tests/fixtures/inheritance.i
+++ b/wrap/tests/fixtures/inheritance.i
@@ -4,7 +4,7 @@ virtual class MyBase {
 };
 
 // A templated class
-template<T = {gtsam::Point2, Matrix}>
+template<T = {gtsam::Point2, Matrix, A}>
 virtual class MyTemplate : MyBase {
   MyTemplate();