Merge pull request #822 from borglab/feature/wrap-multiple-interfaces

Break interface file into multiple files

.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: [Debug, Release]
-        build_type: [Release]
         python_version: [3]
         include:
-          # - name: ubuntu-18.04-gcc-5
+          - name: ubuntu-18.04-gcc-5
-          #   os: ubuntu-18.04
+            os: ubuntu-18.04
-          #   compiler: gcc
+            compiler: gcc
-          #   version: "5"
+            version: "5"
 
           - name: ubuntu-18.04-gcc-9
             os: ubuntu-18.04
@@ -59,11 +57,11 @@ jobs:
             compiler: xcode
             version: "11.3.1"
 
-          # - name: ubuntu-18.04-gcc-5-tbb
+          - name: ubuntu-18.04-gcc-5-tbb
-          #   os: ubuntu-18.04
+            os: ubuntu-18.04
-          #   compiler: gcc
+            compiler: gcc
-          #   version: "5"
+            version: "5"
-          #   flag: tbb
+            flag: tbb
 
     steps:
       - name: Checkout

gtsam/base/base.i

@@ -0,0 +1,115 @@
+//*************************************************************************
+// base
+//*************************************************************************
+
+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>
+#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::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,
+               gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
+virtual class GenericValue : gtsam::Value {
+  void serializable() const;
+};
+
+}  // namespace gtsam

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_;
+};
+
+}

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/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3_S2.h>

gtsam/geometry/triangulation.h

@@ -18,16 +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 {
 

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);
+};
+
+}

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;
+};
+
+}

gtsam/nonlinear/nonlinear.i

@@ -0,0 +1,804 @@
+//*************************************************************************
+// nonlinear
+//*************************************************************************
+
+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>
+#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::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);
+
+  // 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::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>& 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);
+
+  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::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>& 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);
+  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::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,
+                 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>,
+                     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;
+  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::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}>
+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::PinholeCamera<gtsam::Cal3Bundler>,
+               gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
+               gtsam::PinholeCamera<gtsam::Cal3Unified>,
+               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

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_;
-};
-
 }
 

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

gtsam/sfm/sfm.i

@@ -0,0 +1,211 @@
+//*************************************************************************
+// 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);
+  ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
+                   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

gtsam/slam/slam.i

@@ -0,0 +1,338 @@
+//*************************************************************************
+// 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;
+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>
+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;
+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,
+                         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;
+
+  // 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

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

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

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"
 
 }}
 

python/gtsam/preamble.h

@@ -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<<;
-}

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

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>>);

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

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++.
+ */

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<<;
+}

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++.
+ */

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++.
+ */

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++.
+ */

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> > >);

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++.
+ */

python/gtsam/specializations.h

@@ -1,37 +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<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");

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");

python/gtsam/specializations/geometry.h

@@ -0,0 +1,27 @@
+/* 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");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified>>>(
+    m_, "CameraSetCal3Unified");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Fisheye>>>(
+    m_, "CameraSetCal3Fisheye");

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

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.
+ */

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.
+ */

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.
+ */

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.
+ */

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");

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");

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.
+ */

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}

wrap/.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

wrap/.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

wrap/.gitignore

@@ -8,4 +8,4 @@ __pycache__/
 # Files related to code coverage stats
 **/.coverage
 
-gtwrap/matlab_wrapper.tpl
+gtwrap/matlab_wrapper/matlab_wrapper.tpl

wrap/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.

wrap/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.

