From 1b04c6713b75485fa71e0c1c70913e1a411af5bd Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Sun, 11 Sep 2016 18:14:19 -0400 Subject: [PATCH] handle "This". Wrap all geometry --- wrap/Argument.cpp | 14 +- wrap/Argument.h | 4 +- wrap/Class.cpp | 32 +- wrap/Constructor.cpp | 6 +- wrap/Method.cpp | 6 +- wrap/Method.h | 2 +- wrap/Module.cpp | 6 +- wrap/Qualified.h | 12 +- wrap/ReturnType.cpp | 10 +- wrap/ReturnType.h | 2 +- wrap/ReturnValue.cpp | 9 +- wrap/ReturnValue.h | 2 +- wrap/StaticMethod.cpp | 6 +- wrap/StaticMethod.h | 2 +- wrap/TemplateMethod.cpp | 9 +- wrap/TemplateMethod.h | 1 + wrap/tests/cythontest.h | 704 +++++++++++++++++++++++++++++++++++----- 17 files changed, 686 insertions(+), 141 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index f6226f024..3f240f917 100644 --- a/wrap/Argument.cpp +++ b/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 { - string typeName = type.cythonClassName(); +void Argument::emit_cython_pxd(FileWriter& file, const std::string& className) const { + 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 { * 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; diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 0eb28e55c..b6559eac0 100644 --- a/wrap/Class.cpp +++ b/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; icythonClassName() << ")"; + 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() == - this->parentClass->cythonClassName(); + return cls.cythonClass() == + 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& allClasses) const { - pyxFile.oss << "cdef class " << pythonClassName(); - if (parentClass) pyxFile.oss << "(" << parentClass->pythonClassName() << ")"; + pyxFile.oss << "cdef class " << pythonClass(); + 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& 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& 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() - << " ret = " << pythonClassName() << "()\n" + << "\t\tcdef " << pythonClass() + << " ret = " << pythonClass() << "()\n" << "\t\tret." << pyxCythonObj() << " = " << pyxSharedCythonClass() << "(new " << pyxCythonClass() << "(value))\n"; pyxInitParentObj(pyxFile, "\t\tret", "ret." + pyxCythonObj(), allClasses); diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index b221b77c2..169f07f8f 100644 --- a/wrap/Constructor.cpp +++ b/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); diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 41600395a..5de0ca5ef 100644 --- a/wrap/Method.cpp +++ b/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"; diff --git a/wrap/Method.h b/wrap/Method.h index a984a4a5b..299799f9b 100644 --- a/wrap/Method.h +++ b/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: diff --git a/wrap/Module.cpp b/wrap/Module.cpp index fb38ec1ab..7fe0cfd9e 100644 --- a/wrap/Module.cpp +++ b/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 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 { diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index 1288e6853..31bbdaab9 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -5,6 +5,7 @@ */ #include "ReturnType.h" +#include "Class.h" #include "utilities.h" #include @@ -66,12 +67,13 @@ void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const { } /* ************************************************************************* */ -void ReturnType::emit_cython_pxd(FileWriter& file) const { - string typeName = cythonClassName(); +void ReturnType::emit_cython_pxd(FileWriter& file, const std::string& className) const { + 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 << ")"; diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index 4d62f65c0..bc0eebf95 100644 --- a/wrap/ReturnType.h +++ b/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; diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 5217e14c5..4575dac97 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -68,16 +68,15 @@ void ReturnValue::emit_matlab(FileWriter& proxyFile) const { } /* ************************************************************************* */ -void ReturnValue::emit_cython_pxd(FileWriter& file) const { - string output; +void ReturnValue::emit_cython_pxd(FileWriter& file, const std::string& className) const { 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 << " "; } } diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index c804e6e21..be8d3897e 100644 --- a/wrap/ReturnValue.h +++ b/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; diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 87b036cd3..1defd4458 100644 --- a/wrap/StaticMethod.cpp +++ b/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"; } } diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 3b24afb08..902e809b1 100644 --- a/wrap/StaticMethod.h +++ b/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: diff --git a/wrap/TemplateMethod.cpp b/wrap/TemplateMethod.cpp index 84c2f3ecc..e70e9713d 100644 --- a/wrap/TemplateMethod.cpp +++ b/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"; } } diff --git a/wrap/TemplateMethod.h b/wrap/TemplateMethod.h index b1827f2d5..070083231 100644 --- a/wrap/TemplateMethod.h +++ b/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); diff --git a/wrap/tests/cythontest.h b/wrap/tests/cythontest.h index 747960292..ab32d63fe 100644 --- a/wrap/tests/cythontest.h +++ b/wrap/tests/cythontest.h @@ -15,6 +15,113 @@ typedef gtsam::FastSet KeySet; #include template 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 +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 +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 +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 class Point2 { // Standard Constructors @@ -40,6 +147,91 @@ class Point2 { void serialize() const; }; +// std::vector +#include +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 +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 +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 class Rot2 { // Standard Constructors and Named Constructors @@ -84,6 +276,61 @@ class Rot2 { void serialize() const; }; +#include +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 Ypr(double y, double p, double r); + static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 Rodrigues(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; + + // Group + static gtsam::Rot3 identity(); + gtsam::Rot3 inverse() const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; + + // Manifold + //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; + + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; + + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly + Vector quaternion() const; + + // enabling serialization functionality + void serialize() const; +}; + #include class Pose2 { // Standard Constructor @@ -93,7 +340,6 @@ class Pose2 { 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; @@ -134,87 +380,6 @@ class Pose2 { void serialize() const; }; -#include -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 -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); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 Ypr(double y, double p, double r); - static gtsam::Rot3 Quaternion(double w, double x, double y, double z); - static gtsam::Rot3 Rodrigues(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Rot3& rot, double tol) const; - - // Group - static gtsam::Rot3 identity(); - gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; - - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; - - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; - - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; -// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly - Vector quaternion() const; - - // enabling serialization functionality - void serialize() const; -}; - #include class Pose3 { // Standard Constructors @@ -264,6 +429,381 @@ class Pose3 { void serialize() const; }; +// std::vector +#include +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +template +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 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 +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 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 PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + +#include +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 + +// 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 class VectorValues { //Constructors