diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index b6559eac0..d5b225535 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -23,6 +23,10 @@
 #include <boost/lexical_cast.hpp>
 #include <boost/range/adaptor/map.hpp>
 #include <boost/range/algorithm/copy.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/iterator/zip_iterator.hpp>
+#include <boost/range/combine.hpp>
+#include <boost/range/algorithm/remove_if.hpp>
 
 #include <vector> 
 #include <iostream> 
@@ -436,6 +440,75 @@ void Class::appendInheritedMethods(const Class& cls,
   }
 }
 
+/* ************************************************************************* */
+void Class::removeInheritedMethods(vector<Class>& classes) {
+  if (parentClass) {
+    // Find parent
+    for(Class& parent: classes) {
+      // We found a parent class for our parent, TODO So complicated! Improve !
+      if (parent.name() == parentClass->name()) {
+        // make sure parent is clean (no inherited method from grand-parent)
+        parent.removeInheritedMethods(classes);
+
+        // check each method
+        for(const string& methodName: nontemplateMethods_ | boost::adaptors::map_keys) {
+          cout << methodName << endl;
+        }
+        for(const string& methodName: nontemplateMethods_ | boost::adaptors::map_keys) {
+          // Check if the method exists in its parent
+          auto it = parent.nontemplateMethods_.find(methodName);
+          if (it == parent.nontemplateMethods_.end()) continue; // if not: ignore!
+
+          cout << "Duplicate method name: " << methodName << endl;
+
+          Method& parentMethod = it->second;
+          Method& method = nontemplateMethods_[methodName];
+          // check if they have the same modifiers (const/static/templateArgs)
+          if (!method.isSameModifiers(parentMethod)) continue; // if not: ignore
+
+          cout << "same modifiers!" << endl;
+
+          // check and remove duplicate overloads
+          auto methodOverloads = boost::combine(method.returnVals_, method.argLists_);
+          auto parentMethodOverloads = boost::combine(parentMethod.returnVals_, parentMethod.argLists_);
+          auto result = boost::remove_if(
+              methodOverloads,
+              [&](boost::tuple<ReturnValue, ArgumentList> const& overload) {
+                  bool found = std::find_if(
+                             parentMethodOverloads.begin(),
+                             parentMethodOverloads.end(),
+                             [&](boost::tuple<ReturnValue, ArgumentList> const&
+                                     parentOverload) {
+                                 cout << "checking overload of " << name() << ": " << overload.get<0>() << " vs " << parentOverload.get<0>() << endl;
+                                 cout << "  argslist 1:" << overload.get<1>();
+                                 cout << endl;
+                                 cout << "  argslist 2:" << parentOverload.get<1>();
+                                 cout << endl;
+                                 return overload.get<0>() == parentOverload.get<0>() &&
+                                        overload.get<1>() == parentOverload.get<1>();
+                             }) != parentMethodOverloads.end();
+                  cout << "SAME: " << found << endl;
+                  return found;
+              });
+
+          method.returnVals_.erase(boost::get<0>(result.get_iterator_tuple()),
+                                   method.returnVals_.end());
+          method.argLists_.erase(boost::get<1>(result.get_iterator_tuple()),
+                                     method.argLists_.end());
+        }
+        for (auto it = nontemplateMethods_.begin(),
+                  ite = nontemplateMethods_.end();
+             it != ite;) {
+            if (it->second.nrOverloads() == 0)
+                it = nontemplateMethods_.erase(it);
+            else
+                ++it;
+        }
+      }
+    }
+  }
+}
+
 /* ************************************************************************* */
 string Class::getTypedef() const {
   string result;
@@ -674,7 +747,7 @@ void Class::python_wrapper(FileWriter& wrapperFile) const {
 }
 
 /* ************************************************************************* */
-void Class::emit_cython_pxd(FileWriter& pxdFile) const {
+void Class::emit_cython_pxd(FileWriter& pxdFile, const std::vector<Class>& allClasses) const {
   pxdFile.oss << "cdef extern from \"" << includeFile << "\" namespace \""
                 << qualifiedNamespaces("::") << "\":" << endl;
   pxdFile.oss << "\tcdef cppclass " << cythonClass() << " \"" << qualifiedName("::") << "\"";
@@ -698,6 +771,7 @@ void Class::emit_cython_pxd(FileWriter& pxdFile) const {
 
   for(const Method& m: nontemplateMethods_ | boost::adaptors::map_values)
     m.emit_cython_pxd(pxdFile, *this);
+
   for(const TemplateMethod& m: templateMethods_ | boost::adaptors::map_values)
     m.emit_cython_pxd(pxdFile, *this);
   size_t numMethods = constructor.nrOverloads() + static_methods.size() +
@@ -709,8 +783,10 @@ void Class::emit_cython_pxd(FileWriter& pxdFile) const {
 }
 
 /* ************************************************************************* */
-void Class::pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj, const std::string& cySharedObj, const std::vector<Class>& allClasses) const {
-  if (parentClass) {
+void Class::pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj,
+                             const std::string& cySharedObj,
+                             const std::vector<Class>& allClasses) const {
+    if (parentClass) {
       pyxFile.oss << pyObj << "." << parentClass->pyxCythonObj() << " = "
                   << "<" << parentClass->pyxSharedCythonClass() << ">("
                   << cySharedObj << ")\n";
diff --git a/wrap/Class.h b/wrap/Class.h
index 46376e377..1ec53bf1c 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -134,6 +134,8 @@ public:
   void appendInheritedMethods(const Class& cls,
       const std::vector<Class>& classes);
 
+  void removeInheritedMethods(std::vector<Class>& classes);
+
   /// The typedef line for this class, if this class is a typedef, otherwise returns an empty string.
   std::string getTypedef() const;
 
@@ -152,7 +154,7 @@ public:
   void python_wrapper(FileWriter& wrapperFile) const;
 
   // emit cython wrapper
-  void emit_cython_pxd(FileWriter& pxdFile) const;
+  void emit_cython_pxd(FileWriter& pxdFile, const std::vector<Class>& allClasses) const;
   void emit_cython_pyx(FileWriter& pyxFile,
                        const std::vector<Class>& allClasses) const;
   void pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj,
diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h
index 80d974d88..645b7cf82 100644
--- a/wrap/FullyOverloadedFunction.h
+++ b/wrap/FullyOverloadedFunction.h
@@ -27,7 +27,7 @@ namespace wrap {
  */
 class SignatureOverloads: public ArgumentOverloads {
 
-protected:
+public:
 
   std::vector<ReturnValue> returnVals_;
 
diff --git a/wrap/Method.h b/wrap/Method.h
index 299799f9b..a6705961a 100644
--- a/wrap/Method.h
+++ b/wrap/Method.h
@@ -45,6 +45,12 @@ public:
     return is_const_;
   }
 
+  bool isSameModifiers(const Method& other) const {
+      return is_const_ == other.is_const_ &&
+             ((templateArgValue_ && other.templateArgValue_) ||
+              (!templateArgValue_ && !other.templateArgValue_));
+  }
+
   friend std::ostream& operator<<(std::ostream& os, const Method& m) {
     for (size_t i = 0; i < m.nrOverloads(); i++)
       os << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index 7fe0cfd9e..40027abc3 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -193,6 +193,10 @@ void Module::parseMarkup(const std::string& data) {
   for(Class& cls: classes)
     cls.appendInheritedMethods(cls, classes);
 
+  for(Class& cls: uninstantiatedClasses) {
+      cls.removeInheritedMethods(uninstantiatedClasses);
+  }
+
   // Expand templates - This is done first so that template instantiations are
   // counted in the list of valid types, have their attributes and dependencies
   // checked, etc.
@@ -315,8 +319,9 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
 
   //... wrap all classes
   for(const Class& cls: uninstantiatedClasses)
-    cls.emit_cython_pxd(pxdFile);
+    cls.emit_cython_pxd(pxdFile, uninstantiatedClasses);
   //... ctypedef for template instantiations
+  // TODO: put them in the correct place!
   for(const Class& cls: expandedClasses) {
     if (cls.templateClass) {
       pxdFile.oss << "ctypedef " << cls.templateClass->cythonClass() << "[";
diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h
index 55e581ad8..5c200c641 100644
--- a/wrap/OverloadedFunction.h
+++ b/wrap/OverloadedFunction.h
@@ -28,7 +28,7 @@ namespace wrap {
  */
 class ArgumentOverloads {
 
-protected:
+public:
 
   std::vector<ArgumentList> argLists_;
 
diff --git a/wrap/tests/cythontest.h b/wrap/tests/cythontest.h
index ab32d63fe..c4f082ccb 100644
--- a/wrap/tests/cythontest.h
+++ b/wrap/tests/cythontest.h
@@ -15,6 +15,7 @@ typedef gtsam::FastSet<size_t> KeySet;
 #include <gtsam/base/FastMap.h>
 template<K,V> class FastMap{};
 
+#include <gtsam/base/Value.h>
 virtual class Value {
   // No constructors because this is an abstract class
 
@@ -803,6 +804,344 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
     gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
     double rank_tol, bool optimize);
 
+//*************************************************************************
+// Inference
+//*************************************************************************
+
+#include <gtsam/inference/Ordering.h>
+class Ordering {
+  // Standard Constructors and Named Constructors
+  Ordering();
+  Ordering(const gtsam::Ordering& other);
+
+  // Testable
+  void print(string s) 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;
+};
+
+
+//*************************************************************************
+// Symbolic
+//*************************************************************************
+
+#include <gtsam/symbolic/SymbolicFactor.h>
+virtual class SymbolicFactor {
+  // Standard Constructors and Named Constructors
+  SymbolicFactor(const gtsam::SymbolicFactor& f);
+  SymbolicFactor();
+  SymbolicFactor(size_t j);
+  SymbolicFactor(size_t j1, size_t j2);
+  SymbolicFactor(size_t j1, size_t j2, size_t j3);
+  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4);
+  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5);
+  SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6);
+  // static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js);
+
+  // From Factor
+  size_t size() const;
+  void print(string s) 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) 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(gtsam::SymbolicFactor* factor);
+  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& variables);
+  // gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables);
+  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables,
+      const gtsam::Ordering& marginalizedVariableOrdering);
+  // gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables,
+  //     const gtsam::Ordering& marginalizedVariableOrdering);
+  // gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables);
+};
+
+#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;
+  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) 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);
+    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 {
+//   BayesTreeClique();
+//   BayesTreeClique(CONDITIONAL* conditional);
+// //  BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
+//
+//   bool equals(const This& other, double tol) const;
+//   void print(string s) const;
+//   void printTree() const; // Default indent of ""
+//   void printTree(string indent) const;
+//   size_t numCachedSeparatorMarginals() const;
+//
+//   CONDITIONAL* conditional() const;
+//   bool isRoot() const;
+//   size_t treeSize() const;
+// //  const std::list<derived_ptr>& children() const { return children_; }
+// //  derived_ptr parent() const { return parent_.lock(); }
+//
+//   // FIXME: 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& factorGraph);
+//   VariableIndex(const gtsam::GaussianFactorGraph& factorGraph);
+//   VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph);
+//   VariableIndex(const gtsam::VariableIndex& other);
+
+//   // Testable
+//   bool equals(const gtsam::VariableIndex& other, double tol) const;
+//   void print(string s) const;
+
+//   // Standard interface
+//   size_t size() const;
+//   size_t nFactors() const;
+//   size_t nEntries() const;
+// };
+
+//*************************************************************************
+// linear
+//*************************************************************************
+
+namespace noiseModel {
+#include <gtsam/linear/NoiseModel.h>
+virtual class Base {
+};
+
+virtual class Gaussian : gtsam::noiseModel::Base {
+  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
+  static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
+  Matrix R() const;
+  bool equals(gtsam::noiseModel::Base& expected, double tol);
+  void print(string s) 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;
+  // void print(string s) const;
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+virtual class Constrained : gtsam::noiseModel::Diagonal {
+    static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas);
+    static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas);
+    static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances);
+    static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances);
+    static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions);
+    static gtsam::noiseModel::Constrained* MixedPrecisions(const 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);
+  // void print(string s) const;
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+virtual class Unit : gtsam::noiseModel::Isotropic {
+  static gtsam::noiseModel::Unit* Create(size_t dim);
+  // void print(string s) const;
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+namespace mEstimator {
+virtual class Base {
+};
+
+virtual class Null: gtsam::noiseModel::mEstimator::Base {
+  Null();
+  // void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Null* Create();
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+virtual class Fair: gtsam::noiseModel::mEstimator::Base {
+  Fair(double c);
+  // void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Fair* Create(double c);
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+virtual class Huber: gtsam::noiseModel::mEstimator::Base {
+  Huber(double k);
+  // void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Huber* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
+  Tukey(double k);
+  // void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() 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);
+  // void print(string s) const;
+
+  // 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);
+  Sampler(int seed);
+
+    //Standard Interface
+  size_t dim() const;
+  Vector sigmas() const;
+  gtsam::noiseModel::Diagonal* model() const;
+  Vector sample();
+  Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
+};
 
 #include <gtsam/linear/VectorValues.h>
 class VectorValues {
@@ -842,73 +1181,6 @@ class VectorValues {
   void serialize() const;
 };
 
-namespace noiseModel {
-#include <gtsam/linear/NoiseModel.h>
-virtual class Base {
-};
-
-virtual class Gaussian : gtsam::noiseModel::Base {
-  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
-  static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
-  Matrix R() const;
-  bool equals(gtsam::noiseModel::Base& expected, double tol);
-  void print(string s) 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);
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-namespace mEstimator {
-virtual class Base {
-};
-
-virtual class Null: gtsam::noiseModel::mEstimator::Base {
-  Null();
-  void print(string s) const;
-  static gtsam::noiseModel::mEstimator::Null* Create();
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Fair: gtsam::noiseModel::mEstimator::Base {
-  Fair(double c);
-  void print(string s) const;
-  static gtsam::noiseModel::mEstimator::Fair* Create(double c);
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Huber: gtsam::noiseModel::mEstimator::Base {
-  Huber(double k);
-  void print(string s) const;
-  static gtsam::noiseModel::mEstimator::Huber* Create(double k);
-
-  // enabling serialization functionality
-  void serializable() 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);
-  void print(string s) const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-} // namespace noiseModel
-
 #include <gtsam/linear/GaussianFactor.h>
 virtual class GaussianFactor {
   // gtsam::KeyVector keys() const;
@@ -937,12 +1209,16 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
       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);
+  JacobianFactor(const gtsam::GaussianFactorGraph& graph);
 
   //Testable
+  // void print(string s) 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 py_getA() const;
@@ -950,13 +1226,13 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   size_t rows() const;
   size_t cols() const;
   bool isConstrained() const;
-  // pair<Matrix, Vector> jacobianUnweighted() const;
+  pair<Matrix, Vector> jacobianUnweighted() const;
   Matrix augmentedJacobianUnweighted() const;
 
   void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
   gtsam::JacobianFactor whiten() const;
 
-  // pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
+  pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
 
   void setModel(bool anyConstrained, const Vector& sigmas);
 
@@ -966,133 +1242,1303 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   void serialize() const;
 };
 
-#include <gtsam/nonlinear/Values.h>
-class Values {
-  Values();
-  Values(const gtsam::Values& other);
+#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;
-  bool empty() const;
-  void clear();
-  size_t dim() const;
-
-  void print(string s) 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;
-
-  // 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;
-
-  template<T = {gtsam::Point3, gtsam::Rot3, Matrix, Vector}>
-  void insert(size_t j, const T& t);
-
-  template<T = {gtsam::Point3, gtsam::Rot3, Matrix, Vector}>
-  void update(size_t j, const T& t);
-
-  template<T = {gtsam::Point3, gtsam::Rot3, Vector, Matrix}>
-  T at(size_t j);
-
-  /// version for double
-  void insertDouble(size_t j, double c);
-  double atDouble(size_t j) 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;
   void printKeys(string s) const;
-  // NonlinearFactor
-  void 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; //FIXME: Conversion from KeyVector to std::vector does not happen
+  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;
 };
 
+#include <gtsam/linear/GaussianFactorGraph.h>
+class GaussianFactorGraph {
+  GaussianFactorGraph();
+  GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet);
+  GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree);
 
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-class NonlinearFactorGraph {
-  NonlinearFactorGraph();
-  NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
-
-  // FactorGraph
+  // From FactorGraph
   void print(string s) const;
-  bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) 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;
+  bool exists(size_t idx) const;
+
+  // Building the graph
+  void push_back(const gtsam::GaussianFactor* factor);
+  void push_back(const gtsam::GaussianConditional* factor);
+  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& variables);
+  // gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables);
+  gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables,
+    const gtsam::Ordering& marginalizedVariableOrdering);
+  // gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables,
+  //   const gtsam::Ordering& marginalizedVariableOrdering);
+  // gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables);
+
+  // 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;
+};
+
+#include <gtsam/linear/GaussianConditional.h>
+virtual class GaussianConditional : gtsam::GaussianFactor {
+  //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) 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;
+
+  // 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) 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;
+  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;
+
+  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);
   size_t size() const;
   bool empty() const;
-  void remove(size_t i);
-  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;
+  size_t numCachedSeparatorMarginals() const;
+  void saveGraph(string s) const;
 
-  // NonlinearFactorGraph
-  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;
+  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);
 
-virtual class NoiseModelFactor: gtsam::NonlinearFactor {
-  void equals(const gtsam::NoiseModelFactor& other, double tol) const;
-  gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
-  gtsam::noiseModel::Base* noiseModel() const;
-  Vector unwhitenedError(const gtsam::Values& x) const;
-  Vector whitenedError(const gtsam::Values& x) const;
+    //Testable
+    void print(string s);
+    bool equals(const gtsam::Errors& expected, double tol) const;
 };
 
-#include <gtsam/slam/BetweenFactor.h>
-template<T = {gtsam::Point3, gtsam::Rot3}>
-virtual class BetweenFactor : gtsam::NoiseModelFactor {
-  BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
-  T measured() const;
+#include <gtsam/linear/GaussianISAM.h>
+class GaussianISAM {
+  //Constructor
+  GaussianISAM();
 
-  // enabling serialization functionality
-  void serialize() const;
+  //Standard Interface
+  void update(const gtsam::GaussianFactorGraph& newFactors);
+  void saveGraph(string s) const;
+  void clear();
 };
 
-#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;
+#include <gtsam/linear/IterativeSolver.h>
+virtual class IterativeOptimizationParameters {
+  string getVerbosity() const;
+  void setVerbosity(string s) ;
+  void print() const;
 };
 
-typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
+//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);
+  // void print();
+};
+
+#include <gtsam/linear/SubgraphSolver.h>
+virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
+  SubgraphSolverParameters();
+  // void print() const;
+};
+
+virtual class SubgraphSolver  {
+  SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
+  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
+  gtsam::VectorValues optimize() const;
+};
+
+#include <gtsam/linear/KalmanFilter.h>
+class KalmanFilter {
+  KalmanFilter(size_t n);
+  // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
+  gtsam::GaussianDensity* init(Vector x0, Matrix P0);
+  void print(string s) const;
+  static size_t step(gtsam::GaussianDensity* p);
+  gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
+      Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ);
+  gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F,
+      Matrix B, Vector u, Matrix Q);
+  gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0,
+      Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
+  gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H,
+      Vector z, const gtsam::noiseModel::Diagonal* model);
+  gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H,
+      Vector z, Matrix Q);
+};
+
+// //*************************************************************************
+// // nonlinear
+// //*************************************************************************
+
+// #include <gtsam/inference/Symbol.h>
+// size_t symbol(char chr, size_t index);
+// char symbolChr(size_t key);
+// size_t symbolIndex(size_t key);
+
+// // 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/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;
+//   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;
+
+//   // 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;
+
+//   template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
+//                  gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+//                  gtsam::Cal3Bundler, gtsam::EssentialMatrix,
+//                  gtsam::SimpleCamera, gtsam::imuBias::ConstantBias,
+//                  gtsam::Vector, gtsam::Matrix}>
+//   void insert(size_t j, const T& t);
+
+//   template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
+//                  gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+//                  gtsam::Cal3Bundler, gtsam::EssentialMatrix,
+//                  gtsam::SimpleCamera, gtsam::imuBias::ConstantBias,
+//                  gtsam::Vector, gtsam::Matrix}>
+//   void update(size_t j, const T& t);
+
+//   template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
+//   T at(size_t j);
+
+//   /// version for double
+//   void insertDouble(size_t j, double c);
+//   double atDouble(size_t j) 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;
+//   void printKeys(string s) const;
+//   // NonlinearFactor
+//   void 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; //FIXME: Conversion from KeyVector to std::vector does not happen
+// };
+
+// #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+// class NonlinearFactorGraph {
+//   NonlinearFactorGraph();
+//   NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
+
+//   // FactorGraph
+//   void print(string s) const;
+//   bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
+//   size_t size() const;
+//   bool empty() const;
+//   void remove(size_t i);
+//   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;
+
+//   // NonlinearFactorGraph
+//   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;
+// };
+
+// #include <gtsam/nonlinear/NonlinearFactor.h>
+// virtual class NoiseModelFactor: gtsam::NonlinearFactor {
+//   void equals(const gtsam::NoiseModelFactor& other, double tol) const;
+//   gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
+//   gtsam::noiseModel::Base* noiseModel() const;
+//   Vector unwhitenedError(const gtsam::Values& x) const;
+//   Vector whitenedError(const gtsam::Values& x) const;
+// };
+
+// // // Actually a FastList<Key>
+// // #include <gtsam/inference/Key.h>
+// // class KeyList {
+// //   KeyList();
+// //   KeyList(const gtsam::KeyList& other);
+
+// //   // Note: no print function
+
+// //   // common STL methods
+// //   size_t size() const;
+// //   bool empty() const;
+// //   void clear();
+
+// //   // structure specific methods
+// //   size_t front() const;
+// //   size_t back() const;
+// //   void push_back(size_t key);
+// //   void push_front(size_t key);
+// //   void pop_back();
+// //   void pop_front();
+// //   void sort();
+// //   void remove(size_t key);
+
+// //   void serialize() const;
+// // };
+
+// // // Actually a FastSet<Key>
+// // class KeySet {
+// //   KeySet();
+// //   KeySet(const gtsam::KeySet& other);
+// //   KeySet(const gtsam::KeyVector& other);
+// //   KeySet(const gtsam::KeyList& other);
+
+// //   // Testable
+// //   void print(string s) const;
+// //   bool equals(const gtsam::KeySet& other) const;
+
+// //   // common STL methods
+// //   size_t size() const;
+// //   bool empty() const;
+// //   void clear();
+
+// //   // structure specific methods
+// //   void insert(size_t key);
+// //   void merge(gtsam::KeySet& other);
+// //   bool erase(size_t key); // returns true if value was removed
+// //   bool count(size_t key) const; // returns true if value exists
+
+// //   void serialize() const;
+// // };
+
+// // // Actually a vector<Key>
+// // class KeyVector {
+// //   KeyVector();
+// //   KeyVector(const gtsam::KeyVector& other);
+
+// //   // Note: no print function
+
+// //   // common STL methods
+// //   size_t size() const;
+// //   bool empty() const;
+// //   void clear();
+
+// //   // structure specific methods
+// //   size_t at(size_t i) const;
+// //   size_t front() const;
+// //   size_t back() const;
+// //   void push_back(size_t key) const;
+
+// //   void serialize() const;
+// // };
+
+// // // Actually a FastMap<Key,int>
+// // class KeyGroupMap {
+// //   KeyGroupMap();
+
+// //   // Note: no print function
+
+// //   // common STL methods
+// //   size_t size() const;
+// //   bool empty() const;
+// //   void clear();
+
+// //   // structure specific methods
+// //   size_t at(size_t key) const;
+// //   int erase(size_t key);
+// //   bool insert2(size_t key, int val);
+// // };
+
+// #include <gtsam/nonlinear/Marginals.h>
+// class Marginals {
+//   Marginals(const gtsam::NonlinearFactorGraph& graph,
+//       const gtsam::Values& solution);
+
+//   void print(string s) 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) const;
+//   // void print() 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 setOrdering(const gtsam::Ordering& ordering);
+//   void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
+
+//   bool isMultifrontal() const;
+//   bool isSequential() const;
+//   bool isCholmod() const;
+//   bool isIterative() const;
+// };
+
+// bool checkConvergence(double relativeErrorTreshold,
+//     double absoluteErrorTreshold, double errorThreshold,
+//     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();
+
+//   double getlambdaInitial() const;
+//   double getlambdaFactor() const;
+//   double getlambdaUpperBound() const;
+//   string getVerbosityLM() const;
+
+//   void setlambdaInitial(double value);
+//   void setlambdaFactor(double value);
+//   void setlambdaUpperBound(double value);
+//   void setVerbosityLM(string s);
+// };
+
+// #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;
+//   void 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 str) const;
+// };
+
+// #include <gtsam/nonlinear/ISAM2.h>
+// class ISAM2GaussNewtonParams {
+//   ISAM2GaussNewtonParams();
+
+//   void print(string str) const;
+
+//   /** Getters and Setters for all properties */
+//   double getWildfireThreshold() const;
+//   void setWildfireThreshold(double wildfireThreshold);
+// };
+
+// class ISAM2DoglegParams {
+//   ISAM2DoglegParams();
+
+//   void print(string str) 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 str) const;
+
+//   /** Getters and Setters for all properties */
+//   void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params);
+//   void setOptimizationParams(const gtsam::ISAM2DoglegParams& params);
+//   void setRelinearizeThreshold(double relinearizeThreshold);
+//   void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold);
+//   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);
+// };
+
+// class ISAM2Result {
+//   ISAM2Result();
+
+//   void print(string str) const;
+
+//   /** Getters and Setters for all properties */
+//   size_t getVariablesRelinearized() const;
+//   size_t getVariablesReeliminated() const;
+//   size_t getCliques() const;
+// };
+
+// class FactorIndices {};
+
+// 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;
+//   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);
+//   // TODO: wrap the full version of update
+//  //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
+//   //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);
+
+//   gtsam::Values getLinearizationPoint() const;
+//   gtsam::Values calculateEstimate() const;
+//   gtsam::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;
+//   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/geometry/SimpleCamera.h>
+// #include <gtsam/geometry/CalibratedCamera.h>
+// #include <gtsam/geometry/StereoPoint2.h>
+
+// #include <gtsam/slam/PriorFactor.h>
+// template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, 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;
+// };
 
 
-}
\ No newline at end of file
+// #include <gtsam/slam/BetweenFactor.h>
+// template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, 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;
+// };
+
+
+
+// #include <gtsam/nonlinear/NonlinearEquality.h>
+// template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+// virtual class NonlinearEquality : gtsam::NoiseModelFactor {
+//   // Constructor - forces exact evaluation
+//   NonlinearEquality(size_t j, const T& feasible);
+//   // Constructor - allows inexact evaluation
+//   NonlinearEquality(size_t j, const T& feasible, double error_gain);
+
+//   // enabling serialization functionality
+//   void serialize() const;
+// };
+
+
+// #include <gtsam/sam/RangeFactor.h>
+// template<POSE, POINT>
+// virtual class RangeFactor : gtsam::NoiseModelFactor {
+//   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
+// };
+
+// typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
+// typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
+// 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::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
+// typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
+// typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
+
+
+// #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;
+
+// #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);
+
+//   // enabling serialization functionality
+//   void serialize() const;
+// };
+
+// typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
+
+
+// #include <gtsam/slam/ProjectionFactor.h>
+// template<POSE, LANDMARK, CALIBRATION>
+// virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
+//   GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
+//     size_t poseKey, size_t pointKey, const CALIBRATION* k);
+//   GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
+//     size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor);
+
+//   GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
+//       size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
+//     GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
+//       size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality,
+//       const POSE& body_P_sensor);
+
+//   gtsam::Point2 measured() const;
+//   CALIBRATION* calibration() const;
+//   bool verboseCheirality() const;
+//   bool throwCheirality() const;
+
+//   // enabling serialization functionality
+//   void serialize() const;
+// };
+// typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
+// typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
+
+
+// #include <gtsam/slam/GeneralSFMFactor.h>
+// template<CAMERA, LANDMARK>
+// virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
+//   GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
+//   gtsam::Point2 measured() const;
+// };
+// typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
+// typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+
+// template<CALIBRATION = {gtsam::Cal3_S2}>
+// 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>
+// class SmartProjectionParams {
+//   SmartProjectionParams();
+//   // TODO(frank): make these work:
+//   //  void setLinearizationMode(LinearizationMode linMode);
+//   //  void setDegeneracyMode(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::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>
+// pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
+//     gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
+// pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
+//     gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
+// pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
+//     gtsam::noiseModel::Diagonal* model, int maxID);
+// 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);
+// void save2D(const gtsam::NonlinearFactorGraph& graph,
+//     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
+//     string filename);
+
+// pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+// void writeG2o(const gtsam::NonlinearFactorGraph& graph,
+//     const gtsam::Values& estimate, string filename);
+
+// //*************************************************************************
+// // Navigation
+// //*************************************************************************
+// namespace imuBias {
+// #include <gtsam/navigation/ImuBias.h>
+
+// class ConstantBias {
+//   // Constructors
+//   ConstantBias();
+//   ConstantBias(Vector biasAcc, Vector biasGyro);
+
+//   // Testable
+//   void print(string s) const;
+//   bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
+
+//   // Group
+//   static gtsam::imuBias::ConstantBias identity();
+//   gtsam::imuBias::ConstantBias inverse() const;
+//   gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const;
+//   gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
+
+//   // 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;
+// };
+
+// #include <gtsam/navigation/PreintegratedRotation.h>
+// virtual class PreintegratedRotationParams {
+//   PreintegratedRotationParams();
+//   void setGyroscopeCovariance(Matrix cov);
+//   void setOmegaCoriolis(Vector omega);
+//   void setBodyPSensor(const gtsam::Pose3& pose);
+
+//   Matrix getGyroscopeCovariance() const;
+
+//   // TODO(frank): allow optional
+//   //  boost::optional<Vector3> getOmegaCoriolis() const;
+//   //  boost::optional<Pose3>   getBodyPSensor()   const;
+// };
+
+// #include <gtsam/navigation/PreintegrationParams.h>
+// virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
+//   PreintegrationParams(Vector n_gravity);
+//   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();
+//   Matrix preintMeasCov() const;
+//   double deltaTij() const;
+//   gtsam::Rot3 deltaRij() const;
+//   Vector deltaPij() const;
+//   Vector deltaVij() 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>
+// class PreintegratedCombinedMeasurements {
+//   // Testable
+//   void print(string s) const;
+//   bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
+//       double tol);
+
+//   // Standard Interface
+//   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
+//       double deltaT);
+//   void resetIntegration();
+//   Matrix preintMeasCov() const;
+//   double deltaTij() const;
+//   gtsam::Rot3 deltaRij() const;
+//   Vector deltaPij() const;
+//   Vector deltaVij() 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(Vector bias, Matrix measuredOmegaCovariance);
+//   PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
+
+//   // Testable
+//   void print(string s) 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;
+//   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;
+//   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
+//   gtsam::Unit3 nZ() const;
+//   gtsam::Unit3 bRef() const;
+// };
+
+// //*************************************************************************
+// // Utilities
+// //*************************************************************************
+
+// namespace utilities {
+
+//   #include <matlab.h>
+//   gtsam::KeyList createKeyList(Vector I);
+//   gtsam::KeyList createKeyList(string s, Vector I);
+//   gtsam::KeyVector createKeyVector(Vector I);
+//   gtsam::KeyVector createKeyVector(string s, Vector I);
+//   gtsam::KeySet createKeySet(Vector I);
+//   gtsam::KeySet createKeySet(string s, Vector I);
+//   Matrix extractPoint2(const gtsam::Values& values);
+//   Matrix extractPoint3(const gtsam::Values& values);
+//   Matrix extractPose2(const gtsam::Values& values);
+//   gtsam::Values allPose3s(gtsam::Values& values);
+//   Matrix extractPose3(const gtsam::Values& values);
+//   void perturbPoint2(gtsam::Values& values, double sigma, int seed);
+//   void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
+//   void perturbPoint3(gtsam::Values& values, double sigma, int seed);
+//   void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
+//   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
+//   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
+//   Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
+//   gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
+//   gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
+
+// } //\namespace utilities
+
+}