wrap/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

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.
+# User-friendly Pybind11 wrapping and installing function. Builds a Pybind11
-# Builds a Pybind11 module from the provided interface_header.
+# module from the provided interface_headers. For example, for the interface
-# For example, for the interface header gtsam.h, this will
+# header gtsam.h, this will build the wrap module 'gtsam_py.cc'.
-# 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,9 +30,10 @@ 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
+function(
+  pybind_wrap
   target
-         interface_header
+  interface_headers
   generated_cpp
   module_name
   top_namespace
@@ -55,51 +55,56 @@ function(pybind_wrap
     set(GTWRAP_PATH_SEPARATOR ";")
   endif()
 
-  add_custom_command(OUTPUT ${generated_cpp}
+  # Convert .i file names to .cpp file names.
-          COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
+  foreach(filepath ${interface_headers})
-          ${PYTHON_EXECUTABLE}
+    get_filename_component(interface ${filepath} NAME)
-                             ${PYBIND_WRAP_SCRIPT}
+    string(REPLACE ".i" ".cpp" cpp_file ${interface})
-                             --src
+    list(APPEND cpp_files ${cpp_file})
-                             ${interface_header}
+  endforeach()
-                             --out
+
-                             ${generated_cpp}
+  add_custom_command(
-                             --module_name
+    OUTPUT ${cpp_files}
-                             ${module_name}
+    COMMAND
-                             --top_module_namespaces
+      ${CMAKE_COMMAND} -E env
-                             "${top_namespace}"
+      "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
-                             --ignore
+      ${PYTHON_EXECUTABLE} ${PYBIND_WRAP_SCRIPT} --src "${interface_headers}"
-                             ${ignore_classes}
+      --out "${generated_cpp}" --module_name ${module_name}
-                             --template
+      --top_module_namespaces "${top_namespace}" --ignore ${ignore_classes}
-                             ${module_template}
+      --template ${module_template} ${_WRAP_BOOST_ARG}
-                             ${_WRAP_BOOST_ARG}
+    DEPENDS "${interface_headers}" ${module_template}
-                     DEPENDS ${interface_header} ${module_template}
     VERBATIM)
-  add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${generated_cpp})
+
+  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}
+  add_custom_command(
-                     DEPENDS ${interface_header}
+    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
+    # `type_info` objects will become "weak private external" if the templated
-    # export them with `WRAP_EXPORT`. If that happens, the `type_info` for the same templated class will diverge between shared
+    # class is initialized implicitly even if we explicitly export them with
-    # libraries, causing `dynamic_cast` to fail. This is mitigated by telling Clang to mimic the MSVC behavior.
+    # `WRAP_EXPORT`. If that happens, the `type_info` for the same templated
-    # See https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2
+    # 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
+function(install_python_scripts source_directory dest_directory patterns)
-         source_directory
-         dest_directory
-         patterns)
   set(patterns_args "")
   set(exclude_patterns "")
 
@@ -144,14 +146,16 @@ 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}"
+      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}"
+    install(
+      DIRECTORY "${source_directory}"
       DESTINATION "${dest_directory}"
       FILES_MATCHING ${patterns_args}
       PATTERN "${exclude_patterns}" EXCLUDE)
@@ -172,11 +176,12 @@ 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
+      # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if there
-      # there is one
+      # is one
       get_filename_component(location "${dest_directory}" PATH)
       get_filename_component(name "${dest_directory}" NAME)
