handle "This". Wrap all geometry

wrap/Argument.cpp

@@ -99,8 +99,10 @@ void Argument::proxy_check(FileWriter& proxyFile, const string& s) const {
 }
 
 /* ************************************************************************* */
-void Argument::emit_cython_pxd(FileWriter& file) const {
+void Argument::emit_cython_pxd(FileWriter& file, const std::string& className) const {
-  string typeName = type.cythonClassName();
+  string typeName = type.cythonClass();
+  if (typeName == "This") typeName = className;
+
   string cythonType = typeName;
   if (type.isEigen()) {
     cythonType = "const " + typeName + "Xd&";
@@ -115,7 +117,7 @@ void Argument::emit_cython_pxd(FileWriter& file) const {
 
 /* ************************************************************************* */
 void Argument::emit_cython_pyx(FileWriter& file) const {
-  string typeName = type.pythonClassName();
+  string typeName = type.pythonClass();
   string cythonType = typeName;
   // use numpy for Vector and Matrix
   if (type.isEigen()) cythonType = "np.ndarray";
@@ -124,7 +126,7 @@ void Argument::emit_cython_pyx(FileWriter& file) const {
 
 /* ************************************************************************* */
 void Argument::emit_cython_pyx_asParam(FileWriter& file) const {
-  string cythonType = type.cythonClassName();
+  string cythonType = type.cythonClass();
   string cythonVar;
   if (type.isNonBasicType()) {
     cythonVar = name + "." + type.pyxCythonObj();
@@ -217,9 +219,9 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const {
 }
 
 /* ************************************************************************* */
-void ArgumentList::emit_cython_pxd(FileWriter& file) const {
+void ArgumentList::emit_cython_pxd(FileWriter& file, const std::string& className) const {
   for (size_t j = 0; j<size(); ++j) {
-    at(j).emit_cython_pxd(file);
+    at(j).emit_cython_pxd(file, className);
     if (j<size()-1) file.oss << ", ";
   }
 }

wrap/Argument.h

@@ -69,7 +69,7 @@ struct Argument {
    * emit arguments for cython pxd
    * @param file output stream
    */
-  void emit_cython_pxd(FileWriter& file) const;
+  void emit_cython_pxd(FileWriter& file, const std::string& className) const;
   void emit_cython_pyx(FileWriter& file) const;
   void emit_cython_pyx_asParam(FileWriter& file) const;
 
@@ -118,7 +118,7 @@ struct ArgumentList: public std::vector<Argument> {
    * emit arguments for cython pxd
    * @param file output stream
    */
-  void emit_cython_pxd(FileWriter& file) const;
+  void emit_cython_pxd(FileWriter& file, const std::string& className) const;
   void emit_cython_pyx(FileWriter& file) const;
   void emit_cython_pyx_asParams(FileWriter& file) const;
 

wrap/Class.cpp

@@ -677,7 +677,7 @@ void Class::python_wrapper(FileWriter& wrapperFile) const {
 void Class::emit_cython_pxd(FileWriter& pxdFile) const {
   pxdFile.oss << "cdef extern from \"" << includeFile << "\" namespace \""
                 << qualifiedNamespaces("::") << "\":" << endl;
-  pxdFile.oss << "\tcdef cppclass " << cythonClassName() << " \"" << qualifiedName("::") << "\"";
+  pxdFile.oss << "\tcdef cppclass " << cythonClass() << " \"" << qualifiedName("::") << "\"";
   if (templateArgs.size()>0) {
     pxdFile.oss << "[";
     for(size_t i = 0; i<templateArgs.size(); ++i) {
@@ -686,20 +686,20 @@ void Class::emit_cython_pxd(FileWriter& pxdFile) const {
     }
     pxdFile.oss << "]";
   }
-  if (parentClass) pxdFile.oss << "(" <<  parentClass->cythonClassName() << ")";
+  if (parentClass) pxdFile.oss << "(" <<  parentClass->cythonClass() << ")";
   pxdFile.oss << ":\n";
 
-  constructor.emit_cython_pxd(pxdFile, cythonClassName());
+  constructor.emit_cython_pxd(pxdFile, cythonClass());
   if (constructor.nrOverloads()>0) pxdFile.oss << "\n";
 
   for(const StaticMethod& m: static_methods | boost::adaptors::map_values)
-    m.emit_cython_pxd(pxdFile);
+    m.emit_cython_pxd(pxdFile, *this);
   if (static_methods.size()>0) pxdFile.oss << "\n";
 
   for(const Method& m: nontemplateMethods_ | boost::adaptors::map_values)
-    m.emit_cython_pxd(pxdFile);
+    m.emit_cython_pxd(pxdFile, *this);
   for(const TemplateMethod& m: templateMethods_ | boost::adaptors::map_values)
-    m.emit_cython_pxd(pxdFile);
+    m.emit_cython_pxd(pxdFile, *this);
   size_t numMethods = constructor.nrOverloads() + static_methods.size() +
                       methods_.size() + templateMethods_.size();
   if (numMethods == 0)
@@ -718,11 +718,11 @@ void Class::pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj, cons
       // TODO: This search is not efficient. :-(
       auto parent_it = find_if(allClasses.begin(), allClasses.end(),
                                [this](const Class& cls) {
-                                   return cls.cythonClassName() ==
+                                   return cls.cythonClass() ==
-                                          this->parentClass->cythonClassName();
+                                          this->parentClass->cythonClass();
                                });
       if (parent_it == allClasses.end()) {
-          cerr << "Can't find parent class: " << parentClass->cythonClassName();
+          cerr << "Can't find parent class: " << parentClass->cythonClass();
         throw std::runtime_error("Parent class not found!");
       }
       parent_it->pyxInitParentObj(pyxFile, pyObj, cySharedObj, allClasses);
@@ -731,8 +731,8 @@ void Class::pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj, cons
 
 /* ************************************************************************* */
 void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector<Class>& allClasses) const {
-  pyxFile.oss << "cdef class " << pythonClassName();
+  pyxFile.oss << "cdef class " << pythonClass();
-  if (parentClass) pyxFile.oss << "(" <<  parentClass->pythonClassName() << ")";
+  if (parentClass) pyxFile.oss << "(" <<  parentClass->pythonClass() << ")";
   pyxFile.oss << ":\n";
 
   // shared variable of the corresponding cython object 
@@ -749,9 +749,9 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector<Class>& allCl
 
   // cyCreateFromShared
   pyxFile.oss << "\t@staticmethod\n";
-  pyxFile.oss << "\tcdef " << pythonClassName() << " cyCreateFromShared(const " 
+  pyxFile.oss << "\tcdef " << pythonClass() << " cyCreateFromShared(const " 
               << pyxSharedCythonClass() << "& other):\n"
-              << "\t\tcdef " << pythonClassName() << " ret = " << pythonClassName() << "()\n"
+              << "\t\tcdef " << pythonClass() << " ret = " << pythonClass() << "()\n"
               << "\t\tret." << pyxCythonObj() << " = other\n";
   pyxInitParentObj(pyxFile, "\t\tret", "other", allClasses);
   pyxFile.oss << "\t\treturn ret" << "\n";
@@ -763,10 +763,10 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector<Class>& allCl
   if (constructor.nrOverloads() >= 1) { 
     // cyCreateFromValue
     pyxFile.oss << "\t@staticmethod\n";
-    pyxFile.oss << "\tcdef " << pythonClassName() << " cyCreateFromValue(const "
+    pyxFile.oss << "\tcdef " << pythonClass() << " cyCreateFromValue(const "
                 << pyxCythonClass() << "& value):\n"
-                << "\t\tcdef " << pythonClassName()
+                << "\t\tcdef " << pythonClass()
-                << " ret = " << pythonClassName() << "()\n"
+                << " ret = " << pythonClass() << "()\n"
                 << "\t\tret." << pyxCythonObj() << " = " << pyxSharedCythonClass()
                 << "(new " << pyxCythonClass() << "(value))\n";
     pyxInitParentObj(pyxFile, "\t\tret", "ret." + pyxCythonObj(), allClasses);

wrap/Constructor.cpp

@@ -141,11 +141,11 @@ void Constructor::emit_cython_pxd(FileWriter& pxdFile, Str className) const {
     ArgumentList args = argumentList(i);
     // ignore copy constructor, it's generated by default above
     if (args.size() == 1 && args[0].is_const && args[0].is_ref &&
-        !args[0].is_ptr && args[0].type.cythonClassName() == className)
+        !args[0].is_ptr && args[0].type.cythonClass() == className)
         continue;
     // generate the constructor
     pxdFile.oss << "\t\t" << className << "(";
-    args.emit_cython_pxd(pxdFile);
+    args.emit_cython_pxd(pxdFile, className);
     pxdFile.oss << ") " << "except +\n";
   }
 }
@@ -166,7 +166,7 @@ void Constructor::emit_cython_pyx(FileWriter& pyxFile, const Class& cls) const {
     // For example: noiseModel::Robust doesn't have the copy assignment operator
     // because its members are shared_ptr to abstract base classes. That fails
     // Cython to generate the object as it assigns the new obj to a temp variable.
-    pyxFile.oss << "\t\treturn " << cls.cythonClassName() 
+    pyxFile.oss << "\t\treturn " << cls.cythonClass() 
                 << ".cyCreateFromShared(" << cls.pyxSharedCythonClass() 
                 << "(new " << cls.pyxCythonClass() << "(";
     args.emit_cython_pyx_asParams(pyxFile);

wrap/Method.cpp

@@ -77,12 +77,12 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
 }
 
 /* ************************************************************************* */
-void Method::emit_cython_pxd(FileWriter& file) const {
+void Method::emit_cython_pxd(FileWriter& file, const Class& cls) const {
   for(size_t i = 0; i < nrOverloads(); ++i) {
     file.oss << "\t\t";
-    returnVals_[i].emit_cython_pxd(file);
+    returnVals_[i].emit_cython_pxd(file, cls.cythonClass());
     file.oss << ((name_ == "print") ? "_print \"print\"" : name_) << "(";
-    argumentList(i).emit_cython_pxd(file);
+    argumentList(i).emit_cython_pxd(file, cls.cythonClass());
     file.oss << ")";
     if (is_const_) file.oss << " const";
     file.oss << "\n";

wrap/Method.h

@@ -51,7 +51,7 @@ public:
     return os;
   }
 
-  void emit_cython_pxd(FileWriter& file) const;
+  void emit_cython_pxd(FileWriter& file, const Class& cls) const;
   void emit_cython_pyx(FileWriter& file, const Class& cls) const;
 
 private:

wrap/Module.cpp

@@ -319,11 +319,11 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
   //... ctypedef for template instantiations
   for(const Class& cls: expandedClasses) {
     if (cls.templateClass) {
-      pxdFile.oss << "ctypedef " << cls.templateClass->cythonClassName() << "[";
+      pxdFile.oss << "ctypedef " << cls.templateClass->cythonClass() << "[";
       for (size_t i = 0; i<cls.templateInstTypeList.size(); ++i)
-        pxdFile.oss << cls.templateInstTypeList[i].cythonClassName() 
+        pxdFile.oss << cls.templateInstTypeList[i].cythonClass() 
                     << ((i==cls.templateInstTypeList.size()-1)?"":", ");
-      pxdFile.oss << "] " << cls.cythonClassName() << "\n";
+      pxdFile.oss << "] " << cls.cythonClass() << "\n";
     }
   }
   pxdFile.oss << "\n";

wrap/Qualified.h

@@ -174,22 +174,22 @@ public:
   }
 
   /// the Cython class in pxd
-  std::string cythonClassName() const {
+  std::string cythonClass() const {
     return qualifiedName("_", 1);
   }
 
   /// the Python class in pyx
-  std::string pythonClassName() const {
+  std::string pythonClass() const {
-    return cythonClassName();
+    return cythonClass();
   }
 
   /// return the Cython class in pxd corresponding to a Python class in pyx
   std::string pyxCythonClass() const {
       if (isNonBasicType()) {
         if (namespaces_.size() > 0)
-          return namespaces_[0] + "." + cythonClassName();
+          return namespaces_[0] + "." + cythonClass();
         else {
-          std::cerr << "Class without namespace: " << cythonClassName() << std::endl;
+          std::cerr << "Class without namespace: " << cythonClass() << std::endl;
           throw std::runtime_error("Error: User type without namespace!!");
         }
       }
@@ -201,7 +201,7 @@ public:
 
   /// the internal Cython shared obj in a Python class wrappper
   std::string pyxCythonObj() const {
-    return "gt" + cythonClassName() + "_";
+    return "gt" + cythonClass() + "_";
   }
 
   std::string pyxSharedCythonClass() const {

wrap/ReturnType.cpp

@@ -5,6 +5,7 @@
  */
 
 #include "ReturnType.h"
+#include "Class.h"
 #include "utilities.h"
 #include <iostream>
 
@@ -66,12 +67,13 @@ void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const {
 }
 
 /* ************************************************************************* */
-void ReturnType::emit_cython_pxd(FileWriter& file) const {
+void ReturnType::emit_cython_pxd(FileWriter& file, const std::string& className) const {
-  string typeName = cythonClassName();
+  string typeName = cythonClass();
   string cythonType;
   if (isEigen()) cythonType = typeName + "Xd";
   else if (isPtr) cythonType = "shared_ptr[" + typeName + "]";
   else cythonType = typeName;
+  if (typeName == "This") cythonType = className;
   file.oss << cythonType;
 }
 
@@ -88,11 +90,11 @@ void ReturnType::emit_cython_pyx_casting(FileWriter& file, const std::string& va
     file.oss << "ndarray_copy";
   else if (isNonBasicType()){
     if (isPtr) 
-      file.oss << pythonClassName() << ".cyCreateFromShared";
+      file.oss << pythonClass() << ".cyCreateFromShared";
     else {
       // if the function return an object, it must be copy constructible and copy assignable
       // so it's safe to use cyCreateFromValue
-      file.oss << pythonClassName() << ".cyCreateFromValue";
+      file.oss << pythonClass() << ".cyCreateFromValue";
     }
   }
   file.oss << "(" << var << ")";

wrap/ReturnType.h

@@ -47,7 +47,7 @@ struct ReturnType: public Qualified {
       throw DependencyMissing(key, "checking return type of " + s);
   }
 
-  void emit_cython_pxd(FileWriter& file) const;
+  void emit_cython_pxd(FileWriter& file, const std::string& className) const;
   void emit_cython_pyx_return_type(FileWriter& file) const;
   void emit_cython_pyx_casting(FileWriter& file, const std::string& var) const;
 

wrap/ReturnValue.cpp

@@ -68,16 +68,15 @@ void ReturnValue::emit_matlab(FileWriter& proxyFile) const {
 }
 
 /* ************************************************************************* */
-void ReturnValue::emit_cython_pxd(FileWriter& file) const {
+void ReturnValue::emit_cython_pxd(FileWriter& file, const std::string& className) const {
-  string output;
   if (isPair) {
     file.oss << "pair[";
-    type1.emit_cython_pxd(file);
+    type1.emit_cython_pxd(file, className);
     file.oss << ",";
-    type2.emit_cython_pxd(file);
+    type2.emit_cython_pxd(file, className);
     file.oss << "] ";
   } else {
-      type1.emit_cython_pxd(file);
+      type1.emit_cython_pxd(file, className);
       file.oss << " ";
   }
 }

wrap/ReturnValue.h

@@ -71,7 +71,7 @@ struct ReturnValue {
 
   void emit_matlab(FileWriter& proxyFile) const;
 
-  void emit_cython_pxd(FileWriter& file) const;
+  void emit_cython_pxd(FileWriter& file, const std::string& className) const;
   void emit_cython_pyx_return_type(FileWriter& file) const;
   void emit_cython_pyx_casting(FileWriter& file, const std::string& var) const;
 

wrap/StaticMethod.cpp

@@ -57,16 +57,16 @@ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
 }
 
 /* ************************************************************************* */
-void StaticMethod::emit_cython_pxd(FileWriter& file) const {
+void StaticMethod::emit_cython_pxd(FileWriter& file, const Class& cls) const {
   // don't support overloads for static method :-(
   for(size_t i = 0; i < nrOverloads(); ++i) {
     file.oss << "\t\t@staticmethod\n";
     file.oss << "\t\t";
-    returnVals_[i].emit_cython_pxd(file);
+    returnVals_[i].emit_cython_pxd(file, cls.cythonClass());
     file.oss << name_ << ((i > 0) ? "_" + to_string(i) : "") << " \"" << name_
              << "\""
              << "(";
-    argumentList(i).emit_cython_pxd(file);
+    argumentList(i).emit_cython_pxd(file, cls.cythonClass());
     file.oss << ")\n";
   }
 }

wrap/StaticMethod.h

@@ -34,7 +34,7 @@ struct StaticMethod: public MethodBase {
     return os;
   }
 
-  void emit_cython_pxd(FileWriter& file) const;
+  void emit_cython_pxd(FileWriter& file, const Class& cls) const;
   void emit_cython_pyx(FileWriter& file, const Class& cls) const;
 
 protected:

wrap/TemplateMethod.cpp

@@ -15,17 +15,18 @@
  **/
 
 #include "TemplateMethod.h"
+#include "Class.h"
  
 using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void TemplateMethod::emit_cython_pxd(FileWriter& file) const {
+void TemplateMethod::emit_cython_pxd(FileWriter& file, const Class& cls) const {
   for(size_t i = 0; i < nrOverloads(); ++i) {
     file.oss << "\t\t";
-    returnVals_[i].emit_cython_pxd(file);
+    returnVals_[i].emit_cython_pxd(file, cls.cythonClass());
     file.oss << name_ << "[" << argName << "]" << "(";
-    argumentList(i).emit_cython_pxd(file);
+    argumentList(i).emit_cython_pxd(file, cls.cythonClass());
     file.oss << ")\n";
   }
 }

wrap/TemplateMethod.h

@@ -25,6 +25,7 @@ namespace wrap {
 struct TemplateMethod: public Method {
   std::string argName; // name of template argument
 
+  void emit_cython_pxd(FileWriter& file, const Class& cls) const;
   bool addOverload(Str name, const ArgumentList& args,
                    const ReturnValue& retVal, bool is_const,
                    std::string argName, bool verbose = false);

wrap/tests/cythontest.h

@@ -15,6 +15,113 @@ typedef gtsam::FastSet<size_t> KeySet;
 #include <gtsam/base/FastMap.h>
 template<K,V> class FastMap{};
 
+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/deprecated/LieScalar.h>
+class LieScalar {
+  // Standard constructors
+  LieScalar();
+  LieScalar(double d);
+
+  // Standard interface
+  double value() const;
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::LieScalar& expected, double tol) const;
+
+  // Group
+  static gtsam::LieScalar identity();
+  gtsam::LieScalar inverse() const;
+  gtsam::LieScalar compose(const gtsam::LieScalar& p) const;
+  gtsam::LieScalar between(const gtsam::LieScalar& l2) const;
+
+  // Manifold
+  size_t dim() const;
+  gtsam::LieScalar retract(Vector v) const;
+  Vector localCoordinates(const gtsam::LieScalar& t2) const;
+
+  // Lie group
+  static gtsam::LieScalar Expmap(Vector v);
+  static Vector Logmap(const gtsam::LieScalar& p);
+};
+
+#include <gtsam/base/deprecated/LieVector.h>
+class LieVector {
+  // Standard constructors
+  LieVector();
+  LieVector(Vector v);
+
+  // Standard interface
+  Vector vector() const;
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::LieVector& expected, double tol) const;
+
+  // Group
+  static gtsam::LieVector identity();
+  gtsam::LieVector inverse() const;
+  gtsam::LieVector compose(const gtsam::LieVector& p) const;
+  gtsam::LieVector between(const gtsam::LieVector& l2) const;
+
+  // Manifold
+  size_t dim() const;
+  gtsam::LieVector retract(Vector v) const;
+  Vector localCoordinates(const gtsam::LieVector& t2) const;
+
+  // Lie group
+  static gtsam::LieVector Expmap(Vector v);
+  static Vector Logmap(const gtsam::LieVector& p);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/base/deprecated/LieMatrix.h>
+class LieMatrix {
+  // Standard constructors
+  LieMatrix();
+  LieMatrix(Matrix v);
+
+  // Standard interface
+  Matrix matrix() const;
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::LieMatrix& expected, double tol) const;
+
+  // Group
+  static gtsam::LieMatrix identity();
+  gtsam::LieMatrix inverse() const;
+  gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const;
+  gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const;
+
+  // Manifold
+  size_t dim() const;
+  gtsam::LieMatrix retract(Vector v) const;
+  Vector localCoordinates(const gtsam::LieMatrix & t2) const;
+
+  // Lie group
+  static gtsam::LieMatrix Expmap(Vector v);
+  static Vector Logmap(const gtsam::LieMatrix& p);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+//*************************************************************************
+// geometry
+//*************************************************************************
+
 #include <gtsam/geometry/Point2.h>
 class Point2 {
   // Standard Constructors
@@ -40,6 +147,91 @@ class Point2 {
   void serialize() const;
 };
 
+// std::vector<gtsam::Point2>
+#include <gtsam/geometry/Point2.h>
+class Point2Vector
+{
+  // Constructors
+  Point2Vector();
+  Point2Vector(const gtsam::Point2Vector& v);
+
+  //Capacity
+  size_t size() const;
+  size_t max_size() const;
+  void resize(size_t sz);
+  size_t capacity() const;
+  bool empty() const;
+  void reserve(size_t n);
+
+  //Element access
+  gtsam::Point2 at(size_t n) const;
+  gtsam::Point2 front() const;
+  gtsam::Point2 back() const;
+
+  //Modifiers
+  void assign(size_t n, const gtsam::Point2& u);
+  void push_back(const gtsam::Point2& x);
+  void pop_back();
+};
+
+#include <gtsam/geometry/StereoPoint2.h>
+class StereoPoint2 {
+  // Standard Constructors
+  StereoPoint2();
+  StereoPoint2(double uL, double uR, double v);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::StereoPoint2& point, double tol) const;
+
+  // Group
+  static gtsam::StereoPoint2 identity();
+  gtsam::StereoPoint2 inverse() const;
+  gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
+  gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
+
+  // Manifold
+  gtsam::StereoPoint2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::StereoPoint2& p) const;
+
+  // Lie Group
+  static gtsam::StereoPoint2 Expmap(Vector v);
+  static Vector Logmap(const gtsam::StereoPoint2& p);
+
+  // Standard Interface
+  Vector vector() const;
+  double uL() const;
+  double uR() const;
+  double v() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Point3.h>
+class Point3 {
+  // Standard Constructors
+  Point3();
+  Point3(double x, double y, double z);
+  Point3(Vector v);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Point3& p, double tol) const;
+
+  // Group
+  static gtsam::Point3 identity();
+
+  // Standard Interface
+  Vector vector() const;
+  double x() const;
+  double y() const;
+  double z() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 #include <gtsam/geometry/Rot2.h>
 class Rot2 {
   // Standard Constructors and Named Constructors
@@ -84,86 +276,10 @@ class Rot2 {
   void serialize() const;
 };
 
-#include <gtsam/geometry/Pose2.h>
-class Pose2 {
-  // Standard Constructor
-  Pose2();
-  Pose2(const gtsam::Pose2& pose);
-  Pose2(double x, double y, double theta);
-  Pose2(double theta, const gtsam::Point2& t);
-  Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
-  Pose2(Vector v);
-  Pose2(Matrix T);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::Pose2& pose, double tol) const;
-
-  // Group
-  static gtsam::Pose2 identity();
-  gtsam::Pose2 inverse() const;
-  gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
-  gtsam::Pose2 between(const gtsam::Pose2& p2) const;
-
-  // Manifold
-  gtsam::Pose2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Pose2& p) const;
-
-  // Lie Group
-  static gtsam::Pose2 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Pose2& p);
-  Matrix AdjointMap() const;
-  Vector Adjoint(const Vector& xi) const;
-  static Matrix wedge(double vx, double vy, double w);
-
-  // Group Actions on Point2
-  gtsam::Point2 transform_from(const gtsam::Point2& p) const;
-  gtsam::Point2 transform_to(const gtsam::Point2& p) const;
-
-  // Standard Interface
-  double x() const;
-  double y() const;
-  double theta() const;
-  gtsam::Rot2 bearing(const gtsam::Point2& point) const;
-  double range(const gtsam::Point2& point) const;
-  gtsam::Point2 translation() const;
-  gtsam::Rot2 rotation() const;
-  Matrix matrix() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/geometry/Point3.h>
-class Point3 {
-  // Standard Constructors
-  Point3();
-  Point3(const Point3& p);
-  Point3(double x, double y, double z);
-  Point3(Vector v);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::Point3& p, double tol) const;
-
-  // Group
-  static gtsam::Point3 identity();
-
-  // Standard Interface
-  Vector vector() const;
-  double x() const;
-  double y() const;
-  double z() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
 #include <gtsam/geometry/Rot3.h>
 class Rot3 {
   // Standard Constructors and Named Constructors
   Rot3();
-  Rot3(const Rot3& R);
   Rot3(Matrix R);
   static gtsam::Rot3 Rx(double t);
   static gtsam::Rot3 Ry(double t);
@@ -215,6 +331,55 @@ class Rot3 {
   void serialize() const;
 };
 
+#include <gtsam/geometry/Pose2.h>
+class Pose2 {
+  // Standard Constructor
+  Pose2();
+  Pose2(const gtsam::Pose2& pose);
+  Pose2(double x, double y, double theta);
+  Pose2(double theta, const gtsam::Point2& t);
+  Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
+  Pose2(Vector v);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Pose2& pose, double tol) const;
+
+  // Group
+  static gtsam::Pose2 identity();
+  gtsam::Pose2 inverse() const;
+  gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
+  gtsam::Pose2 between(const gtsam::Pose2& p2) const;
+
+  // Manifold
+  gtsam::Pose2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Pose2& p) const;
+
+  // Lie Group
+  static gtsam::Pose2 Expmap(Vector v);
+  static Vector Logmap(const gtsam::Pose2& p);
+  Matrix AdjointMap() const;
+  Vector Adjoint(const Vector& xi) const;
+  static Matrix wedge(double vx, double vy, double w);
+
+  // Group Actions on Point2
+  gtsam::Point2 transform_from(const gtsam::Point2& p) const;
+  gtsam::Point2 transform_to(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double x() const;
+  double y() const;
+  double theta() const;
+  gtsam::Rot2 bearing(const gtsam::Point2& point) const;
+  double range(const gtsam::Point2& point) const;
+  gtsam::Point2 translation() const;
+  gtsam::Rot2 rotation() const;
+  Matrix matrix() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 #include <gtsam/geometry/Pose3.h>
 class Pose3 {
   // Standard Constructors
@@ -264,6 +429,381 @@ class Pose3 {
   void serialize() const;
 };
 
+// std::vector<gtsam::Pose3>
+#include <gtsam/geometry/Pose3.h>
+class Pose3Vector
+{
+  Pose3Vector();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Pose3 at(size_t n) const;
+  void push_back(const gtsam::Pose3& x);
+};
+
+#include <gtsam/geometry/Unit3.h>
+class Unit3 {
+  // Standard Constructors
+  Unit3();
+  Unit3(const gtsam::Point3& pose);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Unit3& pose, double tol) const;
+
+  // Other functionality
+  Matrix basis() const;
+  Matrix skew() const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Unit3 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Unit3& s) const;
+};
+
+#include <gtsam/geometry/EssentialMatrix.h>
+class EssentialMatrix {
+  EssentialMatrix();
+  // Standard Constructors
+  EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::EssentialMatrix retract(Vector v) const;
+  Vector localCoordinates(const gtsam::EssentialMatrix& s) const;
+
+  // Other methods:
+  gtsam::Rot3 rotation() const;
+  gtsam::Unit3 direction() const;
+  Matrix matrix() const;
+  double error(Vector vA, Vector vB);
+};
+
+#include <gtsam/geometry/Cal3_S2.h>
+class Cal3_S2 {
+  // Standard Constructors
+  Cal3_S2();
+  Cal3_S2(double fx, double fy, double s, double u0, double v0);
+  Cal3_S2(Vector v);
+  Cal3_S2(double fov, int w, int h);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3_S2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3_S2& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  Vector vector() const;
+  Matrix matrix() const;
+  Matrix matrix_inverse() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Cal3DS2_Base.h>
+virtual class Cal3DS2_Base {
+  // Standard Constructors
+  Cal3DS2_Base();
+
+  // Testable
+  void print(string s) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  double k1() const;
+  double k2() const;
+
+  // Action on Point2
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  // gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Cal3DS2.h>
+virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
+  // Standard Constructors
+  Cal3DS2();
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
+  Cal3DS2(Vector v);
+
+  // Testable
+  bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
+
+  // Manifold
+  size_t dim() const;
+  static size_t Dim();
+  gtsam::Cal3DS2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3DS2& c) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Cal3Unified.h>
+virtual class Cal3Unified : gtsam::Cal3DS2_Base {
+  // Standard Constructors
+  Cal3Unified();
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
+  Cal3Unified(Vector v);
+
+  // Testable
+  bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
+
+  // Standard Interface
+  double xi() const;
+  gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
+  gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
+
+  // Manifold
+  size_t dim() const;
+  static size_t Dim();
+  gtsam::Cal3Unified retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Unified& c) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+class Cal3_S2Stereo {
+  // Standard Constructors
+  Cal3_S2Stereo();
+  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
+  Cal3_S2Stereo(Vector v);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  double baseline() const;
+};
+
+#include <gtsam/geometry/Cal3Bundler.h>
+class Cal3Bundler {
+  // Standard Constructors
+  Cal3Bundler();
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3Bundler retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double k1() const;
+  double k2() const;
+  double u0() const;
+  double v0() const;
+  Vector vector() const;
+  Vector k() const;
+  //Matrix K() const; //FIXME: Uppercase
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/CalibratedCamera.h>
+class CalibratedCamera {
+  // Standard Constructors and Named Constructors
+  CalibratedCamera();
+  CalibratedCamera(const gtsam::Pose3& pose);
+  CalibratedCamera(const Vector& v);
+  static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::CalibratedCamera retract(const Vector& d) const;
+  Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
+
+  // Action on Point3
+  gtsam::Point2 project(const gtsam::Point3& point) const;
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/PinholeCamera.h>
+template<CALIBRATION>
+class PinholeCamera {
+  // Standard Constructors and Named Constructors
+  PinholeCamera();
+  PinholeCamera(const gtsam::Pose3& pose);
+  PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
+  static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height);
+  static This Level(const gtsam::Pose2& pose, double height);
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+      const gtsam::Point3& upVector, const CALIBRATION& K);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const This& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  CALIBRATION calibration() const;
+
+  // Manifold
+  This retract(const Vector& d) const;
+  Vector localCoordinates(const This& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Pose3& point);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/SimpleCamera.h>
+virtual class SimpleCamera {
+  // Standard Constructors and Named Constructors
+  SimpleCamera();
+  SimpleCamera(const gtsam::Pose3& pose);
+  SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
+  static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
+  static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
+  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+      const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::SimpleCamera& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  gtsam::Cal3_S2 calibration() const;
+
+  // Manifold
+  gtsam::SimpleCamera retract(const Vector& d) const;
+  Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Pose3& point);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+};
+
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+
+#include <gtsam/geometry/StereoCamera.h>
+class StereoCamera {
+  // Standard Constructors and Named Constructors
+  StereoCamera();
+  StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::StereoCamera& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  double baseline() const;
+  gtsam::Cal3_S2Stereo calibration() const;
+
+  // Manifold
+  gtsam::StereoCamera retract(const Vector& d) const;
+  Vector localCoordinates(const gtsam::StereoCamera& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  gtsam::StereoPoint2 project(const gtsam::Point3& point);
+  gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/triangulation.h>
+
+// Templates appear not yet supported for free functions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+    gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
+    double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+    gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
+    double rank_tol, bool optimize);
+
+
 #include <gtsam/linear/VectorValues.h>
 class VectorValues {
     //Constructors