-      install(FILES "${source_files}"
+      install(
+        FILES "${source_files}"
         DESTINATION "${location}/${name}${build_type_tag}"
         CONFIGURATIONS "${build_type}")
     endforeach()
@@ -194,7 +199,8 @@ function(create_symlinks source_folder dest_folder)
     return()
   endif()
 
-  file(GLOB files
+  file(
+    GLOB files
     LIST_DIRECTORIES true
     RELATIVE "${source_folder}"
     "${source_folder}/*")
@@ -224,7 +230,8 @@ function(create_symlinks source_folder dest_folder)
     endif()
     # cmake-format: on
 
-    execute_process(COMMAND ${command}
+    execute_process(
+      COMMAND ${command}
       RESULT_VARIABLE result
       ERROR_VARIABLE output)
 

wrap/gtwrap/interface_parser/__init__.py

@@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 
 import sys
 
-import pyparsing
+import pyparsing  # type: ignore
 
 from .classes import *
 from .declaration import *

wrap/gtwrap/interface_parser/classes.py

@@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 
 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 @@ class Class:
             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 @@ class Class:
 
     def __init__(
         self,
-        template: Template,
+        template: Union[Template, None],
         is_virtual: str,
         name: str,
         parent_class: list,
@@ -292,16 +292,16 @@ class Class:
         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

wrap/gtwrap/interface_parser/declaration.py

@@ -10,7 +10,7 @@ Classes and rules for declarations such as includes and forward declarations.
 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)

wrap/gtwrap/interface_parser/enum.py

@@ -10,7 +10,7 @@ Parser class and rules for parsing C++ enums.
 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

wrap/gtwrap/interface_parser/function.py

@@ -10,9 +10,9 @@ Parser classes and rules for parsing C++ functions.
 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 @@ class Argument:
                  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 @@ class ArgumentList:
             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 @@ class ReturnType:
         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:
         """

wrap/gtwrap/interface_parser/module.py

@@ -12,7 +12,8 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 
 # 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

wrap/gtwrap/interface_parser/namespace.py

@@ -14,7 +14,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 
 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 @@ class Namespace:
         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 @@ class Namespace:
             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:

wrap/gtwrap/interface_parser/template.py

@@ -12,11 +12,11 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 
 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:

wrap/gtwrap/interface_parser/tokens.py

@@ -10,9 +10,9 @@ All the token definitions.
 Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
 """
 
-from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress,
+from pyparsing import (Keyword, Literal, OneOrMore, Or,  # type: ignore
-                       Word, alphanums, alphas, nestedExpr, nums,
+                       QuotedString, Suppress, Word, alphanums, alphas,
-                       originalTextFor, printables)
+                       nestedExpr, nums, originalTextFor, printables)
 
 # rule for identifiers (e.g. variable names)
 IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums)

wrap/gtwrap/interface_parser/type.py

@@ -14,7 +14,8 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 
 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]
 

wrap/gtwrap/interface_parser/variable.py

@@ -10,7 +10,9 @@ Parser classes and rules for parsing C++ variables.
 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 Variable:
                 t.default[0] if isinstance(t.default, ParseResults) else None))
 
     def __init__(self,
-                 ctype: Type,
+                 ctype: List[Type],
                  name: str,
                  default: ParseResults = None,
                  parent=''):

wrap/gtwrap/matlab_wrapper/__init__.py

@@ -0,0 +1,3 @@
+"""Package to wrap C++ code to Matlab via MEX."""
+
+from .wrapper import MatlabWrapper

wrap/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):]

wrap/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='  ')

wrap/gtwrap/matlab_wrapper/wrapper.py

@@ -7,16 +7,19 @@ that Matlab's MEX compiler can use.
 
 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,9 +28,20 @@ class MatlabWrapper(object):
         top_module_namespace: C++ namespace for the top module (default '')
         ignore_classes: A list of classes to ignore (default [])
     """
+    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
+
         # Map the data type to its Matlab class.
         # Found in Argument.cpp in old wrapper
-    data_type = {
+        self.data_type = {
             'string': 'char',
             'char': 'char',
             'unsigned char': 'unsigned char',
@@ -39,7 +53,7 @@ class MatlabWrapper(object):
         }
         # Map the data type into the type used in Matlab methods.
         # Found in matlab.h in old wrapper
-    data_type_param = {
+        self.data_type_param = {
             'string': 'char',
             'char': 'char',
             'unsigned char': 'unsigned char',
@@ -52,62 +66,37 @@ class MatlabWrapper(object):
             '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
+        self.wrapper_id = 0
-    # Map each wrapper id to what its collector function namespace, class, type, and string format
+        # Map each wrapper id to its collector function namespace, class, type, and string format
-    wrapper_map = {}
+        self.wrapper_map: Dict = {}
         # Set of all the includes in the namespace
-    includes = {}  # type: Dict[parser.Include, int]
+        self.includes: List[parser.Include] = []
         # Set of all classes in the namespace
-    classes = [
+        self.classes: List[Union[parser.Class,
-    ]  # type: List[Union[parser.Class, instantiator.InstantiatedClass]]
+                                 instantiator.InstantiatedClass]] = []
-    classes_elems = {
+        self.classes_elems: Dict[Union[parser.Class,
-    }  # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int]
+                                       instantiator.InstantiatedClass],
+                                 int] = {}
         # Id for ordering global functions in the wrapper
-    global_function_id = 0
+        self.global_function_id = 0
         # Files and their content
-    content = []  # type: List[str]
+        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:
-        wrapper_file_header = f.read()
+            self.wrapper_file_headers = f.read()
 
-    def __init__(self,
+    def add_class(self, instantiated_class):
-                 module_name,
+        """Add `instantiated_class` to the list of classes."""
-                 top_module_namespace='',
-                 ignore_classes=()):
-        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):
         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 @@ class MatlabWrapper(object):
         """
         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 @@ class MatlabWrapper(object):
                 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 @@ class MatlabWrapper(object):
             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 @@ class MatlabWrapper(object):
                            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 @@ class MatlabWrapper(object):
 
         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 @@ class MatlabWrapper(object):
                 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 @@ class MatlabWrapper(object):
 
         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 @@ class MatlabWrapper(object):
         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 @@ class MatlabWrapper(object):
                                            prefix="  ")
 
         if serialize:
-            method_text += textwrap.indent(textwrap.dedent("""\
+            method_text += WrapperTemplate.matlab_deserialize.format(
-                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(
                 class_name=namespace_name + '.' + instantiated_class.name,
                 wrapper=self._wrapper_name(),
                 id=self._update_wrapper_id(
                     (namespace_name, instantiated_class, 'string_deserialize',
-                     'deserialize'))),
+                     'deserialize')))
-                                           prefix='  ')
 
         return method_text
 
@@ -1213,33 +967,32 @@ class MatlabWrapper(object):
 
         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()
+        cpp_filename = self._wrapper_name() + '.cpp'
-        self.content.append((matlab_wrapper[0], matlab_wrapper[1]))
+        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 @@ class MatlabWrapper(object):
             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 @@ class MatlabWrapper(object):
         """Wrap the collector function which returns a shared pointer."""
         new_line = '\n' if new_line else ''
 
-        return textwrap.indent(textwrap.dedent('''\
+        return WrapperTemplate.collector_function_shared_return.format(
-            {{
+            name=self._format_type_name(return_type_name,
-            boost::shared_ptr<{name}> shared({shared_obj});
+                                        include_namespace=False),
-            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),
+            new_line=new_line)
-                               prefix='  ')
 
     def wrap_collector_function_return_types(self, return_type, func_id):
         """
@@ -1296,7 +1045,7 @@ class MatlabWrapper(object):
         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 @@ class MatlabWrapper(object):
                 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 @@ class MatlabWrapper(object):
                         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 @@ class MatlabWrapper(object):
         """
         Add function to upcast type from void type.
         """
-        return textwrap.dedent('''\
+        return WrapperTemplate.collector_function_upcast_from_void.format(
-            void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{
+            class_name=class_name, cpp_name=cpp_name, id=func_id)
-              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)
 
     def generate_collector_function(self, func_id):
         """
@@ -1610,158 +1341,109 @@ class MatlabWrapper(object):
             else:
                 next_case = None
 
-        mex_function = textwrap.dedent('''
+        mex_function = WrapperTemplate.mex_function.format(
-            void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+            module_name=self.module_name, cases=cases)
-            {{
-              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)
 
         return mex_function
 
-    def generate_wrapper(self, namespace):
+    def get_class_name(self, cls):
-        """Generate the c++ wrapper."""
+        """Get the name of the class `cls` taking template instantiations into account."""
-        # Includes
+        if cls.instantiations:
-        wrapper_file = self.wrapper_file_header + textwrap.dedent("""
+            class_name_sep = cls.name
-            #include <boost/archive/text_iarchive.hpp>
-            #include <boost/archive/text_oarchive.hpp>
-            #include <boost/serialization/export.hpp>\n
-        """)
-
-        assert namespace
-
-        includes_list = sorted(list(self.includes.keys()),
-                               key=lambda include: include.header)
-
-        # 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),
+            class_name_sep = cls.to_cpp()
-                                   includes_list)
-        wrapper_file += '\n'
 
-        typedef_instances = '\n'
+        class_name = self._format_class_name(cls)
-        typedef_collectors = ''
+
+        return class_name, class_name_sep
+
+    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('''\
+        typedef_collectors = ''
-            void _deleteAllObjects()
+        rtti_classes = ''
-            {
-              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 = ''
 
         for cls in self.classes:
-            uninstantiated_name = "::".join(
+            # Check if class is in ignore list.
-                cls.namespaces()[1:]) + "::" + cls.name
+            # If so, then skip
-            self._debug("Cls: {} -> {}".format(cls.name, uninstantiated_name))
+            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):
+            class_name, class_name_sep = self.get_class_name(cls)
-                for m in cls.methods:
-                    if m.name in self.whitelist:
-                        return True
-                return False
 
+            # 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
+            # Get the Boost exports for serialization
-                class_name = self._format_class_name(cls)
+            if cls.original.namespaces() and self._has_serialization(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)
-            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 and declare the collector objects.
-                typedef std::set<boost::shared_ptr<{class_name_sep}>*> Collector_{class_name};
+            typedef_collectors += WrapperTemplate.typdef_collectors.format(
-                static Collector_{class_name} collector_{class_name};
+                class_name_sep=class_name_sep, class_name=class_name)
-            ''').format(class_name_sep=class_name_sep, class_name=class_name)
+
-            delete_objs += textwrap.indent(textwrap.dedent('''\
+            # Generate the _deleteAllObjects method
-                {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin();
+            delete_objs += WrapperTemplate.delete_obj.format(
-                    iter != collector_{class_name}.end(); ) {{
+                class_name=class_name)
-                  delete *iter;
-                  collector_{class_name}.erase(iter++);
-                  anyDeleted = true;
-                }} }}
-            ''').format(class_name=class_name),
-                                           prefix='  ')
 
             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 @@ class MatlabWrapper(object):
                 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)
+            {delete_all_objs}
-                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
             {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,
+                    delete_all_objs=delete_all_objs,
-                    rtti_register=rtti_reg_start + rtti_reg_mid + rtti_reg_end,
+                    rtti_register=rtti_register,
                     pointer_constructor_fragment=ptr_ctor_frag,
                     mex_function=self.mex_function())
 
@@ -1815,21 +1493,8 @@ class MatlabWrapper(object):
         wrapper_id = self._update_wrapper_id(
             (namespace_name, inst_class, 'string_serialize', 'serialize'))
 
-        return textwrap.dedent('''\
+        return WrapperTemplate.class_serialize_method.format(
-            function varargout = string_serialize(this, varargin)
+            wrapper=self._wrapper_name(),
-              % 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)
 
@@ -1840,18 +1505,8 @@ class MatlabWrapper(object):
         """
         Wrap the serizalize collector function.
         """
-        return textwrap.indent(textwrap.dedent("""\
+        return WrapperTemplate.collector_function_serialize.format(
-            typedef boost::shared_ptr<{full_name}> Shared;
+            class_name=class_name, full_name=full_name, namespace=namespace)
-            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='  ')
 
     def wrap_collector_function_deserialize(self,
                                             class_name,
@@ -1860,33 +1515,10 @@ class MatlabWrapper(object):
         """
         Wrap the deserizalize collector function.
         """
-        return textwrap.indent(textwrap.dedent("""\
+        return WrapperTemplate.collector_function_deserialize.format(
-            typedef boost::shared_ptr<{full_name}> Shared;
+            class_name=class_name, full_name=full_name, namespace=namespace)
-            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='  ')
 
-    def wrap(self, content):
+    def generate_content(self, cc_content, path):
-        """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)
-
-        return self.content
-
-
-def generate_content(cc_content, path, verbose=False):
         """
         Generate files and folders from matlab wrapper content.
 
@@ -1896,47 +1528,38 @@ def generate_content(cc_content, path, verbose=False):
                 (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):
+                if not osp.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]))
+                    self.generate_content(sub_content[1], path_to_folder)
-                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 osp.isdir(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])
 
-            _debug("[generate_content]: {}".format(path_to_file))
+                if not osp.isdir(path_to_file):
-            if not os.path.isdir(path_to_file):
                     try:
                         os.mkdir(path)
                     except OSError:
@@ -1944,3 +1567,33 @@ def generate_content(cc_content, path, verbose=False):
 
                 with open(path_to_file, 'w') as f:
                     f.write(c[1])
+
+    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()
+
+            # 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

wrap/gtwrap/pybind_wrapper.py

@@ -13,6 +13,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 # 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 @@ class PybindWrapper:
         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 @@ class PybindWrapper:
                 '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 @@ class PybindWrapper:
             )
         return wrapped, includes
 
-    def wrap(self, content):
+    def wrap_file(self, content, module_name=None, submodules=None):
-        """Wrap the code in the interface file."""
+        """
+        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 @@ class PybindWrapper:
             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,
+                    cpp_class=cpp_class, new_name=new_name)
-                    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)

wrap/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,11 +29,12 @@ 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[
+                ctype.typename.instantiations[
+                    idx] = instantiations[  # type: ignore
                         template_idx]
 
         return ctype
@@ -212,7 +213,9 @@ class InstantiatedMethod(parser.Method):
         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 @@ class InstantiatedClass(parser.Class):
         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 @@ class InstantiatedClass(parser.Class):
         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,8 +345,8 @@ class InstantiatedClass(parser.Class):
         )
 
     def __repr__(self):
-        return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\
+        return "{virtual}Class {cpp_class} : {parent_class}\n"\
-            "{ctors}\n{static_methods}\n{methods}".format(
+            "{ctors}\n{static_methods}\n{methods}\n{operators}".format(
                virtual="virtual " if self.is_virtual else '',
                name=self.name,
                cpp_class=self.to_cpp(),

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;

wrap/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 @@ and invoked during the wrapping by CMake.
 """
 
 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,7 +37,8 @@ if __name__ == "__main__":
         "`<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",
+    arg_parser.add_argument(
+        "--ignore",
         nargs='*',
         type=str,
         help="A space-separated list of classes to ignore. "
@@ -44,17 +49,10 @@ if __name__ == "__main__":
     if top_module_namespaces[0]:
         top_module_namespaces = [''] + top_module_namespaces
 
-    with open(args.src, 'r') as f:
+    print("[MatlabWrapper] Ignoring classes: {}".format(args.ignore), file=sys.stderr)
-        content = f.read()
-
-    if not os.path.exists(args.src):
-        os.mkdir(args.src)
-
-    print("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)
+    sources = args.src.split(';')
-
+    cc_content = wrapper.wrap(sources, path=args.out)
-    generate_content(cc_content, args.out)

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)
+    sources = args.src.split(';')
-
+    wrapper.wrap(sources, args.out)
-    # Generate the C++ code which Pybind11 will use.
-    with open(args.out, "w") as f:
-        f.write(cc_content)
 
 
 if __name__ == "__main__":

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"

wrap/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

wrap/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

wrap/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

wrap/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);
   }
 }

wrap/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);
   }
 }

wrap/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);
   }
 }

wrap/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,42 +63,38 @@ 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[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_gtsamPoint2.insert(self);
-}
-
 void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
@@ -214,6 +104,15 @@ void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin
   collector_MyBase.insert(self);
 }
 
+void MyBase_upcastFromVoid_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyBase> 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<MyBase>(*asVoid));
+  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
+}
+
 void MyBase_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyBase> Shared;
@@ -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);

wrap/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);
+}

wrap/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);
   }
 }

wrap/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);
   }
 }

wrap/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

wrap/tests/fixtures/part2.i

@@ -0,0 +1,7 @@
+// Second file to test for multi-file support.
+
+namespace gtsam {
+class ClassA {
+  ClassA();
+};
+}  // namespace gtsam

wrap/tests/test_matlab_wrapper.py

@@ -22,74 +22,32 @@ class TestWrap(unittest.TestCase):
     """
     Test the Matlab wrapper
     """
-    TEST_DIR = osp.dirname(osp.realpath(__file__))
+    def setUp(self) -> None:
-    INTERFACE_DIR = osp.join(TEST_DIR, "fixtures")
+        super().setUp()
-    MATLAB_TEST_DIR = osp.join(TEST_DIR, "expected", "matlab")
+
-    MATLAB_ACTUAL_DIR = osp.join(TEST_DIR, "actual", "matlab")
+        # 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(MATLAB_ACTUAL_DIR, exist_ok=True)
+        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 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 compare_and_diff(self, file):
         """
         Compute the comparison between the expected and actual file,
@@ -109,11 +67,7 @@ class TestWrap(unittest.TestCase):
         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:
+        file = osp.join(self.INTERFACE_DIR, 'geometry.i')
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
 
         # Create MATLAB wrapper instance
         wrapper = MatlabWrapper(
@@ -122,24 +76,18 @@ class TestWrap(unittest.TestCase):
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap(content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
-
-        self.generate_content(cc_content)
-
-        self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam')))
 
         files = ['+gtsam/Point2.m', '+gtsam/Point3.m', 'geometry_wrapper.cpp']
 
+        self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam')))
+
         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:
+        file = osp.join(self.INTERFACE_DIR, 'functions.i')
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
 
         wrapper = MatlabWrapper(
             module_name='functions',
@@ -147,9 +95,7 @@ class TestWrap(unittest.TestCase):
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap(content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
-
-        self.generate_content(cc_content)
 
         files = [
             'functions_wrapper.cpp', 'aGlobalFunction.m', 'load2D.m',
@@ -163,11 +109,7 @@ class TestWrap(unittest.TestCase):
 
     def test_class(self):
         """Test interface file with only class info."""
-        with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f:
+        file = osp.join(self.INTERFACE_DIR, 'class.i')
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
 
         wrapper = MatlabWrapper(
             module_name='class',
@@ -175,9 +117,7 @@ class TestWrap(unittest.TestCase):
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap(content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
-
-        self.generate_content(cc_content)
 
         files = [
             'class_wrapper.cpp', 'FunDouble.m', 'FunRange.m',
@@ -191,21 +131,14 @@ class TestWrap(unittest.TestCase):
 
     def test_inheritance(self):
         """Test interface file with class inheritance definitions."""
-        with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f:
+        file = osp.join(self.INTERFACE_DIR, 'inheritance.i')
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
 
         wrapper = MatlabWrapper(
             module_name='inheritance',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
-
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
-        cc_content = wrapper.wrap(content)
-
-        self.generate_content(cc_content)
 
         files = [
             'inheritance_wrapper.cpp', 'MyBase.m', 'MyTemplateMatrix.m',
@@ -219,11 +152,7 @@ class TestWrap(unittest.TestCase):
         """
         Test interface file with full namespace definition.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f:
+        file = osp.join(self.INTERFACE_DIR, 'namespaces.i')
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
 
         wrapper = MatlabWrapper(
             module_name='namespaces',
@@ -231,9 +160,7 @@ class TestWrap(unittest.TestCase):
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap(content)
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
-
-        self.generate_content(cc_content)
 
         files = [
             'namespaces_wrapper.cpp', '+ns1/aGlobalFunction.m',
@@ -249,21 +176,14 @@ class TestWrap(unittest.TestCase):
         """
         Tests for some unique, non-trivial features.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f:
+        file = osp.join(self.INTERFACE_DIR, 'special_cases.i')
-            content = f.read()
-
-        if not osp.exists(self.MATLAB_ACTUAL_DIR):
-            os.mkdir(self.MATLAB_ACTUAL_DIR)
 
         wrapper = MatlabWrapper(
             module_name='special_cases',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
-
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
-        cc_content = wrapper.wrap(content)
-
-        self.generate_content(cc_content)
 
         files = [
             'special_cases_wrapper.cpp',
@@ -274,6 +194,31 @@ class TestWrap(unittest.TestCase):
         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()

wrap/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 @@ class TestWrap(unittest.TestCase):
                                 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:
+        wrapper.wrap(sources, output)
-            f.write(cc_content)
 
         return output
 
@@ -76,39 +73,32 @@ class TestWrap(unittest.TestCase):
         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:
+        source = osp.join(self.INTERFACE_DIR, 'geometry.i')
-            content = f.read()
+        output = self.wrap_content([source], 'geometry_py',
-
-        output = self.wrap_content(content, '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:
+        source = osp.join(self.INTERFACE_DIR, 'functions.i')
-            content = f.read()
+        output = self.wrap_content([source], 'functions_py',
-
-        output = self.wrap_content(content, '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:
+        source = osp.join(self.INTERFACE_DIR, 'class.i')
-            content = f.read()
+        output = self.wrap_content([source], 'class_py',
-
+                                   self.PYTHON_ACTUAL_DIR)
-        output = self.wrap_content(content, '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:
+        source = osp.join(self.INTERFACE_DIR, 'inheritance.i')
-            content = f.read()
+        output = self.wrap_content([source], 'inheritance_py',
-
-        output = self.wrap_content(content, 'inheritance_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('inheritance_pybind.cpp', output)
@@ -119,10 +109,8 @@ class TestWrap(unittest.TestCase):
         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:
+        source = osp.join(self.INTERFACE_DIR, 'namespaces.i')
-            content = f.read()
+        output = self.wrap_content([source], 'namespaces_py',
-
-        output = self.wrap_content(content, 'namespaces_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('namespaces_pybind.cpp', output)
@@ -131,10 +119,8 @@ class TestWrap(unittest.TestCase):
         """
         Tests for operator overloading.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'operator.i'), 'r') as f:
+        source = osp.join(self.INTERFACE_DIR, 'operator.i')
-            content = f.read()
+        output = self.wrap_content([source], 'operator_py',
-
-        output = self.wrap_content(content, 'operator_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('operator_pybind.cpp', output)
@@ -143,10 +129,8 @@ class TestWrap(unittest.TestCase):
         """
         Tests for some unique, non-trivial features.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f:
+        source = osp.join(self.INTERFACE_DIR, 'special_cases.i')
-            content = f.read()
+        output = self.wrap_content([source], 'special_cases_py',
-
-        output = self.wrap_content(content, 'special_cases_py',
                                    self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('special_cases_pybind.cpp', output)
@@ -155,10 +139,8 @@ class TestWrap(unittest.TestCase):
         """
         Test if enum generation is correct.
         """
-        with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f:
+        source = osp.join(self.INTERFACE_DIR, 'enum.i')
-            content = f.read()
+        output = self.wrap_content([source], 'enum_py', self.PYTHON_ACTUAL_DIR)
-
-        output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('enum_pybind.cpp', output)