diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 1f87b5119a..addde8c64c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -19,22 +19,20 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - # ubuntu-18.04-gcc-5, #TODO Enable once the Python wrapper is optimized for memory + ubuntu-18.04-gcc-5, ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, macOS-10.15-xcode-11.3.1, - # ubuntu-18.04-gcc-5-tbb, + ubuntu-18.04-gcc-5-tbb, ] - #TODO update wrapper to prevent OOM - # build_type: [Debug, Release] - build_type: [Release] + build_type: [Debug, Release] python_version: [3] include: - # - name: ubuntu-18.04-gcc-5 - # os: ubuntu-18.04 - # compiler: gcc - # version: "5" + - name: ubuntu-18.04-gcc-5 + os: ubuntu-18.04 + compiler: gcc + version: "5" - name: ubuntu-18.04-gcc-9 os: ubuntu-18.04 @@ -46,7 +44,7 @@ jobs: compiler: clang version: "9" - #NOTE temporarily added this as it is a required check. + # NOTE temporarily added this as it is a required check. - name: ubuntu-18.04-clang-9 os: ubuntu-18.04 compiler: clang @@ -59,11 +57,11 @@ jobs: compiler: xcode version: "11.3.1" - # - name: ubuntu-18.04-gcc-5-tbb - # os: ubuntu-18.04 - # compiler: gcc - # version: "5" - # flag: tbb + - name: ubuntu-18.04-gcc-5-tbb + os: ubuntu-18.04 + compiler: gcc + version: "5" + flag: tbb steps: - name: Checkout diff --git a/gtsam/base/base.i b/gtsam/base/base.i new file mode 100644 index 0000000000..c24b04088a --- /dev/null +++ b/gtsam/base/base.i @@ -0,0 +1,115 @@ +//************************************************************************* +// base +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ##### + +#include +bool isDebugVersion(); + +#include +class IndexPair { + IndexPair(); + IndexPair(size_t i, size_t j); + size_t i() const; + size_t j() const; +}; + +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map sets(); +// }; + +class IndexPairSet { + IndexPairSet(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(gtsam::IndexPair key); + bool erase(gtsam::IndexPair key); // returns true if value was removed + bool count(gtsam::IndexPair key) const; // returns true if value exists +}; + +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; + +gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); + +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; + +class DSFMapIndexPair { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair& key) const; + void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); + gtsam::IndexPairSetMap sets(); +}; + +#include +bool linear_independent(Matrix A, Matrix B, double tol); + +#include +virtual class Value { + // No constructors because this is an abstract class + + // Testable + void print(string s = "") const; + + // Manifold + size_t dim() const; +}; + +#include +template +virtual class GenericValue : gtsam::Value { + void serializable() const; +}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h new file mode 100644 index 0000000000..8eb5617a8e --- /dev/null +++ b/gtsam/base/utilities.h @@ -0,0 +1,29 @@ +#pragma once + +namespace gtsam { +/** + * For Python __str__(). + * Redirect std cout to a string stream so we can return a string representation + * of an object when it prints to cout. + * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string + */ +struct RedirectCout { + /// constructor -- redirect stdout buffer to a stringstream buffer + RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} + + /// return the string + std::string str() const { + return ssBuffer_.str(); + } + + /// destructor -- redirect stdout buffer to its original buffer + ~RedirectCout() { + std::cout.rdbuf(coutBuffer_); + } + +private: + std::stringstream ssBuffer_; + std::streambuf* coutBuffer_; +}; + +} diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 119e9d1a3c..5ff6b9816f 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i new file mode 100644 index 0000000000..4e56347d37 --- /dev/null +++ b/gtsam/geometry/geometry.i @@ -0,0 +1,1001 @@ +//************************************************************************* +// geometry +//************************************************************************* + +namespace gtsam { + +#include +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Point2& point, double tol) const; + + // Group + static gtsam::Point2 identity(); + + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double distance(const gtsam::Point2& p2) const; + double norm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +// std::vector +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; + + // Operator Overloads + gtsam::StereoPoint2 operator-() const; + // gtsam::StereoPoint2 operator+(Vector b) const; //TODO Mixed types not yet + // supported + gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 operator-(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; + + // enable pickling in python + void pickle() 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; + + // enable pickling in python + void pickle() const; +}; + +class Point3Pairs { + Point3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point3Pair at(size_t n) const; + void push_back(const gtsam::Point3Pair& point_pair); +}; + +#include +class Rot2 { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); + + // Testable + void print(string s = "theta") const; + bool equals(const gtsam::Rot2& rot, double tol) const; + + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; + + // Operator Overloads + gtsam::Rot2 operator*(const gtsam::Rot2& p2) const; + + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; + + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2& p); + Vector logmap(const gtsam::Rot2& p); + + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; + + // Standard Interface + static gtsam::Rot2 relativeBearing( + const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class SO3 { + // Standard Constructors + SO3(); + SO3(Matrix R); + static gtsam::SO3 FromMatrix(Matrix R); + static gtsam::SO3 AxisAngle(const Vector axis, double theta); + static gtsam::SO3 ClosestTo(const Matrix M); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SO3& other, double tol) const; + + // Group + static gtsam::SO3 identity(); + gtsam::SO3 inverse() const; + gtsam::SO3 between(const gtsam::SO3& R) const; + gtsam::SO3 compose(const gtsam::SO3& R) const; + + // Operator Overloads + gtsam::SO3 operator*(const gtsam::SO3& R) const; + + // Manifold + gtsam::SO3 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO3& R) const; + static gtsam::SO3 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + +#include +class SO4 { + // Standard Constructors + SO4(); + SO4(Matrix R); + static gtsam::SO4 FromMatrix(Matrix R); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SO4& other, double tol) const; + + // Group + static gtsam::SO4 identity(); + gtsam::SO4 inverse() const; + gtsam::SO4 between(const gtsam::SO4& Q) const; + gtsam::SO4 compose(const gtsam::SO4& Q) const; + + // Operator Overloads + gtsam::SO4 operator*(const gtsam::SO4& Q) const; + + // Manifold + gtsam::SO4 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO4& Q) const; + static gtsam::SO4 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + +#include +class SOn { + // Standard Constructors + SOn(size_t n); + static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SOn& other, double tol) const; + + // Group + static gtsam::SOn identity(); + gtsam::SOn inverse() const; + gtsam::SOn between(const gtsam::SOn& Q) const; + gtsam::SOn compose(const gtsam::SOn& Q) const; + + // Operator Overloads + gtsam::SOn operator*(const gtsam::SOn& Q) const; + + // Manifold + gtsam::SOn retract(Vector v) const; + Vector localCoordinates(const gtsam::SOn& Q) const; + static gtsam::SOn Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Quaternion { + double w() const; + double x() const; + double y() const; + double z() const; + Vector coeffs() const; +}; + +#include +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, + const gtsam::Point3& col3); + Rot3(double R11, double R12, double R13, double R21, double R22, double R23, + double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); + + 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 AxisAngle(const gtsam::Point3& axis, double angle); + static gtsam::Rot3 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); + static gtsam::Rot3 ClosestTo(const Matrix M); + + // 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; + + // Operator Overloads + gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; + + // Manifold + // gtsam::Rot3 retractCayley(Vector v) const; // TODO, 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); + 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; + pair axisAngle() const; + gtsam::Quaternion toQuaternion() const; + Vector quaternion() const; + gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Pose2 { + // Standard Constructor + Pose2(); + Pose2(const gtsam::Pose2& other); + 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; + + // Operator Overloads + gtsam::Pose2 operator*(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); + Vector logmap(const gtsam::Pose2& p); + static Matrix ExpmapDerivative(Vector v); + static Matrix LogmapDerivative(const gtsam::Pose2& v); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector v); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix wedge(double vx, double vy, double w); + + // Group Actions on Point2 + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(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; + + // enable pickling in python + void pickle() const; +}; + +#include +class Pose3 { + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& other); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); + Pose3(Matrix mat); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Pose3& pose, double tol) const; + + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose) const; + + // Operator Overloads + gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; + + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& pose) const; + + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& pose); + gtsam::Pose3 expmap(Vector v); + Vector logmap(const gtsam::Pose3& pose); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix ExpmapDerivative(Vector xi); + static Matrix LogmapDerivative(const gtsam::Pose3& xi); + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, + double vz); + + // Group Action on Point3 + gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point) const; + + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +class Pose3Pairs { + Pose3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Pose3Pair at(size_t n) const; + void push_back(const gtsam::Pose3Pair& pose_pair); +}; + +class Pose3Vector { + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& pose); +}; + +#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; + gtsam::Point3 point3() const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::Unit3& expected, double tol) const; +}; + +#include +class 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 = "Cal3_S2") 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 K() const; + Matrix inverse() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() 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; + Matrix K() const; + Vector k() const; + Vector vector() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() 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; + + // enable pickling in python + void pickle() 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; + + // Action on Point2 + // Note: the signature of this functions differ from the functions + // with equal name in the base class. + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Cal3Fisheye { + // Standard Constructors + Cal3Fisheye(); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double k3, double k4, double tol = 1e-5); + Cal3Fisheye(Vector v); + + // Testable + void print(string s = "Cal3Fisheye") const; + bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Fisheye retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Fisheye& 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 k1() const; + double k2() const; + double k3() const; + double k4() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + Matrix inverse() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() 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); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, + double tol); + + // 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) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double px() const; + double py() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class CalibratedCamera { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(Vector v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, + double height); + + // Testable + void print(string s = "CalibratedCamera") const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(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& point) const; + double range(const gtsam::Pose3& pose) const; + double range(const gtsam::CalibratedCamera& camera) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() 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 = "PinholeCamera") const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(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& pose); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Similarity3 { + // Standard Constructors + Similarity3(); + Similarity3(double s); + Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); + Similarity3(const Matrix& R, const Vector& t, double s); + Similarity3(const Matrix& T); + + gtsam::Point3 transformFrom(const gtsam::Point3& p) const; + gtsam::Pose3 transformFrom(const gtsam::Pose3& T); + + static gtsam::Similarity3 Align(const gtsam::Point3Pairs& abPointPairs); + static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs); + + // Standard Interface + const Matrix matrix() const; + const gtsam::Rot3& rotation(); + const gtsam::Point3& translation(); + double scale() const; +}; + +// Forward declaration of PinholeCameraCalX is defined here. +#include +// 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; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; + +template +class CameraSet { + CameraSet(); + + // structure specific methods + T at(size_t i) const; + void push_back(const T& cam); +}; + +#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(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; + + // enable pickling in python + void pickle() const; +}; + +#include + +// Templates appear not yet supported for free functions - issue raised at +// borglab/wrap#14 to add support +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::Cal3DS2* 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); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + +#include +template +class BearingRange { + BearingRange(const BEARING& b, const RANGE& r); + BEARING bearing() const; + RANGE range() const; + static This Measure(const POSE& pose, const POINT& point); + static BEARING MeasureBearing(const POSE& pose, const POINT& point); + static RANGE MeasureRange(const POSE& pose, const POINT& point); + void print(string s = "") const; +}; + +typedef gtsam::BearingRange + BearingRange2D; + +} // namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index b18c55818d..6f6ade6f70 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,16 +18,16 @@ #pragma once -#include #include #include #include +#include #include #include #include -#include -#include #include +#include +#include namespace gtsam { diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index c67bd9cea3..a55581e50d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2,10 +2,12 @@ * GTSAM Wrap Module Definition * - * These are the current classes available through the matlab and python wrappers, + * These are the current classes available through the matlab and python + wrappers, * add more functions/classes as they are available. * - * Please refer to the wrapping docs: https://github.com/borglab/wrap/blob/master/README.md + * Please refer to the wrapping docs: + https://github.com/borglab/wrap/blob/master/README.md */ namespace gtsam { @@ -61,8 +63,8 @@ class KeySet { // structure specific methods void insert(size_t key); void merge(const 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 + 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; @@ -123,8 +125,8 @@ class FactorIndexSet { // structure specific methods void insert(size_t factorIndex); - bool erase(size_t factorIndex); // returns true if value was removed - bool count(size_t factorIndex) const; // returns true if value exists + bool erase(size_t factorIndex); // returns true if value was removed + bool count(size_t factorIndex) const; // returns true if value exists }; // Actually a vector @@ -144,3569 +146,53 @@ class FactorIndices { void push_back(size_t factorIndex) const; }; -//************************************************************************* -// base -//************************************************************************* - -/** gtsam namespace functions */ - -#include -bool isDebugVersion(); - -#include -class IndexPair { - IndexPair(); - IndexPair(size_t i, size_t j); - size_t i() const; - size_t j() const; -}; - -// template -// class DSFMap { -// DSFMap(); -// KEY find(const KEY& key) const; -// void merge(const KEY& x, const KEY& y); -// std::map sets(); -// }; - -class IndexPairSet { - IndexPairSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(gtsam::IndexPair key); - bool erase(gtsam::IndexPair key); // returns true if value was removed - bool count(gtsam::IndexPair key) const; // returns true if value exists -}; - -class IndexPairVector { - IndexPairVector(); - IndexPairVector(const gtsam::IndexPairVector& other); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPair at(size_t i) const; - void push_back(gtsam::IndexPair key) const; -}; - -gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); - -class IndexPairSetMap { - IndexPairSetMap(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPairSet at(gtsam::IndexPair& key); -}; - -class DSFMapIndexPair { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair& key) const; - void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); - gtsam::IndexPairSetMap sets(); -}; - -#include -bool linear_independent(Matrix A, Matrix B, double tol); - -#include -virtual class Value { - // No constructors because this is an abstract class - - // Testable - void print(string s = "") const; - - // Manifold - size_t dim() const; -}; - -#include -template -virtual class GenericValue : gtsam::Value { - void serializable() const; -}; - -//************************************************************************* -// geometry -//************************************************************************* - -#include -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Point2& point, double tol) const; - - // Group - static gtsam::Point2 identity(); - - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double distance(const gtsam::Point2& p2) const; - double norm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -// std::vector -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; - - // Operator Overloads - gtsam::StereoPoint2 operator-() const; - // gtsam::StereoPoint2 operator+(Vector b) const; //TODO Mixed types not yet supported - gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 operator-(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; - - // enable pickling in python - void pickle() 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; - - // enable pickling in python - void pickle() const; -}; - -class Point3Pairs { - Point3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Point3Pair at(size_t n) const; - void push_back(const gtsam::Point3Pair& point_pair); -}; - -#include -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); - - // Testable - void print(string s = "theta") const; - bool equals(const gtsam::Rot2& rot, double tol) const; - - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2& p2) const; - gtsam::Rot2 between(const gtsam::Rot2& p2) const; - - // Operator Overloads - gtsam::Rot2 operator*(const gtsam::Rot2& p2) const; - - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2& p) const; - - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); - Vector logmap(const gtsam::Rot2& p); - - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2& point) const; - gtsam::Point2 unrotate(const gtsam::Point2& point) const; - - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class SO3 { - // Standard Constructors - SO3(); - SO3(Matrix R); - static gtsam::SO3 FromMatrix(Matrix R); - static gtsam::SO3 AxisAngle(const Vector axis, double theta); - static gtsam::SO3 ClosestTo(const Matrix M); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SO3& other, double tol) const; - - // Group - static gtsam::SO3 identity(); - gtsam::SO3 inverse() const; - gtsam::SO3 between(const gtsam::SO3& R) const; - gtsam::SO3 compose(const gtsam::SO3& R) const; - - // Operator Overloads - gtsam::SO3 operator*(const gtsam::SO3& R) const; - - // Manifold - gtsam::SO3 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO3& R) const; - static gtsam::SO3 Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; -}; - -#include -class SO4 { - // Standard Constructors - SO4(); - SO4(Matrix R); - static gtsam::SO4 FromMatrix(Matrix R); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SO4& other, double tol) const; - - // Group - static gtsam::SO4 identity(); - gtsam::SO4 inverse() const; - gtsam::SO4 between(const gtsam::SO4& Q) const; - gtsam::SO4 compose(const gtsam::SO4& Q) const; - - // Operator Overloads - gtsam::SO4 operator*(const gtsam::SO4& Q) const; - - // Manifold - gtsam::SO4 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO4& Q) const; - static gtsam::SO4 Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; -}; - -#include -class SOn { - // Standard Constructors - SOn(size_t n); - static gtsam::SOn FromMatrix(Matrix R); - static gtsam::SOn Lift(size_t n, Matrix R); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SOn& other, double tol) const; - - // Group - static gtsam::SOn identity(); - gtsam::SOn inverse() const; - gtsam::SOn between(const gtsam::SOn& Q) const; - gtsam::SOn compose(const gtsam::SOn& Q) const; - - // Operator Overloads - gtsam::SOn operator*(const gtsam::SOn& Q) const; - - // Manifold - gtsam::SOn retract(Vector v) const; - Vector localCoordinates(const gtsam::SOn& Q) const; - static gtsam::SOn Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Quaternion { - double w() const; - double x() const; - double y() const; - double z() const; - Vector coeffs() const; -}; - -#include -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33); - Rot3(double w, double x, double y, double z); - - 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 AxisAngle(const gtsam::Point3& axis, double angle); - static gtsam::Rot3 Rodrigues(Vector v); - static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); - static gtsam::Rot3 ClosestTo(const Matrix M); - - // 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; - - // Operator Overloads - gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; - - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // TODO, 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); - 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; - pair axisAngle() const; - gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; - gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Pose2 { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2& other); - 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; - - // Operator Overloads - gtsam::Pose2 operator*(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); - Vector logmap(const gtsam::Pose2& p); - static Matrix ExpmapDerivative(Vector v); - static Matrix LogmapDerivative(const gtsam::Pose2& v); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector v); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix wedge(double vx, double vy, double w); - - // Group Actions on Point2 - gtsam::Point2 transformFrom(const gtsam::Point2& p) const; - gtsam::Point2 transformTo(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; - - // enable pickling in python - void pickle() const; -}; - -#include -class Pose3 { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& other); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); - Pose3(Matrix mat); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Pose3& pose, double tol) const; - - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& pose) const; - gtsam::Pose3 between(const gtsam::Pose3& pose) const; - - // Operator Overloads - gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; - - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& pose) const; - - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& pose); - gtsam::Pose3 expmap(Vector v); - Vector logmap(const gtsam::Pose3& pose); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector xi); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix ExpmapDerivative(Vector xi); - static Matrix LogmapDerivative(const gtsam::Pose3& xi); - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - - // Group Action on Point3 - gtsam::Point3 transformFrom(const gtsam::Point3& point) const; - gtsam::Point3 transformTo(const gtsam::Point3& point) const; - - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; - gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -class Pose3Pairs { - Pose3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Pose3Pair at(size_t n) const; - void push_back(const gtsam::Pose3Pair& pose_pair); -}; - -class Pose3Vector -{ - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& pose); -}; - -#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; - gtsam::Point3 point3() const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3& s) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::Unit3& expected, double tol) const; -}; - -#include -class 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 = "Cal3_S2") 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 K() const; - Matrix inverse() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() 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; - Matrix K() const; - Vector k() const; - Vector vector() const; - - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() 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; - - // enable pickling in python - void pickle() 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; - - // Action on Point2 - // Note: the signature of this functions differ from the functions - // with equal name in the base class. - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() 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); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); - - // 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) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // Standard Interface - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double px() const; - double py() const; - Vector vector() const; - Vector k() const; - Matrix K() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Cal3Fisheye { - // Standard Constructors - Cal3Fisheye(); - Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); - Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4, double tol); - Cal3Fisheye(Vector v); - - // Testable - void print(string s = "Cal3Fisheye") const; - bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Fisheye retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Fisheye& 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 k1() const; - double k2() const; - double k3() const; - double k4() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Vector k() const; - Matrix K() const; - Matrix inverse() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - - -#include -class CalibratedCamera { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(Vector v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); - - // Testable - void print(string s = "CalibratedCamera") const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(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& point) const; - double range(const gtsam::Pose3& pose) const; - double range(const gtsam::CalibratedCamera& camera) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() 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 = "PinholeCamera") const; - bool equals(const This& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() const; - - // Manifold - This retract(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& pose); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - - -#include -class Similarity3 { - // Standard Constructors - Similarity3(); - Similarity3(double s); - Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); - Similarity3(const Matrix& R, const Vector& t, double s); - Similarity3(const Matrix& T); - - gtsam::Point3 transformFrom(const gtsam::Point3& p) const; - gtsam::Pose3 transformFrom(const gtsam::Pose3& T); - - static gtsam::Similarity3 Align(const gtsam::Point3Pairs & abPointPairs); - static gtsam::Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); - - // Standard Interface - const Matrix matrix() const; - const gtsam::Rot3& rotation(); - const gtsam::Point3& translation(); - double scale() const; -}; - - -// Forward declaration of PinholeCameraCalX is defined here. -#include -// 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; -typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; - -template -class CameraSet { - CameraSet(); - - // structure specific methods - T at(size_t i) const; - void push_back(const T& cam); -}; - -#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(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; - - // enable pickling in python - void pickle() const; -}; - -#include - -// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support -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::Cal3DS2* 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); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); - -//************************************************************************* -// Symbolic -//************************************************************************* - -#include -virtual class SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor& f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); - - // From Factor - size_t size() const; - void print(string s = "SymbolicFactor", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicFactor& other, double tol) const; - gtsam::KeyVector keys(); -}; - -#include -virtual class SymbolicFactorGraph { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); - - // From FactorGraph - void push_back(gtsam::SymbolicFactor* factor); - void print(string s = "SymbolicFactorGraph", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; - - // Standard interface - gtsam::KeySet keys() const; - void push_back(const gtsam::SymbolicFactorGraph& graph); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); - void push_back(const gtsam::SymbolicBayesTree& bayesTree); - - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - - gtsam::SymbolicBayesNet* eliminateSequential(); - gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesTree* eliminateMultifrontal(); - gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); -}; - -#include -virtual class SymbolicConditional : gtsam::SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional& other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicConditional& other, double tol) const; - - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; -}; - -#include -class SymbolicBayesNet { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); - // Testable - void print(string s = "SymbolicBayesNet", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional* at(size_t idx) const; - gtsam::SymbolicConditional* front() const; - gtsam::SymbolicConditional* back() const; - void push_back(gtsam::SymbolicConditional* conditional); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); -}; - -#include -class SymbolicBayesTree { - - //Constructors - SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter); - bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; - - //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; - size_t size(); - void saveGraph(string s) const; - void clear(); - void deleteCachedShortcuts(); - size_t numCachedSeparatorMarginals() const; - - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -class SymbolicBayesTreeClique { - SymbolicBayesTreeClique(); - // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional); - - bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const; - void print(string s = "", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - size_t numCachedSeparatorMarginals() const; - // gtsam::sharedConditional* conditional() const; - bool isRoot() const; - size_t treeSize() const; - gtsam::SymbolicBayesTreeClique* parent() const; - -// // TODO: need wrapped versions graphs, BayesNet -// BayesNet shortcut(derived_ptr root, Eliminate function) const; -// FactorGraph marginal(derived_ptr root, Eliminate function) const; -// FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; -// - void deleteCachedShortcuts(); -}; - -#include -class VariableIndex { - // Standard Constructors and Named Constructors - VariableIndex(); - // TODO: Templetize constructor when wrap supports it - //template - //VariableIndex(const T& factorGraph, size_t nVariables); - //VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& sfg); - VariableIndex(const gtsam::GaussianFactorGraph& gfg); - VariableIndex(const gtsam::NonlinearFactorGraph& fg); - VariableIndex(const gtsam::VariableIndex& other); - - // Testable - bool equals(const gtsam::VariableIndex& other, double tol) const; - void print(string s = "VariableIndex: ", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - - // Standard interface - size_t size() const; - size_t nFactors() const; - size_t nEntries() const; -}; - -//************************************************************************* -// linear -//************************************************************************* - -namespace noiseModel { -#include -virtual class Base { - void print(string s = "") const; - // Methods below are available for all noise models. However, can't add them - // because wrap (incorrectly) thinks robust classes derive from this Base as well. - // bool isConstrained() const; - // bool isUnit() const; - // size_t dim() const; - // Vector sigmas() const; -}; - -virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* Information(Matrix R); - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - - bool equals(gtsam::noiseModel::Base& expected, double tol); - - // access to noise model - Matrix R() const; - Matrix information() const; - Matrix covariance() const; - - // Whitening operations - Vector whiten(Vector v) const; - Vector unwhiten(Vector v) const; - Matrix Whiten(Matrix H) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); - Matrix R() const; - - // access to noise model - Vector sigmas() const; - Vector invsigmas() const; - Vector precisions() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); - static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); - - static gtsam::noiseModel::Constrained* All(size_t dim); - static gtsam::noiseModel::Constrained* All(size_t dim, double mu); - - gtsam::noiseModel::Constrained* unit() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - - // access to noise model - double sigma() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - - // enabling serialization functionality - void serializable() const; -}; - -namespace mEstimator { -virtual class Base { - void print(string s = "") const; -}; - -virtual class Null: gtsam::noiseModel::mEstimator::Base { - Null(); - static gtsam::noiseModel::mEstimator::Null* Create(); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Fair: gtsam::noiseModel::mEstimator::Base { - Fair(double c); - static gtsam::noiseModel::mEstimator::Fair* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Huber: gtsam::noiseModel::mEstimator::Base { - Huber(double k); - static gtsam::noiseModel::mEstimator::Huber* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { - Cauchy(double k); - static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); - static gtsam::noiseModel::mEstimator::Tukey* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Welsch: gtsam::noiseModel::mEstimator::Base { - Welsch(double k); - static gtsam::noiseModel::mEstimator::Welsch* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { - GemanMcClure(double c); - static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class DCS: gtsam::noiseModel::mEstimator::Base { - DCS(double c); - static gtsam::noiseModel::mEstimator::DCS* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { - L2WithDeadZone(double k); - static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -}///\namespace mEstimator - -virtual class Robust : gtsam::noiseModel::Base { - Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace noiseModel - -#include -class Sampler { - // Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - - // Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); -}; - -#include -class VectorValues { - //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); - - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s = "VectorValues", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues& values); - - //Advanced Interface - void setZero(); - - gtsam::VectorValues add(const gtsam::VectorValues& c) const; - void addInPlace(const gtsam::VectorValues& c); - gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); - - bool hasSameStructure(const gtsam::VectorValues& other) const; - double dot(const gtsam::VectorValues& V) const; - double norm() const; - double squaredNorm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class GaussianFactor { - gtsam::KeyVector keys() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* clone() const; - gtsam::GaussianFactor* negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; -}; - -#include -virtual class JacobianFactor : gtsam::GaussianFactor { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactorGraph& graph); - - //Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; - - pair eliminate(const gtsam::Ordering& keys) const; - - void setModel(bool anyConstrained, Vector sigmas); - - gtsam::noiseModel::Diagonal* get_model() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class HessianFactor : gtsam::GaussianFactor { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor& factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianFactorGraph& factors); - - //Testable - size_t size() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - size_t rows() const; - Matrix information() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); - - // From FactorGraph - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - bool exists(size_t idx) const; - - // Building the graph - void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianFactorGraph& graph); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - void push_back(const gtsam::GaussianBayesTree& bayesTree); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; - - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; - - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - - // Elimination and marginals - gtsam::GaussianBayesNet* eliminateSequential(); - gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::GaussianBayesTree* eliminateMultifrontal(); - gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); - - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; - pair jacobian() const; - pair jacobian(const gtsam::Ordering& ordering) const; - Matrix augmentedHessian() const; - Matrix augmentedHessian(const gtsam::Ordering& ordering) const; - pair hessian() const; - pair hessian(const gtsam::Ordering& ordering) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class GaussianConditional : gtsam::JacobianFactor { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); - - //Standard Interface - void print(string s = "GaussianConditional", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianConditional& cg, double tol) const; - - // Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, - const gtsam::VectorValues& rhs) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; - Matrix R() const; - Matrix S() const; - Vector d() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - - //Standard Interface - void print(string s = "GaussianDensity", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; - Vector mean() const; - Matrix covariance() const; -}; - -#include -virtual class GaussianBayesNet { - //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianBayesNet& other, double tol) const; - size_t size() const; - - // FactorGraph derived interface - // size_t size() const; - gtsam::GaussianConditional* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - void saveGraph(const string& s) const; - - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; -}; - -#include -virtual class GaussianBayesTree { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree& other); - bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter); - size_t size() const; - bool empty() const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional* marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -#include -class Errors { - //Constructors - Errors(); - Errors(const gtsam::VectorValues& V); - - //Testable - void print(string s = "Errors"); - bool equals(const gtsam::Errors& expected, double tol) const; -}; - -#include -class GaussianISAM { - //Constructor - GaussianISAM(); - - //Standard Interface - void update(const gtsam::GaussianFactorGraph& newFactors); - void saveGraph(string s) const; - void clear(); -}; - -#include -virtual class IterativeOptimizationParameters { - string getVerbosity() const; - void setVerbosity(string s) ; -}; - -//virtual class IterativeSolver { -// IterativeSolver(); -// gtsam::VectorValues optimize (); -//}; - -#include -virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { - ConjugateGradientParameters(); - int getMinIterations() const ; - int getMaxIterations() const ; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; - - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); -}; - -#include -virtual class PreconditionerParameters { - PreconditionerParameters(); -}; - -virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { - DummyPreconditionerParameters(); -}; - -#include -virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { - PCGSolverParameters(); - void print(string s = ""); - void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); -}; - -#include -virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { - SubgraphSolverParameters(); -}; - -virtual class SubgraphSolver { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - gtsam::VectorValues optimize() const; -}; - -#include -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 - -class Symbol { - Symbol(); - Symbol(char c, uint64_t j); - Symbol(size_t key); - - size_t key() const; - void print(const string& s = "") const; - bool equals(const gtsam::Symbol& expected, double tol) const; - - char chr() const; - uint64_t index() const; - string string() const; -}; - -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); - -namespace symbol_shorthand { - size_t A(size_t j); - size_t B(size_t j); - size_t C(size_t j); - size_t D(size_t j); - size_t E(size_t j); - size_t F(size_t j); - size_t G(size_t j); - size_t H(size_t j); - size_t I(size_t j); - size_t J(size_t j); - size_t K(size_t j); - size_t L(size_t j); - size_t M(size_t j); - size_t N(size_t j); - size_t O(size_t j); - size_t P(size_t j); - size_t Q(size_t j); - size_t R(size_t j); - size_t S(size_t j); - size_t T(size_t j); - size_t U(size_t j); - size_t V(size_t j); - size_t W(size_t j); - size_t X(size_t j); - size_t Y(size_t j); - size_t Z(size_t j); -}///\namespace symbol - -// 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 -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 -class Ordering { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering& other); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::Ordering& ord, double tol) const; - - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - - // FactorGraph - void print(string s = "NonlinearFactorGraph: ", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - void replace(size_t i, gtsam::NonlinearFactor* factors); - void resize(size_t size); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); - void push_back(gtsam::NonlinearFactor* factor); - void add(gtsam::NonlinearFactor* factor); - bool exists(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - - template - void addPrior(size_t key, const T& prior, - const gtsam::noiseModel::Base* noiseModel); - - // NonlinearFactorGraph - void printErrors(const gtsam::Values& values) const; - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - void saveGraph(const string& s) const; -}; - -#include -virtual class NonlinearFactor { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - // NonlinearFactor - bool equals(const gtsam::NonlinearFactor& other, double tol) const; - double error(const gtsam::Values& c) const; - size_t dim() const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; - gtsam::NonlinearFactor* clone() const; - gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; -}; - -#include -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - bool equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; - -#include -virtual class CustomFactor: gtsam::NoiseModelFactor { - /* - * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there. - * This is achieved by adding `gtsam::CustomFactor` to the ignore list in `matlab/CMakeLists.txt`. - */ - CustomFactor(); - /* - * Example: - * ``` - * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): - * - * if not H is None: - * - * H[0] = J1 # 2-d numpy array for a Jacobian block - * H[1] = J2 - * ... - * return error # 1-d numpy array - * - * cf = CustomFactor(noise_model, keys, error_func) - * ``` - */ - CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, - const gtsam::CustomErrorFunction& errorFunction); - - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - -#include -class Values { - Values(); - Values(const gtsam::Values& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::Values& other, double tol) const; - - void insert(const gtsam::Values& values); - void update(const gtsam::Values& values); - void erase(size_t j); - void swap(gtsam::Values& values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - // The order is important: Vector has to precede Point2/Point3 so `atVector` - // can work for those fixed-size vectors. - void insert(size_t j, Vector vector); - void insert(size_t j, Matrix matrix); - void insert(size_t j, const gtsam::Point2& point2); - void insert(size_t j, const gtsam::Point3& point3); - void insert(size_t j, const gtsam::Rot2& rot2); - void insert(size_t j, const gtsam::Pose2& pose2); - void insert(size_t j, const gtsam::SO3& R); - void insert(size_t j, const gtsam::SO4& Q); - void insert(size_t j, const gtsam::SOn& P); - void insert(size_t j, const gtsam::Rot3& rot3); - void insert(size_t j, const gtsam::Pose3& pose3); - void insert(size_t j, const gtsam::Unit3& unit3); - void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); - void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); - void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); - void insert(size_t j, const gtsam::Cal3Unified& cal3unified); - void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); - void insert(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera); - void insert(size_t j, const gtsam::PinholeCameraCal3Unified& camera); - void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void insert(size_t j, const gtsam::NavState& nav_state); - void insert(size_t j, double c); - - void update(size_t j, const gtsam::Point2& point2); - void update(size_t j, const gtsam::Point3& point3); - void update(size_t j, const gtsam::Rot2& rot2); - void update(size_t j, const gtsam::Pose2& pose2); - void update(size_t j, const gtsam::SO3& R); - void update(size_t j, const gtsam::SO4& Q); - void update(size_t j, const gtsam::SOn& P); - void update(size_t j, const gtsam::Rot3& rot3); - void update(size_t j, const gtsam::Pose3& pose3); - void update(size_t j, const gtsam::Unit3& unit3); - void update(size_t j, const gtsam::Cal3_S2& cal3_s2); - void update(size_t j, const gtsam::Cal3DS2& cal3ds2); - void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); - void update(size_t j, const gtsam::Cal3Unified& cal3unified); - void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); - void update(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera); - void update(size_t j, const gtsam::PinholeCameraCal3Unified& camera); - void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void update(size_t j, const gtsam::NavState& nav_state); - void update(size_t j, Vector vector); - void update(size_t j, Matrix matrix); - void update(size_t j, double c); - - template - T at(size_t j); - -}; - -#include -class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - Marginals(const gtsam::GaussianFactorGraph& gfgraph, - const gtsam::Values& solution); - Marginals(const gtsam::GaussianFactorGraph& gfgraph, - const gtsam::VectorValues& solutionvec); - - void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; -}; - -class JointMarginal { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; -}; - -#include -virtual class LinearContainerFactor : gtsam::NonlinearFactor { - - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor); - - gtsam::GaussianFactor* factor() const; -// const boost::optional& 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 -// -//// 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 -virtual class NonlinearOptimizerParams { - NonlinearOptimizerParams(); - void print(string s = "") const; - - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; - - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); - - string getLinearSolverType() const; - void setLinearSolverType(string solver); - - void setIterativeParams(gtsam::IterativeOptimizationParameters* params); - void setOrdering(const gtsam::Ordering& ordering); - string getOrderingType() const; - void setOrderingType(string ordering); - - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; -}; - -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); -bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, - double currentError, double newError); - -#include -virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { - GaussNewtonParams(); -}; - -#include -virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { - LevenbergMarquardtParams(); - - bool getDiagonalDamping() const; - double getlambdaFactor() const; - double getlambdaInitial() const; - double getlambdaLowerBound() const; - double getlambdaUpperBound() const; - bool getUseFixedLambdaFactor(); - string getLogFile() const; - string getVerbosityLM() const; - - void setDiagonalDamping(bool flag); - void setlambdaFactor(double value); - void setlambdaInitial(double value); - void setlambdaLowerBound(double value); - void setlambdaUpperBound(double value); - void setUseFixedLambdaFactor(bool flag); - void setLogFile(string s); - void setVerbosityLM(string s); - - static gtsam::LevenbergMarquardtParams LegacyDefaults(); - static gtsam::LevenbergMarquardtParams CeresDefaults(); - - static gtsam::LevenbergMarquardtParams EnsureHasOrdering( - gtsam::LevenbergMarquardtParams params, - const gtsam::NonlinearFactorGraph& graph); - static gtsam::LevenbergMarquardtParams ReplaceOrdering( - gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); -}; - -#include -virtual class DoglegParams : gtsam::NonlinearOptimizerParams { - DoglegParams(); - - double getDeltaInitial() const; - string getVerbosityDL() const; - - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; -}; - -#include -virtual class NonlinearOptimizer { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - gtsam::NonlinearFactorGraph graph() const; - gtsam::GaussianFactorGraph* iterate() const; -}; - -#include -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 -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 -virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); - double lambda() const; - void print(string s = "") const; -}; - -#include -class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; - -class ISAM2DoglegParams { - ISAM2DoglegParams(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); -}; - -class ISAM2ThresholdMapValue { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); -}; - -class ISAM2ThresholdMap { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue& value) const; -}; - -class ISAM2Params { - ISAM2Params(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); - void setRelinearizeThreshold(double threshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); -}; - -class ISAM2Clique { - - //Constructors - ISAM2Clique(); - - //Standard Interface - Vector gradientContribution() const; - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - -class ISAM2Result { - ISAM2Result(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; - double getErrorBefore() const; - double getErrorAfter() const; -}; - -class ISAM2 { - ISAM2(); - ISAM2(const gtsam::ISAM2Params& params); - ISAM2(const gtsam::ISAM2& other); - - bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printStats() const; - void saveGraph(string s) const; - - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - - gtsam::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - template - VALUE 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 -class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); - void reorder_relinearize(); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -//************************************************************************* -// Nonlinear factor types -//************************************************************************* -#include -template -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - T prior() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); - T measured() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -template -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 -template -virtual class RangeFactor : gtsam::NoiseModelFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::RangeFactor RangeFactor2D; -typedef gtsam::RangeFactor RangeFactor3D; -typedef gtsam::RangeFactor RangeFactorPose2; -typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; - - -#include -template -virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { - RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; - -#include -template -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 BearingFactor2D; -typedef gtsam::BearingFactor BearingFactor3D; -typedef gtsam::BearingFactor BearingFactorPose2; - -#include -template -class BearingRange { - BearingRange(const BEARING& b, const RANGE& r); - BEARING bearing() const; - RANGE range() const; - static This Measure(const POSE& pose, const POINT& point); - static BEARING MeasureBearing(const POSE& pose, const POINT& point); - static RANGE MeasureRange(const POSE& pose, const POINT& point); - void print(string s = "") const; -}; - -typedef gtsam::BearingRange BearingRange2D; - -#include -template -virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, - const BEARING& measuredBearing, const RANGE& measuredRange, - const gtsam::noiseModel::Base* noiseModel); - - gtsam::BearingRange measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; -typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; - - -#include -template -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 GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Fisheye; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; - - -#include -template -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 GeneralSFMFactorCal3_S2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Bundler; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Fisheye; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Unified; - -template -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 - -/// Linearization mode: what factor to linearize to -enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; - -/// How to manage degeneracy -enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; - -class SmartProjectionParams { - SmartProjectionParams(); - void setLinearizationMode(gtsam::LinearizationMode linMode); - void setDegeneracyMode(gtsam::DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); -}; - -#include -template -virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::SmartProjectionParams& params); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor, - const gtsam::SmartProjectionParams& params); - - void add(const gtsam::Point2& measured_i, size_t poseKey_i); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - - -#include -template -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 GenericStereoFactor3D; - -#include -template -virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { - PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; -typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; - -#include -template -virtual class PoseRotationPrior : gtsam::NoiseModelFactor { - PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseRotationPrior PoseRotationPrior2D; -typedef gtsam::PoseRotationPrior PoseRotationPrior3D; - -#include -virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); -}; - -#include - -class SfmTrack { - SfmTrack(); - SfmTrack(const gtsam::Point3& pt); - const Point3& point3() const; - - double r; - double g; - double b; - - std::vector> measurements; - - size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void add_measurement(size_t idx, const gtsam::Point2& m); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmTrack& expected, double tol) const; -}; - -class SfmData { - SfmData(); - size_t number_cameras() const; - size_t number_tracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; - void add_track(const gtsam::SfmTrack& t) ; - void add_camera(const gtsam::SfmCamera& cam); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmData& expected, double tol) const; -}; - -gtsam::SfmData readBal(string filename); -bool writeBAL(string filename, gtsam::SfmData& data); -gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); -gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); - -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); -pair load2D_robust(string filename, - gtsam::noiseModel::Base* model, int maxIndex); -void save2D(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, - string filename); - -// std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose2] -class BetweenFactorPose2s -{ - BetweenFactorPose2s(); - size_t size() const; - gtsam::BetweenFactor* at(size_t i) const; - void push_back(const gtsam::BetweenFactor* factor); -}; -gtsam::BetweenFactorPose2s parse2DFactors(string filename); - -// std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose3] -class BetweenFactorPose3s -{ - BetweenFactorPose3s(); - size_t size() const; - gtsam::BetweenFactor* at(size_t i) const; - void push_back(const gtsam::BetweenFactor* factor); -}; -gtsam::BetweenFactorPose3s parse3DFactors(string filename); - -pair load3D(string filename); - -pair readG2o(string filename); -pair readG2o(string filename, bool is3D); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); - -#include -class InitializePose3 { - static gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph& pose3Graph); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess); - static gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph& graph); - static gtsam::Values initializeOrientations( - const gtsam::NonlinearFactorGraph& graph); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& givenGuess, - bool useGradient); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); -}; - -#include -template -virtual class KarcherMeanFactor : gtsam::NonlinearFactor { - KarcherMeanFactor(const gtsam::KeyVector& keys); -}; - -#include -gtsam::noiseModel::Isotropic* ConvertNoiseModel( - gtsam::noiseModel::Base* model, size_t d); - -template -virtual class FrobeniusFactor : gtsam::NoiseModelFactor { - FrobeniusFactor(size_t key1, size_t key2); - FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); - - Vector evaluateError(const T& R1, const T& R2); -}; - -template -virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); - - Vector evaluateError(const T& R1, const T& R2); -}; - -#include - -virtual class ShonanFactor3 : gtsam::NoiseModelFactor { - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p); - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p, gtsam::noiseModel::Base *model); - Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); -}; - -#include -template -class BinaryMeasurement { - BinaryMeasurement(size_t key1, size_t key2, const T& measured, - const gtsam::noiseModel::Base* model); - size_t key1() const; - size_t key2() const; - T measured() const; - gtsam::noiseModel::Base* noiseModel() const; -}; - -typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; -typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; - -class BinaryMeasurementsUnit3 { - BinaryMeasurementsUnit3(); - size_t size() const; - gtsam::BinaryMeasurement at(size_t idx) const; - void push_back(const gtsam::BinaryMeasurement& measurement); -}; - -#include - -// TODO(frank): copy/pasta below until we have integer template paremeters in wrap! - -class ShonanAveragingParameters2 { - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot2& value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight() const; - void setGaugesWeight(double value); - double getGaugesWeight() const; - void setUseHuber(bool value); - bool getUseHuber() const; - void setCertifyOptimality(bool value); - bool getCertifyOptimality() const; -}; - -class ShonanAveragingParameters3 { - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot3& value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight() const; - void setGaugesWeight(double value); - double getGaugesWeight() const; - void setUseHuber(bool value); - bool getUseHuber() const; - void setCertifyOptimality(bool value); - bool getCertifyOptimality() const; -}; - -class ShonanAveraging2 { - ShonanAveraging2(string g2oFile); - ShonanAveraging2(string g2oFile, - const gtsam::ShonanAveragingParameters2 ¶meters); - ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, - const gtsam::ShonanAveragingParameters2 ¶meters); - - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot2 measured(size_t i); - gtsam::KeyVector keys(size_t i); - - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values& values) const; - Matrix computeA_(const gtsam::Values& values) const; - double computeMinEigenValue(const gtsam::Values& values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, - const Vector& minEigenVector, double minEigenValue) const; - - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values& values) const; - pair computeMinEigenVector(const gtsam::Values& values) const; - bool checkOptimality(const gtsam::Values& values) const; - gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; - gtsam::Values roundSolution(const gtsam::Values& values) const; - - // Basic API - double cost(const gtsam::Values& values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; -}; - -class ShonanAveraging3 { - ShonanAveraging3(string g2oFile); - ShonanAveraging3(string g2oFile, - const gtsam::ShonanAveragingParameters3 ¶meters); - - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, - const gtsam::ShonanAveragingParameters3 ¶meters); - - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot3 measured(size_t i); - gtsam::KeyVector keys(size_t i); - - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values& values) const; - Matrix computeA_(const gtsam::Values& values) const; - double computeMinEigenValue(const gtsam::Values& values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, - const Vector& minEigenVector, double minEigenValue) const; - - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values& values) const; - pair computeMinEigenVector(const gtsam::Values& values) const; - bool checkOptimality(const gtsam::Values& values) const; - gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; - gtsam::Values roundSolution(const gtsam::Values& values) const; - - // Basic API - double cost(const gtsam::Values& values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; -}; - -#include - -class KeyPairDoubleMap { - KeyPairDoubleMap(); - KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t at(const pair& keypair) const; -}; - -class MFAS { - MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::Unit3& projectionDirection); - - gtsam::KeyPairDoubleMap computeOutlierWeights() const; - gtsam::KeyVector computeOrdering() const; -}; - -#include -class TranslationRecovery { - TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, - const gtsam::LevenbergMarquardtParams &lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams - gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 -}; - -//************************************************************************* -// Navigation -//************************************************************************* -namespace imuBias { -#include - -class ConstantBias { - // Constructors - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; - - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; - - // Operator Overloads - gtsam::imuBias::ConstantBias operator-() const; - gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; - - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; -}; - -}///\namespace imuBias - -#include -class NavState { - // Constructors - NavState(); - NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); - NavState(const gtsam::Pose3& pose, Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::NavState& expected, double tol) const; - - // Access - gtsam::Rot3 attitude() const; - gtsam::Point3 position() const; - Vector velocity() const; - gtsam::Pose3 pose() const; - - gtsam::NavState retract(const Vector& x) const; - Vector localCoordinates(const gtsam::NavState& g) const; -}; - -#include -virtual class PreintegratedRotationParams { - PreintegratedRotationParams(); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); - - void setGyroscopeCovariance(Matrix cov); - void setOmegaCoriolis(Vector omega); - void setBodyPSensor(const gtsam::Pose3& pose); - - Matrix getGyroscopeCovariance() const; - - boost::optional getOmegaCoriolis() const; - boost::optional getBodyPSensor() const; -}; - -#include -virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { - PreintegrationParams(Vector n_gravity); - - static gtsam::PreintegrationParams* MakeSharedD(double g); - static gtsam::PreintegrationParams* MakeSharedU(double g); - static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegrationParams& expected, double tol); - - void setAccelerometerCovariance(Matrix cov); - void setIntegrationCovariance(Matrix cov); - void setUse2ndOrderCoriolis(bool flag); - - Matrix getAccelerometerCovariance() const; - Matrix getIntegrationCovariance() const; - bool getUse2ndOrderCoriolis() const; -}; - -#include -class PreintegratedImuMeasurements { - // Constructors - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, - const gtsam::imuBias::ConstantBias& bias); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - - Matrix preintMeasCov() const; - Vector preintegrated() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - -virtual class ImuFactor: gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); - - // Standard Interface - gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, - const gtsam::Pose3& pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias& bias); -}; - -#include -virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { - PreintegrationCombinedParams(Vector n_gravity); - - static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); - static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); - static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); - - void setBiasAccCovariance(Matrix cov); - void setBiasOmegaCovariance(Matrix cov); - void setBiasAccOmegaInt(Matrix cov); - - Matrix getBiasAccCovariance() const ; - Matrix getBiasOmegaCovariance() const ; - Matrix getBiasAccOmegaInt() const; - -}; - -class PreintegratedCombinedMeasurements { -// Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, - const gtsam::imuBias::ConstantBias& bias); - // Testable - void print(string s = "Preintegrated Measurements:") const; - bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, - double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - - Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - -virtual class CombinedImuFactor: gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); - - // Standard Interface - gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, - const gtsam::Pose3& pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::imuBias::ConstantBias& bias_j); -}; - -#include -class PreintegratedAhrsMeasurements { - // Standard Constructor - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); - - // Testable - void print(string s = "Preintegrated Measurements: ") const; - bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); - - // get Data - gtsam::Rot3 deltaRij() const; - double deltaTij() const; - Vector biasHat() const; - - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void resetIntegration() ; -}; - -virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - - // Standard Interface - gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, - Vector omegaCoriolis) const; -}; - -#include -//virtual class AttitudeFactor : gtsam::NonlinearFactor { -// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); -// AttitudeFactor(); -//}; -virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Rot3AttitudeFactor(); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, - const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, - const gtsam::noiseModel::Diagonal* model); - Pose3AttitudeFactor(); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -#include -virtual class GPSFactor : gtsam::NonlinearFactor{ - GPSFactor(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor& expected, double tol); - - // Standard Interface - gtsam::Point3 measurementIn() const; -}; - -virtual class GPSFactor2 : gtsam::NonlinearFactor { - GPSFactor2(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor2& expected, double tol); - - // Standard Interface - gtsam::Point3 measurementIn() const; -}; - -#include -virtual class Scenario { - gtsam::Pose3 pose(double t) const; - Vector omega_b(double t) const; - Vector velocity_n(double t) const; - Vector acceleration_n(double t) const; - gtsam::Rot3 rotation(double t) const; - gtsam::NavState navState(double t) const; - Vector velocity_b(double t) const; - Vector acceleration_b(double t) const; -}; - -virtual class ConstantTwistScenario : gtsam::Scenario { - ConstantTwistScenario(Vector w, Vector v); - ConstantTwistScenario(Vector w, Vector v, - const gtsam::Pose3& nTb0); -}; - -virtual class AcceleratingScenario : gtsam::Scenario { - AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, - Vector v0, Vector a_n, - Vector omega_b); -}; - -#include -class ScenarioRunner { - ScenarioRunner(const gtsam::Scenario& scenario, - const gtsam::PreintegrationParams* p, - double imuSampleTime, - const gtsam::imuBias::ConstantBias& bias); - Vector gravity_n() const; - Vector actualAngularVelocity(double t) const; - Vector actualSpecificForce(double t) const; - Vector measuredAngularVelocity(double t) const; - Vector measuredSpecificForce(double t) const; - double imuSampleTime() const; - gtsam::PreintegratedImuMeasurements integrate( - double T, const gtsam::imuBias::ConstantBias& estimatedBias, - bool corrupted) const; - gtsam::NavState predict( - const gtsam::PreintegratedImuMeasurements& pim, - const gtsam::imuBias::ConstantBias& estimatedBias) const; - Matrix estimateCovariance( - double T, size_t N, - const gtsam::imuBias::ConstantBias& estimatedBias) const; - Matrix estimateNoiseCovariance(size_t N) const; -}; - //************************************************************************* // Utilities //************************************************************************* namespace utilities { - #include - 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); - gtsam::Values allPose2s(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::PinholeCameraCal3_S2& 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 - #include +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); +gtsam::Values allPose2s(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::PinholeCamera& 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 + class RedirectCout { RedirectCout(); string str(); }; -} +} // namespace gtsam diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i new file mode 100644 index 0000000000..63047bf4f3 --- /dev/null +++ b/gtsam/linear/linear.i @@ -0,0 +1,653 @@ +//************************************************************************* +// linear +//************************************************************************* +namespace gtsam { + +namespace noiseModel { +#include +virtual class Base { + void print(string s = "") const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; +}; + +virtual class Gaussian : gtsam::noiseModel::Base { + static gtsam::noiseModel::Gaussian* Information(Matrix R); + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + + bool equals(gtsam::noiseModel::Base& expected, double tol); + + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Diagonal : gtsam::noiseModel::Gaussian { + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + Matrix R() const; + + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Constrained : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); + + static gtsam::noiseModel::Constrained* All(size_t dim); + static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + + gtsam::noiseModel::Constrained* unit() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Isotropic : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + + // access to noise model + double sigma() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Unit : gtsam::noiseModel::Isotropic { + static gtsam::noiseModel::Unit* Create(size_t dim); + + // enabling serialization functionality + void serializable() const; +}; + +namespace mEstimator { +virtual class Base { + void print(string s = "") const; +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { + Cauchy(double k); + static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Welsch: gtsam::noiseModel::mEstimator::Base { + Welsch(double k); + static gtsam::noiseModel::mEstimator::Welsch* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class DCS: gtsam::noiseModel::mEstimator::Base { + DCS(double c); + static gtsam::noiseModel::mEstimator::DCS* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { + L2WithDeadZone(double k); + static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace noiseModel + +#include +class Sampler { + // Constructors + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + + // Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); +}; + +#include +class VectorValues { + //Constructors + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s = "VectorValues", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); + + //Advanced Interface + void setZero(); + + gtsam::VectorValues add(const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); + + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class GaussianFactor { + gtsam::KeyVector keys() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; + gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; +}; + +#include +virtual class JacobianFactor : gtsam::GaussianFactor { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactorGraph& graph); + + //Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; + + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + + pair eliminate(const gtsam::Ordering& keys) const; + + void setModel(bool anyConstrained, Vector sigmas); + + gtsam::noiseModel::Diagonal* get_model() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class HessianFactor : gtsam::GaussianFactor { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianFactorGraph& factors); + + //Testable + size_t size() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + size_t rows() const; + Matrix information() const; + double constantTerm() const; + Vector linearTerm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class GaussianFactorGraph { + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + + // From FactorGraph + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + bool exists(size_t idx) const; + + // Building the graph + void push_back(const gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianFactorGraph& graph); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + void push_back(const gtsam::GaussianBayesTree& bayesTree); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; + + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; + + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + + // Elimination and marginals + gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); + + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; + pair jacobian() const; + pair jacobian(const gtsam::Ordering& ordering) const; + Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering& ordering) const; + pair hessian() const; + pair hessian(const gtsam::Ordering& ordering) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class GaussianConditional : gtsam::JacobianFactor { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T); + + //Standard Interface + void print(string s = "GaussianConditional", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianConditional& cg, double tol) const; + + // Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, + const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + Matrix R() const; + Matrix S() const; + Vector d() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianDensity : gtsam::GaussianConditional { + //Constructors + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + + //Standard Interface + void print(string s = "GaussianDensity", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; +}; + +#include +virtual class GaussianBayesNet { + //Constructors + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianBayesNet& other, double tol) const; + size_t size() const; + + // FactorGraph derived interface + // size_t size() const; + gtsam::GaussianConditional* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + void saveGraph(const string& s) const; + + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; +}; + +#include +virtual class GaussianBayesTree { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree& other); + bool equals(const gtsam::GaussianBayesTree& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional* marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +#include +class Errors { + //Constructors + Errors(); + Errors(const gtsam::VectorValues& V); + + //Testable + void print(string s = "Errors"); + bool equals(const gtsam::Errors& expected, double tol) const; +}; + +#include +class GaussianISAM { + //Constructor + GaussianISAM(); + + //Standard Interface + void update(const gtsam::GaussianFactorGraph& newFactors); + void saveGraph(string s) const; + void clear(); +}; + +#include +virtual class IterativeOptimizationParameters { + string getVerbosity() const; + void setVerbosity(string s) ; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + int getMinIterations() const ; + int getMaxIterations() const ; + int getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); +}; + +#include +virtual class PreconditionerParameters { + PreconditionerParameters(); +}; + +virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { + DummyPreconditionerParameters(); +}; + +#include +virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { + PCGSolverParameters(); + void print(string s = ""); + void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); +}; + +virtual class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + gtsam::VectorValues optimize() const; +}; + +#include +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); +}; + +} \ No newline at end of file diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i new file mode 100644 index 0000000000..48a5a35de6 --- /dev/null +++ b/gtsam/navigation/navigation.i @@ -0,0 +1,355 @@ +//************************************************************************* +// Navigation +//************************************************************************* + +namespace gtsam { + +namespace imuBias { +#include + +class ConstantBias { + // Constructors + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; + + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; + + // Operator Overloads + gtsam::imuBias::ConstantBias operator-() const; + gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; + + // Manifold + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; + + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; + +}///\namespace imuBias + +#include +class NavState { + // Constructors + NavState(); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); + NavState(const gtsam::Pose3& pose, Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::NavState& expected, double tol) const; + + // Access + gtsam::Rot3 attitude() const; + gtsam::Point3 position() const; + Vector velocity() const; + gtsam::Pose3 pose() const; + + gtsam::NavState retract(const Vector& x) const; + Vector localCoordinates(const gtsam::NavState& g) const; +}; + +#include +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); + + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); + + Matrix getGyroscopeCovariance() const; + + boost::optional getOmegaCoriolis() const; + boost::optional getBodyPSensor() const; +}; + +#include +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { + PreintegrationParams(Vector n_gravity); + + static gtsam::PreintegrationParams* MakeSharedD(double g); + static gtsam::PreintegrationParams* MakeSharedU(double g); + static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegrationParams& expected, double tol); + + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); + + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; +}; + +#include +class PreintegratedImuMeasurements { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + + Matrix preintMeasCov() const; + Vector preintegrated() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; + +virtual class ImuFactor: gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias); +}; + +#include +virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { + PreintegrationCombinedParams(Vector n_gravity); + + static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); + + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); + + Matrix getBiasAccCovariance() const ; + Matrix getBiasOmegaCovariance() const ; + Matrix getBiasAccOmegaInt() const; + +}; + +class PreintegratedCombinedMeasurements { +// Constructors + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); + // Testable + void print(string s = "Preintegrated Measurements:") const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, + double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + + Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; + +virtual class CombinedImuFactor: gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j); +}; + +#include +class PreintegratedAhrsMeasurements { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); + + // Testable + void print(string s = "Preintegrated Measurements: ") const; + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); + + // get Data + gtsam::Rot3 deltaRij() const; + double deltaTij() const; + Vector biasHat() const; + + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void resetIntegration() ; +}; + +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; +}; + +#include +//virtual class AttitudeFactor : gtsam::NonlinearFactor { +// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); +// AttitudeFactor(); +//}; +virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Rot3AttitudeFactor(); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model); + Pose3AttitudeFactor(); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +#include +virtual class GPSFactor : gtsam::NonlinearFactor{ + GPSFactor(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GPSFactor& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + +virtual class GPSFactor2 : gtsam::NonlinearFactor { + GPSFactor2(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GPSFactor2& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + +#include +virtual class Scenario { + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; +}; + +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, + const gtsam::Pose3& nTb0); +}; + +virtual class AcceleratingScenario : gtsam::Scenario { + AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, + Vector v0, Vector a_n, + Vector omega_b); +}; + +#include +class ScenarioRunner { + ScenarioRunner(const gtsam::Scenario& scenario, + const gtsam::PreintegrationParams* p, + double imuSampleTime, + const gtsam::imuBias::ConstantBias& bias); + Vector gravity_n() const; + Vector actualAngularVelocity(double t) const; + Vector actualSpecificForce(double t) const; + Vector measuredAngularVelocity(double t) const; + Vector measuredSpecificForce(double t) const; + double imuSampleTime() const; + gtsam::PreintegratedImuMeasurements integrate( + double T, const gtsam::imuBias::ConstantBias& estimatedBias, + bool corrupted) const; + gtsam::NavState predict( + const gtsam::PreintegratedImuMeasurements& pim, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateCovariance( + double T, size_t N, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateNoiseCovariance(size_t N) const; +}; + +} diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i new file mode 100644 index 0000000000..8071e8bc79 --- /dev/null +++ b/gtsam/nonlinear/nonlinear.i @@ -0,0 +1,804 @@ +//************************************************************************* +// nonlinear +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Symbol { + Symbol(); + Symbol(char c, uint64_t j); + Symbol(size_t key); + + size_t key() const; + void print(const string& s = "") const; + bool equals(const gtsam::Symbol& expected, double tol) const; + + char chr() const; + uint64_t index() const; + string string() const; +}; + +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); + +namespace symbol_shorthand { +size_t A(size_t j); +size_t B(size_t j); +size_t C(size_t j); +size_t D(size_t j); +size_t E(size_t j); +size_t F(size_t j); +size_t G(size_t j); +size_t H(size_t j); +size_t I(size_t j); +size_t J(size_t j); +size_t K(size_t j); +size_t L(size_t j); +size_t M(size_t j); +size_t N(size_t j); +size_t O(size_t j); +size_t P(size_t j); +size_t Q(size_t j); +size_t R(size_t j); +size_t S(size_t j); +size_t T(size_t j); +size_t U(size_t j); +size_t V(size_t j); +size_t W(size_t j); +size_t X(size_t j); +size_t Y(size_t j); +size_t Z(size_t j); +} // namespace symbol_shorthand + +// Default keyformatter +void PrintKeyList(const gtsam::KeyList& keys); +void PrintKeyList(const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet(const gtsam::KeySet& keys); +void PrintKeySet(const gtsam::KeySet& keys, string s); + +#include +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 +class Ordering { + // Standard Constructors and Named Constructors + Ordering(); + Ordering(const gtsam::Ordering& other); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::Ordering& ord, double tol) const; + + // Standard interface + size_t size() const; + size_t at(size_t key) const; + void push_back(size_t key); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class NonlinearFactorGraph { + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s = "NonlinearFactorGraph: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + void replace(size_t i, gtsam::NonlinearFactor* factors); + void resize(size_t size); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + + template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> + void addPrior(size_t key, const T& prior, + const gtsam::noiseModel::Base* noiseModel); + + // NonlinearFactorGraph + void printErrors(const gtsam::Values& values) const; + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const + // std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + void saveGraph(const string& s) const; +}; + +#include +virtual class NonlinearFactor { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + // NonlinearFactor + bool equals(const gtsam::NonlinearFactor& other, double tol) const; + double error(const gtsam::Values& c) const; + size_t dim() const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; + gtsam::NonlinearFactor* clone() const; + gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; +}; + +#include +virtual class NoiseModelFactor : gtsam::NonlinearFactor { + bool equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#include +virtual class CustomFactor : gtsam::NoiseModelFactor { + /* + * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting + * machinery there. This is achieved by adding `gtsam::CustomFactor` to the + * ignore list in `matlab/CMakeLists.txt`. + */ + CustomFactor(); + /* + * Example: + * ``` + * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + * + * if not H is None: + * + * H[0] = J1 # 2-d numpy array for a Jacobian block + * H[1] = J2 + * ... + * return error # 1-d numpy array + * + * cf = CustomFactor(noise_model, keys, error_func) + * ``` + */ + CustomFactor(const gtsam::SharedNoiseModel& noiseModel, + const gtsam::KeyVector& keys, + const gtsam::CustomErrorFunction& errorFunction); + + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +#include +class Values { + Values(); + Values(const gtsam::Values& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; + + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::Values& other, double tol) const; + + void insert(const gtsam::Values& values); + void update(const gtsam::Values& values); + void erase(size_t j); + void swap(gtsam::Values& values); + + bool exists(size_t j) const; + gtsam::KeyVector keys() const; + + gtsam::VectorValues zeroVectors() const; + + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // New in 4.0, we have to specialize every insert/update/at to generate + // wrappers Instead of the old: void insert(size_t j, const gtsam::Value& + // value); void update(size_t j, const gtsam::Value& val); gtsam::Value + // at(size_t j) const; + + // The order is important: Vector has to precede Point2/Point3 so `atVector` + // can work for those fixed-size vectors. + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); + void insert(size_t j, const gtsam::Point2& point2); + void insert(size_t j, const gtsam::Point3& point3); + void insert(size_t j, const gtsam::Rot2& rot2); + void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::SO3& R); + void insert(size_t j, const gtsam::SO4& Q); + void insert(size_t j, const gtsam::SOn& P); + void insert(size_t j, const gtsam::Rot3& rot3); + void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Unit3& unit3); + void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); + void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); + void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void insert(size_t j, const gtsam::Cal3Unified& cal3unified); + void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(size_t j, const gtsam::NavState& nav_state); + void insert(size_t j, double c); + + void update(size_t j, const gtsam::Point2& point2); + void update(size_t j, const gtsam::Point3& point3); + void update(size_t j, const gtsam::Rot2& rot2); + void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::SO3& R); + void update(size_t j, const gtsam::SO4& Q); + void update(size_t j, const gtsam::SOn& P); + void update(size_t j, const gtsam::Rot3& rot3); + void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Unit3& unit3); + void update(size_t j, const gtsam::Cal3_S2& cal3_s2); + void update(size_t j, const gtsam::Cal3DS2& cal3ds2); + void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void update(size_t j, const gtsam::Cal3Unified& cal3unified); + void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void update(size_t j, const gtsam::NavState& nav_state); + void update(size_t j, Vector vector); + void update(size_t j, Matrix matrix); + void update(size_t j, double c); + + template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias, + gtsam::NavState, + Vector, + Matrix, + double}> + T at(size_t j); +}; + +#include +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::VectorValues& solutionvec); + + void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance( + const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation( + const gtsam::KeyVector& variables) const; +}; + +class JointMarginal { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s = "", gtsam::KeyFormatter keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; + +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + LinearContainerFactor(gtsam::GaussianFactor* factor, + const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor); + + gtsam::GaussianFactor* factor() const; + // const boost::optional& 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 +// +//// 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 +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + void print(string s = "") const; + + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; + + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); + + string getLinearSolverType() const; + void setLinearSolverType(string solver); + + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); + + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; +}; + +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); +bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, + double currentError, double newError); + +#include +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { + GaussNewtonParams(); +}; + +#include +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); + + bool getDiagonalDamping() const; + double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; + double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; + string getVerbosityLM() const; + + void setDiagonalDamping(bool flag); + void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); + void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); + void setVerbosityLM(string s); + + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); + + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); +}; + +#include +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { + DoglegParams(); + + double getDeltaInitial() const; + string getVerbosityDL() const; + + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; + +#include +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + gtsam::NonlinearFactorGraph graph() const; + gtsam::GaussianFactorGraph* iterate() const; +}; + +#include +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 +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 +virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::LevenbergMarquardtParams& params); + double lambda() const; + void print(string s = "") const; +}; + +#include +class ISAM2GaussNewtonParams { + ISAM2GaussNewtonParams(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + +class ISAM2DoglegParams { + ISAM2DoglegParams(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + double getInitialDelta() const; + void setInitialDelta(double initialDelta); + string getAdaptationMode() const; + void setAdaptationMode(string adaptationMode); + bool isVerbose() const; + void setVerbose(bool verbose); +}; + +class ISAM2ThresholdMapValue { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); +}; + +class ISAM2ThresholdMap { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue& value) const; +}; + +class ISAM2Params { + ISAM2Params(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + void setOptimizationParams( + const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); + void setRelinearizeThreshold(double threshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); + int getRelinearizeSkip() const; + void setRelinearizeSkip(int relinearizeSkip); + bool isEnableRelinearization() const; + void setEnableRelinearization(bool enableRelinearization); + bool isEvaluateNonlinearError() const; + void setEvaluateNonlinearError(bool evaluateNonlinearError); + string getFactorization() const; + void setFactorization(string factorization); + bool isCacheLinearizedFactors() const; + void setCacheLinearizedFactors(bool cacheLinearizedFactors); + bool isEnableDetailedResults() const; + void setEnableDetailedResults(bool enableDetailedResults); + bool isEnablePartialRelinearizationCheck() const; + void setEnablePartialRelinearizationCheck( + bool enablePartialRelinearizationCheck); +}; + +class ISAM2Clique { + // Constructors + ISAM2Clique(); + + // Standard Interface + Vector gradientContribution() const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +class ISAM2Result { + ISAM2Result(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() const; + double getErrorBefore() const; + double getErrorAfter() const; +}; + +class ISAM2 { + ISAM2(); + ISAM2(const gtsam::ISAM2Params& params); + ISAM2(const gtsam::ISAM2& other); + + bool equals(const gtsam::ISAM2& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printStats() const; + void saveGraph(string s) const; + + gtsam::ISAM2Result update(); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + const gtsam::KeyGroupMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys, + const gtsam::KeyList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys, + const gtsam::KeyList& extraReelimKeys, + bool force_relinearize); + + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, Vector, Matrix}> + VALUE calculateEstimate(size_t key) const; + gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; + gtsam::VectorValues getDelta() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + gtsam::VariableIndex getVariableIndex() const; + gtsam::ISAM2Params params() const; +}; + +#include +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& initialValues); + void reorder_relinearize(); + + // These might be expensive as instead of a reference the wrapper will make a + // copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; +}; + +//************************************************************************* +// Nonlinear factor types +//************************************************************************* +#include +template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, + const gtsam::noiseModel::Base* noiseModel); + T prior() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> +virtual class NonlinearEquality : gtsam::NoiseModelFactor { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); + + // enabling serialization functionality + void serialize() const; +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index ffc279872d..4e79e20ffd 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -260,30 +260,5 @@ Values localToWorld(const Values& local, const Pose2& base, } // namespace utilities -/** - * For Python __str__(). - * Redirect std cout to a string stream so we can return a string representation - * of an object when it prints to cout. - * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string - */ -struct RedirectCout { - /// constructor -- redirect stdout buffer to a stringstream buffer - RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} - - /// return the string - std::string str() const { - return ssBuffer_.str(); - } - - /// destructor -- redirect stdout buffer to its original buffer - ~RedirectCout() { - std::cout.rdbuf(coutBuffer_); - } - -private: - std::stringstream ssBuffer_; - std::streambuf* coutBuffer_; -}; - } diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i new file mode 100644 index 0000000000..370e1c3ea7 --- /dev/null +++ b/gtsam/sam/sam.i @@ -0,0 +1,96 @@ +//************************************************************************* +// sam +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include + +// ##### + +#include +template +virtual class RangeFactor : gtsam::NoiseModelFactor { + RangeFactor(size_t key1, size_t key2, double measured, + const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; +typedef gtsam::RangeFactor RangeFactorPose2; +typedef gtsam::RangeFactor RangeFactorPose3; +typedef gtsam::RangeFactor + RangeFactorCalibratedCameraPoint; +typedef gtsam::RangeFactor, gtsam::Point3> + RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor + RangeFactorCalibratedCamera; +typedef gtsam::RangeFactor, + gtsam::PinholeCamera> + RangeFactorSimpleCamera; + +#include +template +virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { + RangeFactorWithTransform(size_t key1, size_t key2, double measured, + const gtsam::noiseModel::Base* noiseModel, + const POSE& body_T_sensor); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransform2D; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransform3D; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransformPose2; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransformPose3; + +#include +template +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 + BearingFactor2D; +typedef gtsam::BearingFactor + BearingFactor3D; +typedef gtsam::BearingFactor + BearingFactorPose2; + +#include +template +virtual class BearingRangeFactor : gtsam::NoiseModelFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); + + gtsam::BearingRange measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingRangeFactor + BearingRangeFactor2D; +typedef gtsam::BearingRangeFactor + BearingRangeFactorPose2; + +} // namespace gtsam diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i new file mode 100644 index 0000000000..705892e60f --- /dev/null +++ b/gtsam/sfm/sfm.i @@ -0,0 +1,211 @@ +//************************************************************************* +// sfm +//************************************************************************* + +namespace gtsam { + +// ##### + +#include + +virtual class ShonanFactor3 : gtsam::NoiseModelFactor { + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p); + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p, + gtsam::noiseModel::Base* model); + Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); +}; + +#include +template +class BinaryMeasurement { + BinaryMeasurement(size_t key1, size_t key2, const T& measured, + const gtsam::noiseModel::Base* model); + size_t key1() const; + size_t key2() const; + T measured() const; + gtsam::noiseModel::Base* noiseModel() const; +}; + +typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; +typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; + +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + +#include + +// TODO(frank): copy/pasta below until we have integer template paremeters in +// wrap! + +class ShonanAveragingParameters2 { + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, + string method); + gtsam::LevenbergMarquardtParams getLMParams() const; + void setOptimalityThreshold(double value); + double getOptimalityThreshold() const; + void setAnchor(size_t index, const gtsam::Rot2& value); + pair getAnchor(); + void setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight() const; + void setGaugesWeight(double value); + double getGaugesWeight() const; + void setUseHuber(bool value); + bool getUseHuber() const; + void setCertifyOptimality(bool value); + bool getCertifyOptimality() const; +}; + +class ShonanAveragingParameters3 { + ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, + string method); + gtsam::LevenbergMarquardtParams getLMParams() const; + void setOptimalityThreshold(double value); + double getOptimalityThreshold() const; + void setAnchor(size_t index, const gtsam::Rot3& value); + pair getAnchor(); + void setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight() const; + void setGaugesWeight(double value); + double getGaugesWeight() const; + void setUseHuber(bool value); + bool getUseHuber() const; + void setCertifyOptimality(bool value); + bool getCertifyOptimality() const; +}; + +class ShonanAveraging2 { + ShonanAveraging2(string g2oFile); + ShonanAveraging2(string g2oFile, + const gtsam::ShonanAveragingParameters2& parameters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); + + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot2 measured(size_t i); + gtsam::KeyVector keys(size_t i); + + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, + double minEigenValue) const; + + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt( + size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; + + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, + size_t max_p) const; +}; + +class ShonanAveraging3 { + ShonanAveraging3(string g2oFile); + ShonanAveraging3(string g2oFile, + const gtsam::ShonanAveragingParameters3& parameters); + + // TODO(frank): deprecate once we land pybind wrapper + ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors); + ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors, + const gtsam::ShonanAveragingParameters3& parameters); + + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot3 measured(size_t i); + gtsam::KeyVector keys(size_t i); + + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, + double minEigenValue) const; + + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt( + size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; + + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, + size_t max_p) const; +}; + +#include + +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t at(const pair& keypair) const; +}; + +class MFAS { + MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::Unit3& projectionDirection); + + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; +}; + +#include +class TranslationRecovery { + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::LevenbergMarquardtParams& lmParams); + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3& + relativeTranslations); // default LevenbergMarquardtParams + gtsam::Values run(const double scale) const; + gtsam::Values run() const; // default scale = 1.0 +}; + +} // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i new file mode 100644 index 0000000000..1c04fd14c0 --- /dev/null +++ b/gtsam/slam/slam.i @@ -0,0 +1,338 @@ +//************************************************************************* +// slam +//************************************************************************* + +namespace gtsam { + +#include +#include +#include + +// ###### + +#include +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, + const gtsam::noiseModel::Base* noiseModel); + T measured() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +template +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 + GenericProjectionFactorCal3_S2; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3DS2; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Fisheye; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Unified; + +#include +template +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::Point3> + GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3DS2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Fisheye; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Unified; + +template +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 + +/// Linearization mode: what factor to linearize to +enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; + +/// How to manage degeneracy +enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; + +class SmartProjectionParams { + SmartProjectionParams(); + void setLinearizationMode(gtsam::LinearizationMode linMode); + void setDegeneracyMode(gtsam::DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; + +#include +template +virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::SmartProjectionParams& params); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor + SmartProjectionPose3Factor; + +#include +template +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 + GenericStereoFactor3D; + +#include +template +virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, + const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; + +#include +template +virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, + const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; + +#include +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, + const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); +}; + +#include + +class SfmTrack { + SfmTrack(); + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; + + double r; + double g; + double b; + + std::vector> measurements; + + size_t number_measurements() const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void add_measurement(size_t idx, const gtsam::Point2& m); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmTrack& expected, double tol) const; +}; + +class SfmData { + SfmData(); + size_t number_cameras() const; + size_t number_tracks() const; + gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack track(size_t idx) const; + void add_track(const gtsam::SfmTrack& t); + void add_camera(const gtsam::SfmCamera& cam); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmData& expected, double tol) const; +}; + +gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); +gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); +gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); + +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, + bool addNoise, bool smart); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, + bool addNoise); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair load2D_robust( + string filename, gtsam::noiseModel::Base* model, int maxIndex); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose2] +class BetweenFactorPose2s { + BetweenFactorPose2s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose2s parse2DFactors(string filename); + +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose3] +class BetweenFactorPose3s { + BetweenFactorPose3s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose3s parse3DFactors(string filename); + +pair load3D(string filename); + +pair readG2o(string filename); +pair readG2o(string filename, + bool is3D); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); + +#include +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; + +#include +template +virtual class KarcherMeanFactor : gtsam::NonlinearFactor { + KarcherMeanFactor(const gtsam::KeyVector& keys); +}; + +#include +gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, + size_t d); + +template +virtual class FrobeniusFactor : gtsam::NoiseModelFactor { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +template +virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, + gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +} // namespace gtsam diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i new file mode 100644 index 0000000000..4e7cca68ae --- /dev/null +++ b/gtsam/symbolic/symbolic.i @@ -0,0 +1,201 @@ +//************************************************************************* +// Symbolic +//************************************************************************* +namespace gtsam { + +#include +#include + +// ################### + +#include +virtual class SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicFactor(const gtsam::SymbolicFactor& f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, + size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); + + // From Factor + size_t size() const; + void print(string s = "SymbolicFactor", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicFactor& other, double tol) const; + gtsam::KeyVector keys(); +}; + +#include +virtual class SymbolicFactorGraph { + SymbolicFactorGraph(); + SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); + SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); + + // From FactorGraph + void push_back(gtsam::SymbolicFactor* factor); + void print(string s = "SymbolicFactorGraph", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; + size_t size() const; + bool exists(size_t idx) const; + + // Standard interface + gtsam::KeySet keys() const; + void push_back(const gtsam::SymbolicFactorGraph& graph); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); + void push_back(const gtsam::SymbolicBayesTree& bayesTree); + + // Advanced Interface + void push_factor(size_t key); + void push_factor(size_t key1, size_t key2); + void push_factor(size_t key1, size_t key2, size_t key3); + void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + + gtsam::SymbolicBayesNet* eliminateSequential(); + gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesTree* eliminateMultifrontal(); + gtsam::SymbolicBayesTree* eliminateMultifrontal( + const gtsam::Ordering& ordering); + pair + eliminatePartialSequential(const gtsam::Ordering& ordering); + pair + eliminatePartialSequential(const gtsam::KeyVector& keys); + pair + eliminatePartialMultifrontal(const gtsam::Ordering& ordering); + pair + eliminatePartialMultifrontal(const gtsam::KeyVector& keys); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::Ordering& ordering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::KeyVector& key_vector); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); +}; + +#include +virtual class SymbolicConditional : gtsam::SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional& other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, + size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, + size_t nrFrontals); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicConditional& other, double tol) const; + + // Standard interface + size_t nrFrontals() const; + size_t nrParents() const; +}; + +#include +class SymbolicBayesNet { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); + // Testable + void print(string s = "SymbolicBayesNet", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; + + // Standard interface + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional* at(size_t idx) const; + gtsam::SymbolicConditional* front() const; + gtsam::SymbolicConditional* back() const; + void push_back(gtsam::SymbolicConditional* conditional); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); +}; + +#include +class SymbolicBayesTree { + // Constructors + SymbolicBayesTree(); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); + bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; + + // Standard Interface + // size_t findParentClique(const gtsam::IndexVector& parents) const; + size_t size(); + void saveGraph(string s) const; + void clear(); + void deleteCachedShortcuts(); + size_t numCachedSeparatorMarginals() const; + + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +class SymbolicBayesTreeClique { + SymbolicBayesTreeClique(); + // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional); + + bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + size_t numCachedSeparatorMarginals() const; + // gtsam::sharedConditional* conditional() const; + bool isRoot() const; + size_t treeSize() const; + gtsam::SymbolicBayesTreeClique* parent() const; + + // // TODO: need wrapped versions graphs, BayesNet + // BayesNet shortcut(derived_ptr root, Eliminate function) + // const; FactorGraph marginal(derived_ptr root, Eliminate + // function) const; FactorGraph joint(derived_ptr C2, derived_ptr + // root, Eliminate function) const; + // + void deleteCachedShortcuts(); +}; + +#include +class VariableIndex { + // Standard Constructors and Named Constructors + VariableIndex(); + // TODO: Templetize constructor when wrap supports it + // template + // VariableIndex(const T& factorGraph, size_t nVariables); + // VariableIndex(const T& factorGraph); + VariableIndex(const gtsam::SymbolicFactorGraph& sfg); + VariableIndex(const gtsam::GaussianFactorGraph& gfg); + VariableIndex(const gtsam::NonlinearFactorGraph& fg); + VariableIndex(const gtsam::VariableIndex& other); + + // Testable + bool equals(const gtsam::VariableIndex& other, double tol) const; + void print(string s = "VariableIndex: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + + // Standard interface + size_t size() const; + size_t nFactors() const; + size_t nEntries() const; +}; + +} // namespace gtsam diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 528732f4bd..7b8347da5c 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -50,8 +50,22 @@ set(ignore gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) +set(interface_headers + ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i + ${PROJECT_SOURCE_DIR}/gtsam/base/base.i + ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i + ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i + ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i + ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i + ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i + ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i + ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i +) + + pybind_wrap(gtsam_py # target - ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header + "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp "gtsam" # module_name "gtsam" # top_namespace diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index b800f7c35b..93e7ffbafd 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -18,7 +18,7 @@ #include #include "gtsam/config.h" #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. // These are the included headers listed in `gtsam.i` {includes} @@ -32,20 +32,24 @@ // Preamble for STL classes // TODO(fan): make this automatic -#include "python/gtsam/preamble.h" +#include "python/gtsam/preamble/{module_name}.h" using namespace std; namespace py = pybind11; -PYBIND11_MODULE({module_name}, m_) {{ +{submodules} + +{module_def} {{ m_.doc() = "pybind11 wrapper of {module_name}"; +{submodules_init} + {wrapped_namespace} // Specializations for STL classes // TODO(fan): make this automatic -#include "python/gtsam/specializations.h" +#include "python/gtsam/specializations/{module_name}.h" }} diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h deleted file mode 100644 index 7294a6ac8e..0000000000 --- a/python/gtsam/preamble.h +++ /dev/null @@ -1,30 +0,0 @@ -/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html - * These are required to save one copy operation on Python calls. - * - * NOTES - * ================= - * - * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding, - * such that the raw objects can be accessed in Python. Without this they will be automatically - * converted to a Python object, and all mutations on Python side will not be reflected on C++. - */ -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif -PYBIND11_MAKE_OPAQUE(std::vector >); -PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); -PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector - -// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this. -namespace std { - using gtsam::operator<<; -} diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h new file mode 100644 index 0000000000..626b47ae4a --- /dev/null +++ b/python/gtsam/preamble/base.h @@ -0,0 +1,16 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE(std::vector); + +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h new file mode 100644 index 0000000000..498c7dc58f --- /dev/null +++ b/python/gtsam/preamble/geometry.h @@ -0,0 +1,21 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE( + std::vector>); +PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); +PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE( + gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/preamble/gtsam.h b/python/gtsam/preamble/gtsam.h new file mode 100644 index 0000000000..ec39c410a8 --- /dev/null +++ b/python/gtsam/preamble/gtsam.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ +#ifdef GTSAM_ALLOCATOR_TBB +PYBIND11_MAKE_OPAQUE(std::vector>); +#else +PYBIND11_MAKE_OPAQUE(std::vector); +#endif diff --git a/python/gtsam/preamble/linear.h b/python/gtsam/preamble/linear.h new file mode 100644 index 0000000000..a34e730580 --- /dev/null +++ b/python/gtsam/preamble/linear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/navigation.h b/python/gtsam/preamble/navigation.h new file mode 100644 index 0000000000..b647ef029a --- /dev/null +++ b/python/gtsam/preamble/navigation.h @@ -0,0 +1,18 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. +// We should find a way to NOT do this. +namespace std { +using gtsam::operator<<; +} diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h new file mode 100644 index 0000000000..a34e730580 --- /dev/null +++ b/python/gtsam/preamble/nonlinear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/sam.h b/python/gtsam/preamble/sam.h new file mode 100644 index 0000000000..a34e730580 --- /dev/null +++ b/python/gtsam/preamble/sam.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h new file mode 100644 index 0000000000..a34e730580 --- /dev/null +++ b/python/gtsam/preamble/sfm.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h new file mode 100644 index 0000000000..34dbb4b7ab --- /dev/null +++ b/python/gtsam/preamble/slam.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE( + std::vector > >); +PYBIND11_MAKE_OPAQUE( + std::vector > >); diff --git a/python/gtsam/preamble/symbolic.h b/python/gtsam/preamble/symbolic.h new file mode 100644 index 0000000000..a34e730580 --- /dev/null +++ b/python/gtsam/preamble/symbolic.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h deleted file mode 100644 index 2e8612fec6..0000000000 --- a/python/gtsam/specializations.h +++ /dev/null @@ -1,37 +0,0 @@ -/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html - * These are required to save one copy operation on Python calls. - * - * NOTES - * ================= - * - * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding, - * such that the raw objects can be accessed in Python. Without this they will be automatically - * converted to a Python object, and all mutations on Python side will not be reflected on C++. - * - * `py::bind_vector` and similar machinery gives the std container a Python-like interface, but - * without the `` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this - * allows the types to be modified with Python, and saves one copy operation. - */ -#ifdef GTSAM_ALLOCATOR_TBB -py::bind_vector > >(m_, "KeyVector"); -py::implicitly_convertible > >(); -#else -py::bind_vector >(m_, "KeyVector"); -py::implicitly_convertible >(); -#endif - -py::bind_vector > >(m_, "Point2Vector"); -py::bind_vector >(m_, "Point3Pairs"); -py::bind_vector >(m_, "Pose3Pairs"); -py::bind_vector >(m_, "Pose3Vector"); -py::bind_vector > > >(m_, "BetweenFactorPose3s"); -py::bind_vector > > >(m_, "BetweenFactorPose2s"); -py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); -py::bind_map(m_, "IndexPairSetMap"); -py::bind_vector(m_, "IndexPairVector"); -py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector > >(m_, "CameraSetCal3_S2"); -py::bind_vector > >(m_, "CameraSetCal3Bundler"); -py::bind_vector > >(m_, "CameraSetCal3Unified"); -py::bind_vector > >(m_, "CameraSetCal3Fisheye"); -py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/base.h b/python/gtsam/specializations/base.h new file mode 100644 index 0000000000..9eefdeca82 --- /dev/null +++ b/python/gtsam/specializations/base.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_map(m_, "IndexPairSetMap"); +py::bind_vector(m_, "IndexPairVector"); + +py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h new file mode 100644 index 0000000000..e11d3cc4fe --- /dev/null +++ b/python/gtsam/specializations/geometry.h @@ -0,0 +1,27 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector< + std::vector>>( + m_, "Point2Vector"); +py::bind_vector>(m_, "Point3Pairs"); +py::bind_vector>(m_, "Pose3Pairs"); +py::bind_vector>(m_, "Pose3Vector"); +py::bind_vector>>( + m_, "CameraSetCal3_S2"); +py::bind_vector>>( + m_, "CameraSetCal3Bundler"); +py::bind_vector>>( + m_, "CameraSetCal3Unified"); +py::bind_vector>>( + m_, "CameraSetCal3Fisheye"); diff --git a/python/gtsam/specializations/gtsam.h b/python/gtsam/specializations/gtsam.h new file mode 100644 index 0000000000..490d719023 --- /dev/null +++ b/python/gtsam/specializations/gtsam.h @@ -0,0 +1,20 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +#ifdef GTSAM_ALLOCATOR_TBB +py::bind_vector > >(m_, "KeyVector"); +py::implicitly_convertible > >(); +#else +py::bind_vector >(m_, "KeyVector"); +py::implicitly_convertible >(); +#endif diff --git a/python/gtsam/specializations/linear.h b/python/gtsam/specializations/linear.h new file mode 100644 index 0000000000..d46ccdc668 --- /dev/null +++ b/python/gtsam/specializations/linear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/navigation.h b/python/gtsam/specializations/navigation.h new file mode 100644 index 0000000000..d46ccdc668 --- /dev/null +++ b/python/gtsam/specializations/navigation.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h new file mode 100644 index 0000000000..d46ccdc668 --- /dev/null +++ b/python/gtsam/specializations/nonlinear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/sam.h b/python/gtsam/specializations/sam.h new file mode 100644 index 0000000000..d46ccdc668 --- /dev/null +++ b/python/gtsam/specializations/sam.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h new file mode 100644 index 0000000000..6de15217fb --- /dev/null +++ b/python/gtsam/specializations/sfm.h @@ -0,0 +1,16 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector > >( + m_, "BinaryMeasurementsUnit3"); +py::bind_map(m_, "KeyPairDoubleMap"); diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h new file mode 100644 index 0000000000..198485a72e --- /dev/null +++ b/python/gtsam/specializations/slam.h @@ -0,0 +1,19 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector< + std::vector > > >( + m_, "BetweenFactorPose3s"); +py::bind_vector< + std::vector > > >( + m_, "BetweenFactorPose2s"); diff --git a/python/gtsam/specializations/symbolic.h b/python/gtsam/specializations/symbolic.h new file mode 100644 index 0000000000..d46ccdc668 --- /dev/null +++ b/python/gtsam/specializations/symbolic.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index c1033ba43c..aa7ac6bdb8 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -16,7 +16,7 @@ #include #include #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. // These are the included headers listed in `gtsam_unstable.i` {includes} diff --git a/wrap/.github/workflows/linux-ci.yml b/wrap/.github/workflows/linux-ci.yml index 0ca9ba8f5b..34623385ed 100644 --- a/wrap/.github/workflows/linux-ci.yml +++ b/wrap/.github/workflows/linux-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.5, 3.6, 3.7, 3.8, 3.9] + python-version: [3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/wrap/.github/workflows/macos-ci.yml b/wrap/.github/workflows/macos-ci.yml index b0ccb3fbe9..3910d28d8a 100644 --- a/wrap/.github/workflows/macos-ci.yml +++ b/wrap/.github/workflows/macos-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.5, 3.6, 3.7, 3.8, 3.9] + python-version: [3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/wrap/.gitignore b/wrap/.gitignore index 8e2bafa7a0..9f79deafab 100644 --- a/wrap/.gitignore +++ b/wrap/.gitignore @@ -8,4 +8,4 @@ __pycache__/ # Files related to code coverage stats **/.coverage -gtwrap/matlab_wrapper.tpl +gtwrap/matlab_wrapper/matlab_wrapper.tpl diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 9e03da0607..2a11a760d4 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -58,7 +58,7 @@ if(NOT DEFINED GTWRAP_INCLUDE_NAME) endif() configure_file(${PROJECT_SOURCE_DIR}/templates/matlab_wrapper.tpl.in - ${PROJECT_SOURCE_DIR}/gtwrap/matlab_wrapper.tpl) + ${PROJECT_SOURCE_DIR}/gtwrap/matlab_wrapper/matlab_wrapper.tpl) # Install the gtwrap python package as a directory so it can be found by CMake # for wrapping. diff --git a/wrap/DOCS.md b/wrap/DOCS.md index 8537ddd276..c8285baeff 100644 --- a/wrap/DOCS.md +++ b/wrap/DOCS.md @@ -192,12 +192,14 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the - **DO NOT** re-define an overriden function already declared in the external (forward-declared) base class. This will cause an ambiguity problem in the Pybind header file. +- Splitting wrapper over multiple files + - The Pybind11 wrapper supports splitting the wrapping code over multiple files. + - To be able to use classes from another module, simply import the C++ header file in that wrapper file. + - Unfortunately, this means that aliases can no longer be used. + - Similarly, there can be multiple `preamble.h` and `specializations.h` files. Each of these should match the module file name. ### TODO -- Default values for arguments. - - WORKAROUND: make multiple versions of the same function for different configurations of default arguments. - Handle `gtsam::Rot3M` conversions to quaternions. - Parse return of const ref arguments. - Parse `std::string` variants and convert directly to special string. -- Add enum support. - Add generalized serialization support via `boost.serialization` with hooks to MATLAB save/load. diff --git a/wrap/README.md b/wrap/README.md index 442fc2f934..a04a2ef2d0 100644 --- a/wrap/README.md +++ b/wrap/README.md @@ -29,8 +29,10 @@ Using `wrap` in your project is straightforward from here. In your `CMakeLists.t ```cmake find_package(gtwrap) +set(interface_files ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h) + pybind_wrap(${PROJECT_NAME}_py # target - ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h # interface header file + "${interface_files}" # list of interface header files "${PROJECT_NAME}.cpp" # the generated cpp "${PROJECT_NAME}" # module_name "${PROJECT_MODULE_NAME}" # top namespace in the cpp file e.g. gtsam diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake index 331dfff8c4..f341c2f989 100644 --- a/wrap/cmake/PybindWrap.cmake +++ b/wrap/cmake/PybindWrap.cmake @@ -13,15 +13,14 @@ gtwrap_get_python_version(${WRAP_PYTHON_VERSION}) message(STATUS "Setting Python version for wrapper") set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) -# User-friendly Pybind11 wrapping and installing function. -# Builds a Pybind11 module from the provided interface_header. -# For example, for the interface header gtsam.h, this will -# build the wrap module 'gtsam_py.cc'. +# User-friendly Pybind11 wrapping and installing function. Builds a Pybind11 +# module from the provided interface_headers. For example, for the interface +# header gtsam.h, this will build the wrap module 'gtsam_py.cc'. # # Arguments: # ~~~ # target: The Make target -# interface_header: The relative path to the wrapper interface definition file. +# interface_headers: List of paths to the wrapper interface definition files. The top level interface file should be first. # generated_cpp: The name of the cpp file which is generated from the tpl file. # module_name: The name of the Python module to use. # top_namespace: The C++ namespace under which the code to be wrapped exists. @@ -31,16 +30,17 @@ set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) # libs: Libraries to link with. # dependencies: Dependencies which need to be built before the wrapper. # use_boost (optional): Flag indicating whether to include Boost. -function(pybind_wrap - target - interface_header - generated_cpp - module_name - top_namespace - ignore_classes - module_template - libs - dependencies) +function( + pybind_wrap + target + interface_headers + generated_cpp + module_name + top_namespace + ignore_classes + module_template + libs + dependencies) set(ExtraMacroArgs ${ARGN}) list(GET ExtraMacroArgs 0 USE_BOOST) if(USE_BOOST) @@ -49,57 +49,62 @@ function(pybind_wrap set(_WRAP_BOOST_ARG "") endif(USE_BOOST) - if (UNIX) + if(UNIX) set(GTWRAP_PATH_SEPARATOR ":") else() set(GTWRAP_PATH_SEPARATOR ";") endif() - add_custom_command(OUTPUT ${generated_cpp} - COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} - ${PYBIND_WRAP_SCRIPT} - --src - ${interface_header} - --out - ${generated_cpp} - --module_name - ${module_name} - --top_module_namespaces - "${top_namespace}" - --ignore - ${ignore_classes} - --template - ${module_template} - ${_WRAP_BOOST_ARG} - DEPENDS ${interface_header} ${module_template} - VERBATIM) - add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${generated_cpp}) + # Convert .i file names to .cpp file names. + foreach(filepath ${interface_headers}) + get_filename_component(interface ${filepath} NAME) + string(REPLACE ".i" ".cpp" cpp_file ${interface}) + list(APPEND cpp_files ${cpp_file}) + endforeach() + + add_custom_command( + OUTPUT ${cpp_files} + COMMAND + ${CMAKE_COMMAND} -E env + "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} ${PYBIND_WRAP_SCRIPT} --src "${interface_headers}" + --out "${generated_cpp}" --module_name ${module_name} + --top_module_namespaces "${top_namespace}" --ignore ${ignore_classes} + --template ${module_template} ${_WRAP_BOOST_ARG} + DEPENDS "${interface_headers}" ${module_template} + VERBATIM) + + add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${cpp_files}) # Late dependency injection, to make sure this gets called whenever the # interface header or the wrap library are updated. # ~~~ # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes # ~~~ - add_custom_command(OUTPUT ${generated_cpp} - DEPENDS ${interface_header} - # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py - # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py - # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py - APPEND) + add_custom_command( + OUTPUT ${cpp_files} + DEPENDS ${interface_headers} + # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py + # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py + # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py + APPEND) - pybind11_add_module(${target} ${generated_cpp}) + pybind11_add_module(${target} "${cpp_files}") if(APPLE) - # `type_info` objects will become "weak private external" if the templated class is initialized implicitly even if we explicitly - # export them with `WRAP_EXPORT`. If that happens, the `type_info` for the same templated class will diverge between shared - # libraries, causing `dynamic_cast` to fail. This is mitigated by telling Clang to mimic the MSVC behavior. - # See https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2 + # `type_info` objects will become "weak private external" if the templated + # class is initialized implicitly even if we explicitly export them with + # `WRAP_EXPORT`. If that happens, the `type_info` for the same templated + # class will diverge between shared libraries, causing `dynamic_cast` to + # fail. This is mitigated by telling Clang to mimic the MSVC behavior. See + # https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2 # https://github.com/CppMicroServices/CppMicroServices/pull/82/files # https://www.russellmcc.com/posts/2013-08-03-rtti.html target_compile_options(${target} PRIVATE "-fvisibility-ms-compat") endif() + add_dependencies(${target} pybind_wrap_${module_name}) + if(NOT "${libs}" STREQUAL "") target_link_libraries(${target} PRIVATE "${libs}") endif() @@ -121,10 +126,7 @@ endfunction() # dest_directory: The destination directory to install to. # patterns: list of file patterns to install # ~~~ -function(install_python_scripts - source_directory - dest_directory - patterns) +function(install_python_scripts source_directory dest_directory patterns) set(patterns_args "") set(exclude_patterns "") @@ -144,17 +146,19 @@ function(install_python_scripts # there is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) - install(DIRECTORY "${source_directory}" - DESTINATION "${location}/${name}${build_type_tag}" - CONFIGURATIONS "${build_type}" - FILES_MATCHING ${patterns_args} - PATTERN "${exclude_patterns}" EXCLUDE) + install( + DIRECTORY "${source_directory}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" - DESTINATION "${dest_directory}" - FILES_MATCHING ${patterns_args} - PATTERN "${exclude_patterns}" EXCLUDE) + install( + DIRECTORY "${source_directory}" + DESTINATION "${dest_directory}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() @@ -172,13 +176,14 @@ function(install_python_files source_files dest_directory) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) set(build_type_tag "") - # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if - # there is one + # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if there + # is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) - install(FILES "${source_files}" - DESTINATION "${location}/${name}${build_type_tag}" - CONFIGURATIONS "${build_type}") + install( + FILES "${source_files}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}") endforeach() else() install(FILES "${source_files}" DESTINATION "${dest_directory}") @@ -194,18 +199,19 @@ function(create_symlinks source_folder dest_folder) return() endif() - file(GLOB files - LIST_DIRECTORIES true - RELATIVE "${source_folder}" - "${source_folder}/*") + file( + GLOB files + LIST_DIRECTORIES true + RELATIVE "${source_folder}" + "${source_folder}/*") foreach(path_file ${files}) get_filename_component(folder ${path_file} PATH) get_filename_component(ext ${path_file} EXT) set(ignored_ext ".tpl" ".h") - list (FIND ignored_ext "${ext}" _index) - if (${_index} GREATER -1) + list(FIND ignored_ext "${ext}" _index) + if(${_index} GREATER -1) continue() - endif () + endif() # Create REAL folder file(MAKE_DIRECTORY "${dest_folder}") @@ -224,9 +230,10 @@ function(create_symlinks source_folder dest_folder) endif() # cmake-format: on - execute_process(COMMAND ${command} - RESULT_VARIABLE result - ERROR_VARIABLE output) + execute_process( + COMMAND ${command} + RESULT_VARIABLE result + ERROR_VARIABLE output) if(NOT ${result} EQUAL 0) message( diff --git a/wrap/gtwrap/interface_parser/__init__.py b/wrap/gtwrap/interface_parser/__init__.py index 0f87eaaa9d..3be52d7d9f 100644 --- a/wrap/gtwrap/interface_parser/__init__.py +++ b/wrap/gtwrap/interface_parser/__init__.py @@ -12,7 +12,7 @@ import sys -import pyparsing +import pyparsing # type: ignore from .classes import * from .declaration import * diff --git a/wrap/gtwrap/interface_parser/classes.py b/wrap/gtwrap/interface_parser/classes.py index ea7a3b3c38..3e6a0fc3c7 100644 --- a/wrap/gtwrap/interface_parser/classes.py +++ b/wrap/gtwrap/interface_parser/classes.py @@ -12,7 +12,7 @@ from typing import Iterable, List, Union -from pyparsing import Literal, Optional, ZeroOrMore +from pyparsing import Literal, Optional, ZeroOrMore # type: ignore from .enum import Enum from .function import ArgumentList, ReturnType @@ -233,7 +233,7 @@ def __init__(self, self.static_methods = [] self.properties = [] self.operators = [] - self.enums = [] + self.enums: List[Enum] = [] for m in members: if isinstance(m, Constructor): self.ctors.append(m) @@ -274,7 +274,7 @@ def __init__(self, def __init__( self, - template: Template, + template: Union[Template, None], is_virtual: str, name: str, parent_class: list, @@ -292,16 +292,16 @@ def __init__( if parent_class: # If it is in an iterable, extract the parent class. if isinstance(parent_class, Iterable): - parent_class = parent_class[0] + parent_class = parent_class[0] # type: ignore # If the base class is a TemplatedType, # we want the instantiated Typename if isinstance(parent_class, TemplatedType): - parent_class = parent_class.typename + parent_class = parent_class.typename # type: ignore self.parent_class = parent_class else: - self.parent_class = '' + self.parent_class = '' # type: ignore self.ctors = ctors self.methods = methods diff --git a/wrap/gtwrap/interface_parser/declaration.py b/wrap/gtwrap/interface_parser/declaration.py index 292d6aeaa6..f47ee6e057 100644 --- a/wrap/gtwrap/interface_parser/declaration.py +++ b/wrap/gtwrap/interface_parser/declaration.py @@ -10,7 +10,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import CharsNotIn, Optional +from pyparsing import CharsNotIn, Optional # type: ignore from .tokens import (CLASS, COLON, INCLUDE, LOPBRACK, ROPBRACK, SEMI_COLON, VIRTUAL) diff --git a/wrap/gtwrap/interface_parser/enum.py b/wrap/gtwrap/interface_parser/enum.py index fca7080ef2..265e1ad612 100644 --- a/wrap/gtwrap/interface_parser/enum.py +++ b/wrap/gtwrap/interface_parser/enum.py @@ -10,7 +10,7 @@ Author: Varun Agrawal """ -from pyparsing import delimitedList +from pyparsing import delimitedList # type: ignore from .tokens import ENUM, IDENT, LBRACE, RBRACE, SEMI_COLON from .type import Typename diff --git a/wrap/gtwrap/interface_parser/function.py b/wrap/gtwrap/interface_parser/function.py index 3b9a5d4ada..995aba10e1 100644 --- a/wrap/gtwrap/interface_parser/function.py +++ b/wrap/gtwrap/interface_parser/function.py @@ -10,9 +10,9 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from typing import Iterable, List, Union +from typing import Any, Iterable, List, Union -from pyparsing import Optional, ParseResults, delimitedList +from pyparsing import Optional, ParseResults, delimitedList # type: ignore from .template import Template from .tokens import (COMMA, DEFAULT_ARG, EQUAL, IDENT, LOPBRACK, LPAREN, PAIR, @@ -42,12 +42,12 @@ def __init__(self, name: str, default: ParseResults = None): if isinstance(ctype, Iterable): - self.ctype = ctype[0] + self.ctype = ctype[0] # type: ignore else: self.ctype = ctype self.name = name self.default = default - self.parent = None # type: Union[ArgumentList, None] + self.parent: Union[ArgumentList, None] = None def __repr__(self) -> str: return self.to_cpp() @@ -70,7 +70,7 @@ def __init__(self, args_list: List[Argument]): arg.parent = self # The parent object which contains the argument list # E.g. Method, StaticMethod, Template, Constructor, GlobalFunction - self.parent = None + self.parent: Any = None @staticmethod def from_parse_result(parse_result: ParseResults): @@ -123,7 +123,7 @@ def __init__(self, type1: Union[Type, TemplatedType], type2: Type): self.type2 = type2 # The parent object which contains the return type # E.g. Method, StaticMethod, Template, Constructor, GlobalFunction - self.parent = None + self.parent: Any = None def is_void(self) -> bool: """ diff --git a/wrap/gtwrap/interface_parser/module.py b/wrap/gtwrap/interface_parser/module.py index 6412098b8a..7912c40d5b 100644 --- a/wrap/gtwrap/interface_parser/module.py +++ b/wrap/gtwrap/interface_parser/module.py @@ -12,7 +12,8 @@ # pylint: disable=unnecessary-lambda, unused-import, expression-not-assigned, no-else-return, protected-access, too-few-public-methods, too-many-arguments -from pyparsing import ParseResults, ZeroOrMore, cppStyleComment, stringEnd +from pyparsing import (ParseResults, ZeroOrMore, # type: ignore + cppStyleComment, stringEnd) from .classes import Class from .declaration import ForwardDeclaration, Include diff --git a/wrap/gtwrap/interface_parser/namespace.py b/wrap/gtwrap/interface_parser/namespace.py index 575d982371..9c135ffe8c 100644 --- a/wrap/gtwrap/interface_parser/namespace.py +++ b/wrap/gtwrap/interface_parser/namespace.py @@ -14,7 +14,7 @@ from typing import List, Union -from pyparsing import Forward, ParseResults, ZeroOrMore +from pyparsing import Forward, ParseResults, ZeroOrMore # type: ignore from .classes import Class, collect_namespaces from .declaration import ForwardDeclaration, Include @@ -93,7 +93,7 @@ def from_parse_result(t: ParseResults): return Namespace(t.name, content) def find_class_or_function( - self, typename: Typename) -> Union[Class, GlobalFunction]: + self, typename: Typename) -> Union[Class, GlobalFunction, ForwardDeclaration]: """ Find the Class or GlobalFunction object given its typename. We have to traverse the tree of namespaces. @@ -115,7 +115,7 @@ def find_class_or_function( return res[0] def top_level(self) -> "Namespace": - """Return the top leve namespace.""" + """Return the top level namespace.""" if self.name == '' or self.parent == '': return self else: diff --git a/wrap/gtwrap/interface_parser/template.py b/wrap/gtwrap/interface_parser/template.py index dc9d0ce44f..fd9de830ae 100644 --- a/wrap/gtwrap/interface_parser/template.py +++ b/wrap/gtwrap/interface_parser/template.py @@ -12,11 +12,11 @@ from typing import List -from pyparsing import Optional, ParseResults, delimitedList +from pyparsing import Optional, ParseResults, delimitedList # type: ignore from .tokens import (EQUAL, IDENT, LBRACE, LOPBRACK, RBRACE, ROPBRACK, SEMI_COLON, TEMPLATE, TYPEDEF) -from .type import Typename, TemplatedType +from .type import TemplatedType, Typename class Template: diff --git a/wrap/gtwrap/interface_parser/tokens.py b/wrap/gtwrap/interface_parser/tokens.py index 4eba95900a..0f8d38d868 100644 --- a/wrap/gtwrap/interface_parser/tokens.py +++ b/wrap/gtwrap/interface_parser/tokens.py @@ -10,9 +10,9 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress, - Word, alphanums, alphas, nestedExpr, nums, - originalTextFor, printables) +from pyparsing import (Keyword, Literal, OneOrMore, Or, # type: ignore + QuotedString, Suppress, Word, alphanums, alphas, + nestedExpr, nums, originalTextFor, printables) # rule for identifiers (e.g. variable names) IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums) diff --git a/wrap/gtwrap/interface_parser/type.py b/wrap/gtwrap/interface_parser/type.py index b9f2bd8f74..0b9be65017 100644 --- a/wrap/gtwrap/interface_parser/type.py +++ b/wrap/gtwrap/interface_parser/type.py @@ -14,7 +14,8 @@ from typing import Iterable, List, Union -from pyparsing import Forward, Optional, Or, ParseResults, delimitedList +from pyparsing import (Forward, Optional, Or, ParseResults, # type: ignore + delimitedList) from .tokens import (BASIS_TYPES, CONST, IDENT, LOPBRACK, RAW_POINTER, REF, ROPBRACK, SHARED_POINTER) @@ -48,7 +49,7 @@ class Typename: def __init__(self, t: ParseResults, - instantiations: Union[tuple, list, str, ParseResults] = ()): + instantiations: Iterable[ParseResults] = ()): self.name = t[-1] # the name is the last element in this list self.namespaces = t[:-1] diff --git a/wrap/gtwrap/interface_parser/variable.py b/wrap/gtwrap/interface_parser/variable.py index fcb02666f7..3779cf74fa 100644 --- a/wrap/gtwrap/interface_parser/variable.py +++ b/wrap/gtwrap/interface_parser/variable.py @@ -10,7 +10,9 @@ Author: Varun Agrawal, Gerry Chen """ -from pyparsing import Optional, ParseResults +from typing import List + +from pyparsing import Optional, ParseResults # type: ignore from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON from .type import TemplatedType, Type @@ -40,7 +42,7 @@ class Hello { t.default[0] if isinstance(t.default, ParseResults) else None)) def __init__(self, - ctype: Type, + ctype: List[Type], name: str, default: ParseResults = None, parent=''): diff --git a/wrap/gtwrap/matlab_wrapper/__init__.py b/wrap/gtwrap/matlab_wrapper/__init__.py new file mode 100644 index 0000000000..f10338c1c7 --- /dev/null +++ b/wrap/gtwrap/matlab_wrapper/__init__.py @@ -0,0 +1,3 @@ +"""Package to wrap C++ code to Matlab via MEX.""" + +from .wrapper import MatlabWrapper diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py new file mode 100644 index 0000000000..061cea2833 --- /dev/null +++ b/wrap/gtwrap/matlab_wrapper/mixins.py @@ -0,0 +1,222 @@ +"""Mixins for reducing the amount of boilerplate in the main wrapper class.""" + +import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator + + +class CheckMixin: + """Mixin to provide various checks.""" + # Data types that are primitive types + not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] + # Ignore the namespace for these datatypes + ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3'] + # Methods that should be ignored + ignore_methods = ['pickle'] + # Methods that should not be wrapped directly + whitelist = ['serializable', 'serialize'] + # Datatypes that do not need to be checked in methods + not_check_type: list = [] + + def _has_serialization(self, cls): + for m in cls.methods: + if m.name in self.whitelist: + return True + return False + + def is_shared_ptr(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + shared pointer in the wrapper. + """ + return arg_type.is_shared_ptr or ( + arg_type.typename.name not in self.not_ptr_type + and arg_type.typename.name not in self.ignore_namespace + and arg_type.typename.name != 'string') + + def is_ptr(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + raw pointer in the wrapper. + """ + return arg_type.is_ptr or ( + arg_type.typename.name not in self.not_ptr_type + and arg_type.typename.name not in self.ignore_namespace + and arg_type.typename.name != 'string') + + def is_ref(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + reference in the wrapper. + """ + return arg_type.typename.name not in self.ignore_namespace and \ + arg_type.typename.name not in self.not_ptr_type and \ + arg_type.is_ref + + +class FormatMixin: + """Mixin to provide formatting utilities.""" + def _clean_class_name(self, instantiated_class): + """Reformatted the C++ class name to fit Matlab defined naming + standards + """ + if len(instantiated_class.ctors) != 0: + return instantiated_class.ctors[0].name + + return instantiated_class.name + + def _format_type_name(self, + type_name, + separator='::', + include_namespace=True, + constructor=False, + method=False): + """ + Args: + type_name: an interface_parser.Typename to reformat + separator: the statement to add between namespaces and typename + include_namespace: whether to include namespaces when reformatting + constructor: if the typename will be in a constructor + method: if the typename will be in a method + + Raises: + constructor and method cannot both be true + """ + if constructor and method: + raise ValueError( + 'Constructor and method parameters cannot both be True') + + formatted_type_name = '' + name = type_name.name + + if include_namespace: + for namespace in type_name.namespaces: + if name not in self.ignore_namespace and namespace != '': + formatted_type_name += namespace + separator + + if constructor: + formatted_type_name += self.data_type.get(name) or name + elif method: + formatted_type_name += self.data_type_param.get(name) or name + else: + formatted_type_name += name + + if separator == "::": # C++ + templates = [] + for idx in range(len(type_name.instantiations)): + template = '{}'.format( + self._format_type_name(type_name.instantiations[idx], + include_namespace=include_namespace, + constructor=constructor, + method=method)) + templates.append(template) + + if len(templates) > 0: # If there are no templates + formatted_type_name += '<{}>'.format(','.join(templates)) + + else: + for idx in range(len(type_name.instantiations)): + formatted_type_name += '{}'.format( + self._format_type_name(type_name.instantiations[idx], + separator=separator, + include_namespace=False, + constructor=constructor, + method=method)) + + return formatted_type_name + + def _format_return_type(self, + return_type, + include_namespace=False, + separator="::"): + """Format return_type. + + Args: + return_type: an interface_parser.ReturnType to reformat + include_namespace: whether to include namespaces when reformatting + """ + return_wrap = '' + + if self._return_count(return_type) == 1: + return_wrap = self._format_type_name( + return_type.type1.typename, + separator=separator, + include_namespace=include_namespace) + else: + return_wrap = 'pair< {type1}, {type2} >'.format( + type1=self._format_type_name( + return_type.type1.typename, + separator=separator, + include_namespace=include_namespace), + type2=self._format_type_name( + return_type.type2.typename, + separator=separator, + include_namespace=include_namespace)) + + return return_wrap + + def _format_class_name(self, instantiated_class, separator=''): + """Format a template_instantiator.InstantiatedClass name.""" + if instantiated_class.parent == '': + parent_full_ns = [''] + else: + parent_full_ns = instantiated_class.parent.full_namespaces() + # class_name = instantiated_class.parent.name + # + # if class_name != '': + # class_name += separator + # + # class_name += instantiated_class.name + parentname = "".join([separator + x + for x in parent_full_ns]) + separator + + class_name = parentname[2 * len(separator):] + + class_name += instantiated_class.name + + return class_name + + def _format_static_method(self, static_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(static_method, parser.StaticMethod): + method += "".join([separator + x for x in static_method.parent.namespaces()]) + \ + separator + static_method.parent.name + separator + + return method[2 * len(separator):] + + def _format_instance_method(self, instance_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(instance_method, instantiator.InstantiatedMethod): + method_list = [ + separator + x + for x in instance_method.parent.parent.full_namespaces() + ] + method += "".join(method_list) + separator + + method += instance_method.parent.name + separator + method += instance_method.original.name + method += "<" + instance_method.instantiations.to_cpp() + ">" + + return method[2 * len(separator):] + + def _format_global_method(self, static_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(static_method, parser.GlobalFunction): + method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \ + separator + + return method[2 * len(separator):] diff --git a/wrap/gtwrap/matlab_wrapper/templates.py b/wrap/gtwrap/matlab_wrapper/templates.py new file mode 100644 index 0000000000..7aaf8f487b --- /dev/null +++ b/wrap/gtwrap/matlab_wrapper/templates.py @@ -0,0 +1,166 @@ +import textwrap + + +class WrapperTemplate: + """Class to encapsulate string templates for use in wrapper generation""" + boost_headers = textwrap.dedent(""" + #include + #include + #include + """) + + typdef_collectors = textwrap.dedent('''\ + typedef std::set*> Collector_{class_name}; + static Collector_{class_name} collector_{class_name}; + ''') + + delete_obj = textwrap.indent(textwrap.dedent('''\ + {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin(); + iter != collector_{class_name}.end(); ) {{ + delete *iter; + collector_{class_name}.erase(iter++); + anyDeleted = true; + }} }} + '''), + prefix=' ') + + delete_all_objects = textwrap.dedent(''' + void _deleteAllObjects() + {{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout);\n + bool anyDeleted = false; + {delete_objs} + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\\n" + "calling destructors, call \'clear all\' again if you plan to now recompile a wrap\\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); + }} + ''') + + rtti_register = textwrap.dedent('''\ + void _{module_name}_RTTIRegister() {{ + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created"); + if(!alreadyCreated) {{ + std::map types; + + {rtti_classes} + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(const StringPair& rtti_matlab: types) {{ + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) {{ + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + }} + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + }} + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {{ + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + }} + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {{ + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + }} + mxDestroyArray(newAlreadyCreated); + }} + }} + ''') + + collector_function_upcast_from_void = textwrap.dedent('''\ + void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr<{cpp_name}> Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast<{cpp_name}>(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; + }}\n + ''') + + class_serialize_method = textwrap.dedent('''\ + function varargout = string_serialize(this, varargin) + % STRING_SERIALIZE usage: string_serialize() : returns string + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 0 + varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}}); + else + error('Arguments do not match any overload of function {class_name}.string_serialize'); + end + end\n + function sobj = saveobj(obj) + % SAVEOBJ Saves the object to a matlab-readable format + sobj = obj.string_serialize(); + end + ''') + + collector_function_serialize = textwrap.indent(textwrap.dedent("""\ + typedef boost::shared_ptr<{full_name}> Shared; + checkArguments("string_serialize",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}"); + ostringstream out_archive_stream; + boost::archive::text_oarchive out_archive(out_archive_stream); + out_archive << *obj; + out[0] = wrap< string >(out_archive_stream.str()); + """), + prefix=' ') + + collector_function_deserialize = textwrap.indent(textwrap.dedent("""\ + typedef boost::shared_ptr<{full_name}> Shared; + checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1); + string serialized = unwrap< string >(in[0]); + istringstream in_archive_stream(serialized); + boost::archive::text_iarchive in_archive(in_archive_stream); + Shared output(new {full_name}()); + in_archive >> *output; + out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false); + """), + prefix=' ') + + mex_function = textwrap.dedent(''' + void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) + {{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout);\n + _{module_name}_RTTIRegister();\n + int id = unwrap(in[0]);\n + try {{ + switch(id) {{ + {cases} }} + }} catch(const std::exception& e) {{ + mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str()); + }}\n + std::cout.rdbuf(outbuf); + }} + ''') + + collector_function_shared_return = textwrap.indent(textwrap.dedent('''\ + {{ + boost::shared_ptr<{name}> shared({shared_obj}); + out[{id}] = wrap_shared_ptr(shared,"{name}"); + }}{new_line}'''), + prefix=' ') + + matlab_deserialize = textwrap.indent(textwrap.dedent("""\ + function varargout = string_deserialize(varargin) + % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name} + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 1 + varargout{{1}} = {wrapper}({id}, varargin{{:}}); + else + error('Arguments do not match any overload of function {class_name}.string_deserialize'); + end + end\n + function obj = loadobj(sobj) + % LOADOBJ Saves the object to a matlab-readable format + obj = {class_name}.string_deserialize(sobj); + end + """), + prefix=' ') diff --git a/wrap/gtwrap/matlab_wrapper.py b/wrap/gtwrap/matlab_wrapper/wrapper.py similarity index 68% rename from wrap/gtwrap/matlab_wrapper.py rename to wrap/gtwrap/matlab_wrapper/wrapper.py index de6221bbcf..b040d27311 100755 --- a/wrap/gtwrap/matlab_wrapper.py +++ b/wrap/gtwrap/matlab_wrapper/wrapper.py @@ -7,16 +7,19 @@ import os import os.path as osp -import sys import textwrap from functools import partial, reduce from typing import Dict, Iterable, List, Union +from loguru import logger + import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator +from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin +from gtwrap.matlab_wrapper.templates import WrapperTemplate -class MatlabWrapper(object): +class MatlabWrapper(CheckMixin, FormatMixin): """ Wrap the given C++ code into Matlab. Attributes @@ -25,89 +28,75 @@ class MatlabWrapper(object): top_module_namespace: C++ namespace for the top module (default '') ignore_classes: A list of classes to ignore (default []) """ - # Map the data type to its Matlab class. - # Found in Argument.cpp in old wrapper - data_type = { - 'string': 'char', - 'char': 'char', - 'unsigned char': 'unsigned char', - 'Vector': 'double', - 'Matrix': 'double', - 'int': 'numeric', - 'size_t': 'numeric', - 'bool': 'logical' - } - # Map the data type into the type used in Matlab methods. - # Found in matlab.h in old wrapper - data_type_param = { - 'string': 'char', - 'char': 'char', - 'unsigned char': 'unsigned char', - 'size_t': 'int', - 'int': 'int', - 'double': 'double', - 'Point2': 'double', - 'Point3': 'double', - 'Vector': 'double', - 'Matrix': 'double', - 'bool': 'bool' - } - # Methods that should not be wrapped directly - whitelist = ['serializable', 'serialize'] - # Methods that should be ignored - ignore_methods = ['pickle'] - # Datatypes that do not need to be checked in methods - not_check_type = [] # type: list - # Data types that are primitive types - not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] - # Ignore the namespace for these datatypes - ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3'] - # The amount of times the wrapper has created a call to geometry_wrapper - wrapper_id = 0 - # Map each wrapper id to what its collector function namespace, class, type, and string format - wrapper_map = {} - # Set of all the includes in the namespace - includes = {} # type: Dict[parser.Include, int] - # Set of all classes in the namespace - classes = [ - ] # type: List[Union[parser.Class, instantiator.InstantiatedClass]] - classes_elems = { - } # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] - # Id for ordering global functions in the wrapper - global_function_id = 0 - # Files and their content - content = [] # type: List[str] - - # Ensure the template file is always picked up from the correct directory. - dir_path = osp.dirname(osp.realpath(__file__)) - with open(osp.join(dir_path, "matlab_wrapper.tpl")) as f: - wrapper_file_header = f.read() - def __init__(self, module_name, top_module_namespace='', ignore_classes=()): + super().__init__() + self.module_name = module_name self.top_module_namespace = top_module_namespace self.ignore_classes = ignore_classes self.verbose = False - def _debug(self, message): - if not self.verbose: - return - print(message, file=sys.stderr) - - def _add_include(self, include): - self.includes[include] = 0 - - def _add_class(self, instantiated_class): + # Map the data type to its Matlab class. + # Found in Argument.cpp in old wrapper + self.data_type = { + 'string': 'char', + 'char': 'char', + 'unsigned char': 'unsigned char', + 'Vector': 'double', + 'Matrix': 'double', + 'int': 'numeric', + 'size_t': 'numeric', + 'bool': 'logical' + } + # Map the data type into the type used in Matlab methods. + # Found in matlab.h in old wrapper + self.data_type_param = { + 'string': 'char', + 'char': 'char', + 'unsigned char': 'unsigned char', + 'size_t': 'int', + 'int': 'int', + 'double': 'double', + 'Point2': 'double', + 'Point3': 'double', + 'Vector': 'double', + 'Matrix': 'double', + 'bool': 'bool' + } + # The amount of times the wrapper has created a call to geometry_wrapper + self.wrapper_id = 0 + # Map each wrapper id to its collector function namespace, class, type, and string format + self.wrapper_map: Dict = {} + # Set of all the includes in the namespace + self.includes: List[parser.Include] = [] + # Set of all classes in the namespace + self.classes: List[Union[parser.Class, + instantiator.InstantiatedClass]] = [] + self.classes_elems: Dict[Union[parser.Class, + instantiator.InstantiatedClass], + int] = {} + # Id for ordering global functions in the wrapper + self.global_function_id = 0 + # Files and their content + self.content: List[str] = [] + + # Ensure the template file is always picked up from the correct directory. + dir_path = osp.dirname(osp.realpath(__file__)) + with open(osp.join(dir_path, "matlab_wrapper.tpl")) as f: + self.wrapper_file_headers = f.read() + + def add_class(self, instantiated_class): + """Add `instantiated_class` to the list of classes.""" if self.classes_elems.get(instantiated_class) is None: self.classes_elems[instantiated_class] = 0 self.classes.append(instantiated_class) def _update_wrapper_id(self, collector_function=None, id_diff=0): - """Get and define wrapper ids. - + """ + Get and define wrapper ids. Generates the map of id -> collector function. Args: @@ -150,34 +139,6 @@ def _insert_spaces(self, x, y): """ return x + '\n' + ('' if y == '' else ' ') + y - def _is_shared_ptr(self, arg_type): - """ - Determine if the `interface_parser.Type` should be treated as a - shared pointer in the wrapper. - """ - return arg_type.is_shared_ptr or ( - arg_type.typename.name not in self.not_ptr_type - and arg_type.typename.name not in self.ignore_namespace - and arg_type.typename.name != 'string') - - def _is_ptr(self, arg_type): - """ - Determine if the `interface_parser.Type` should be treated as a - raw pointer in the wrapper. - """ - return arg_type.is_ptr or ( - arg_type.typename.name not in self.not_ptr_type - and arg_type.typename.name not in self.ignore_namespace - and arg_type.typename.name != 'string') - - def _is_ref(self, arg_type): - """Determine if the interface_parser.Type should be treated as a - reference in the wrapper. - """ - return arg_type.typename.name not in self.ignore_namespace and \ - arg_type.typename.name not in self.not_ptr_type and \ - arg_type.is_ref - def _group_methods(self, methods): """Group overloaded methods together""" method_map = {} @@ -190,181 +151,10 @@ def _group_methods(self, methods): method_map[method.name] = len(method_out) method_out.append([method]) else: - self._debug("[_group_methods] Merging {} with {}".format( - method_index, method.name)) method_out[method_index].append(method) return method_out - def _clean_class_name(self, instantiated_class): - """Reformatted the C++ class name to fit Matlab defined naming - standards - """ - if len(instantiated_class.ctors) != 0: - return instantiated_class.ctors[0].name - - return instantiated_class.name - - @classmethod - def _format_type_name(cls, - type_name, - separator='::', - include_namespace=True, - constructor=False, - method=False): - """ - Args: - type_name: an interface_parser.Typename to reformat - separator: the statement to add between namespaces and typename - include_namespace: whether to include namespaces when reformatting - constructor: if the typename will be in a constructor - method: if the typename will be in a method - - Raises: - constructor and method cannot both be true - """ - if constructor and method: - raise Exception( - 'Constructor and method parameters cannot both be True') - - formatted_type_name = '' - name = type_name.name - - if include_namespace: - for namespace in type_name.namespaces: - if name not in cls.ignore_namespace and namespace != '': - formatted_type_name += namespace + separator - - #self._debug("formatted_ns: {}, ns: {}".format(formatted_type_name, type_name.namespaces)) - if constructor: - formatted_type_name += cls.data_type.get(name) or name - elif method: - formatted_type_name += cls.data_type_param.get(name) or name - else: - formatted_type_name += name - - if separator == "::": # C++ - templates = [] - for idx in range(len(type_name.instantiations)): - template = '{}'.format( - cls._format_type_name(type_name.instantiations[idx], - include_namespace=include_namespace, - constructor=constructor, - method=method)) - templates.append(template) - - if len(templates) > 0: # If there are no templates - formatted_type_name += '<{}>'.format(','.join(templates)) - - else: - for idx in range(len(type_name.instantiations)): - formatted_type_name += '{}'.format( - cls._format_type_name(type_name.instantiations[idx], - separator=separator, - include_namespace=False, - constructor=constructor, - method=method)) - - return formatted_type_name - - @classmethod - def _format_return_type(cls, - return_type, - include_namespace=False, - separator="::"): - """Format return_type. - - Args: - return_type: an interface_parser.ReturnType to reformat - include_namespace: whether to include namespaces when reformatting - """ - return_wrap = '' - - if cls._return_count(return_type) == 1: - return_wrap = cls._format_type_name( - return_type.type1.typename, - separator=separator, - include_namespace=include_namespace) - else: - return_wrap = 'pair< {type1}, {type2} >'.format( - type1=cls._format_type_name( - return_type.type1.typename, - separator=separator, - include_namespace=include_namespace), - type2=cls._format_type_name( - return_type.type2.typename, - separator=separator, - include_namespace=include_namespace)) - - return return_wrap - - def _format_class_name(self, instantiated_class, separator=''): - """Format a template_instantiator.InstantiatedClass name.""" - if instantiated_class.parent == '': - parent_full_ns = [''] - else: - parent_full_ns = instantiated_class.parent.full_namespaces() - # class_name = instantiated_class.parent.name - # - # if class_name != '': - # class_name += separator - # - # class_name += instantiated_class.name - parentname = "".join([separator + x - for x in parent_full_ns]) + separator - - class_name = parentname[2 * len(separator):] - - class_name += instantiated_class.name - - return class_name - - def _format_static_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(static_method, parser.StaticMethod): - method += "".join([separator + x for x in static_method.parent.namespaces()]) + \ - separator + static_method.parent.name + separator - - return method[2 * len(separator):] - - def _format_instance_method(self, instance_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(instance_method, instantiator.InstantiatedMethod): - method_list = [ - separator + x - for x in instance_method.parent.parent.full_namespaces() - ] - method += "".join(method_list) + separator - - method += instance_method.parent.name + separator - method += instance_method.original.name - method += "<" + instance_method.instantiations.to_cpp() + ">" - - return method[2 * len(separator):] - - def _format_global_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(static_method, parser.GlobalFunction): - method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \ - separator - - return method[2 * len(separator):] - def _wrap_args(self, args): """Wrap an interface_parser.ArgumentList into a list of arguments. @@ -520,7 +310,7 @@ def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False): if params != '': params += ',' - if self._is_ref(arg.ctype): # and not constructor: + if self.is_ref(arg.ctype): # and not constructor: ctype_camel = self._format_type_name(arg.ctype.typename, separator='') body_args += textwrap.indent(textwrap.dedent('''\ @@ -531,7 +321,7 @@ def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False): id=arg_id)), prefix=' ') - elif (self._is_shared_ptr(arg.ctype) or self._is_ptr(arg.ctype)) and \ + elif (self.is_shared_ptr(arg.ctype) or self.is_ptr(arg.ctype)) and \ arg.ctype.typename.name not in self.ignore_namespace: if arg.ctype.is_shared_ptr: call_type = arg.ctype.is_shared_ptr @@ -665,22 +455,13 @@ def class_comment(self, instantiated_class): return comment - def generate_matlab_wrapper(self): - """Generate the C++ file for the wrapper.""" - file_name = self._wrapper_name() + '.cpp' - - wrapper_file = self.wrapper_file_header - - return file_name, wrapper_file - def wrap_method(self, methods): - """Wrap methods in the body of a class.""" + """ + Wrap methods in the body of a class. + """ if not isinstance(methods, list): methods = [methods] - # for method in methods: - # output = '' - return '' def wrap_methods(self, methods, global_funcs=False, global_ns=None): @@ -697,10 +478,6 @@ def wrap_methods(self, methods, global_funcs=False, global_ns=None): continue if global_funcs: - self._debug("[wrap_methods] wrapping: {}..{}={}".format( - method[0].parent.name, method[0].name, - type(method[0].parent.name))) - method_text = self.wrap_global_function(method) self.content.append(("".join([ '+' + x + '/' for x in global_ns.full_namespaces()[1:] @@ -838,11 +615,6 @@ def wrap_class_constructors(self, namespace_name, inst_class, parent_name, base_obj = '' - if has_parent: - self._debug("class: {} ns: {}".format( - parent_name, - self._format_class_name(inst_class.parent, separator="."))) - if has_parent: base_obj = ' obj = obj@{parent_name}(uint64(5139824614673773682), base_ptr);'.format( parent_name=parent_name) @@ -850,9 +622,6 @@ def wrap_class_constructors(self, namespace_name, inst_class, parent_name, if base_obj: base_obj = '\n' + base_obj - self._debug("class: {}, name: {}".format( - inst_class.name, self._format_class_name(inst_class, - separator="."))) methods_wrap += textwrap.indent(textwrap.dedent('''\ else error('Arguments do not match any overload of {class_name_doc} constructor'); @@ -1101,27 +870,12 @@ def wrap_static_methods(self, namespace_name, instantiated_class, prefix=" ") if serialize: - method_text += textwrap.indent(textwrap.dedent("""\ - function varargout = string_deserialize(varargin) - % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name} - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 1 - varargout{{1}} = {wrapper}({id}, varargin{{:}}); - else - error('Arguments do not match any overload of function {class_name}.string_deserialize'); - end - end\n - function obj = loadobj(sobj) - % LOADOBJ Saves the object to a matlab-readable format - obj = {class_name}.string_deserialize(sobj); - end - """).format( + method_text += WrapperTemplate.matlab_deserialize.format( class_name=namespace_name + '.' + instantiated_class.name, wrapper=self._wrapper_name(), id=self._update_wrapper_id( (namespace_name, instantiated_class, 'string_deserialize', - 'deserialize'))), - prefix=' ') + 'deserialize'))) return method_text @@ -1213,33 +967,32 @@ def wrap_instantiated_class(self, instantiated_class, namespace_name=''): return file_name + '.m', content_text - def wrap_namespace(self, namespace, parent=()): + def wrap_namespace(self, namespace): """Wrap a namespace by wrapping all of its components. Args: namespace: the interface_parser.namespace instance of the namespace parent: parent namespace """ - test_output = '' namespaces = namespace.full_namespaces() inner_namespace = namespace.name != '' wrapped = [] - self._debug("wrapping ns: {}, parent: {}".format( - namespace.full_namespaces(), parent)) - matlab_wrapper = self.generate_matlab_wrapper() - self.content.append((matlab_wrapper[0], matlab_wrapper[1])) + cpp_filename = self._wrapper_name() + '.cpp' + self.content.append((cpp_filename, self.wrapper_file_headers)) current_scope = [] namespace_scope = [] for element in namespace.content: if isinstance(element, parser.Include): - self._add_include(element) + self.includes.append(element) + elif isinstance(element, parser.Namespace): - self.wrap_namespace(element, namespaces) + self.wrap_namespace(element) + elif isinstance(element, instantiator.InstantiatedClass): - self._add_class(element) + self.add_class(element) if inner_namespace: class_text = self.wrap_instantiated_class( @@ -1265,7 +1018,7 @@ def wrap_namespace(self, namespace, parent=()): if isinstance(func, parser.GlobalFunction) ] - test_output += self.wrap_methods(all_funcs, True, global_ns=namespace) + self.wrap_methods(all_funcs, True, global_ns=namespace) return wrapped @@ -1277,16 +1030,12 @@ def wrap_collector_function_shared_return(self, """Wrap the collector function which returns a shared pointer.""" new_line = '\n' if new_line else '' - return textwrap.indent(textwrap.dedent('''\ - {{ - boost::shared_ptr<{name}> shared({shared_obj}); - out[{id}] = wrap_shared_ptr(shared,"{name}"); - }}{new_line}''').format(name=self._format_type_name( - return_type_name, include_namespace=False), - shared_obj=shared_obj, - id=func_id, - new_line=new_line), - prefix=' ') + return WrapperTemplate.collector_function_shared_return.format( + name=self._format_type_name(return_type_name, + include_namespace=False), + shared_obj=shared_obj, + id=func_id, + new_line=new_line) def wrap_collector_function_return_types(self, return_type, func_id): """ @@ -1296,7 +1045,7 @@ def wrap_collector_function_return_types(self, return_type, func_id): pair_value = 'first' if func_id == 0 else 'second' new_line = '\n' if func_id == 0 else '' - if self._is_shared_ptr(return_type) or self._is_ptr(return_type): + if self.is_shared_ptr(return_type) or self.is_ptr(return_type): shared_obj = 'pairResult.' + pair_value if not (return_type.is_shared_ptr or return_type.is_ptr): @@ -1355,16 +1104,12 @@ def wrap_collector_function_return(self, method): method_name = self._format_static_method(method, '::') method_name += method.name - if "MeasureRange" in method_name: - self._debug("method: {}, method: {}, inst: {}".format( - method_name, method.name, method.parent.to_cpp())) - obj = ' ' if return_1_name == 'void' else '' obj += '{}{}({})'.format(obj_start, method_name, params) if return_1_name != 'void': if return_count == 1: - if self._is_shared_ptr(return_1) or self._is_ptr(return_1): + if self.is_shared_ptr(return_1) or self.is_ptr(return_1): sep_method_name = partial(self._format_type_name, return_1.typename, include_namespace=True) @@ -1377,12 +1122,6 @@ def wrap_collector_function_return(self, method): shared_obj = '{obj},"{method_name_sep}"'.format( obj=obj, method_name_sep=sep_method_name('.')) else: - self._debug("Non-PTR: {}, {}".format( - return_1, type(return_1))) - self._debug("Inner type is: {}, {}".format( - return_1.typename.name, sep_method_name('.'))) - self._debug("Inner type instantiations: {}".format( - return_1.typename.instantiations)) method_name_sep_dot = sep_method_name('.') shared_obj_template = 'boost::make_shared<{method_name_sep_col}>({obj}),' \ '"{method_name_sep_dot}"' @@ -1417,16 +1156,8 @@ def wrap_collector_function_upcast_from_void(self, class_name, func_id, """ Add function to upcast type from void type. """ - return textwrap.dedent('''\ - void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{ - mexAtExit(&_deleteAllObjects); - typedef boost::shared_ptr<{cpp_name}> Shared; - boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); - out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast<{cpp_name}>(*asVoid)); - *reinterpret_cast(mxGetData(out[0])) = self; - }}\n - ''').format(class_name=class_name, cpp_name=cpp_name, id=func_id) + return WrapperTemplate.collector_function_upcast_from_void.format( + class_name=class_name, cpp_name=cpp_name, id=func_id) def generate_collector_function(self, func_id): """ @@ -1610,158 +1341,109 @@ def mex_function(self): else: next_case = None - mex_function = textwrap.dedent(''' - void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) - {{ - mstream mout; - std::streambuf *outbuf = std::cout.rdbuf(&mout);\n - _{module_name}_RTTIRegister();\n - int id = unwrap(in[0]);\n - try {{ - switch(id) {{ - {cases} }} - }} catch(const std::exception& e) {{ - mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str()); - }}\n - std::cout.rdbuf(outbuf); - }} - ''').format(module_name=self.module_name, cases=cases) + mex_function = WrapperTemplate.mex_function.format( + module_name=self.module_name, cases=cases) return mex_function - def generate_wrapper(self, namespace): - """Generate the c++ wrapper.""" - # Includes - wrapper_file = self.wrapper_file_header + textwrap.dedent(""" - #include - #include - #include \n - """) - - assert namespace + def get_class_name(self, cls): + """Get the name of the class `cls` taking template instantiations into account.""" + if cls.instantiations: + class_name_sep = cls.name + else: + class_name_sep = cls.to_cpp() - includes_list = sorted(list(self.includes.keys()), - key=lambda include: include.header) + class_name = self._format_class_name(cls) - # Check the number of includes. - # If no includes, do nothing, if 1 then just append newline. - # if more than one, concatenate them with newlines. - if len(includes_list) == 0: - pass - elif len(includes_list) == 1: - wrapper_file += (str(includes_list[0]) + '\n') - else: - wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), - includes_list) - wrapper_file += '\n' + return class_name, class_name_sep - typedef_instances = '\n' - typedef_collectors = '' + def generate_preamble(self): + """ + Generate the preamble of the wrapper file, which includes + the Boost exports, typedefs for collectors, and + the _deleteAllObjects and _RTTIRegister functions. + """ + delete_objs = '' + typedef_instances = [] boost_class_export_guid = '' - delete_objs = textwrap.dedent('''\ - void _deleteAllObjects() - { - mstream mout; - std::streambuf *outbuf = std::cout.rdbuf(&mout);\n - bool anyDeleted = false; - ''') - rtti_reg_start = textwrap.dedent('''\ - void _{module_name}_RTTIRegister() {{ - const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created"); - if(!alreadyCreated) {{ - std::map types; - ''').format(module_name=self.module_name) - rtti_reg_mid = '' - rtti_reg_end = textwrap.indent( - textwrap.dedent(''' - mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); - if(!registry) - registry = mxCreateStructMatrix(1, 1, 0, NULL); - typedef std::pair StringPair; - for(const StringPair& rtti_matlab: types) { - int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) - mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); - mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); - mxSetFieldByNumber(registry, 0, fieldId, matlabName); - } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) - mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); - mxDestroyArray(registry); - '''), - prefix=' ') + ' \n' + textwrap.dedent('''\ - mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) - mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); - mxDestroyArray(newAlreadyCreated); - } - } - ''') - ptr_ctor_frag = '' + typedef_collectors = '' + rtti_classes = '' for cls in self.classes: - uninstantiated_name = "::".join( - cls.namespaces()[1:]) + "::" + cls.name - self._debug("Cls: {} -> {}".format(cls.name, uninstantiated_name)) - + # Check if class is in ignore list. + # If so, then skip + uninstantiated_name = "::".join(cls.namespaces()[1:] + [cls.name]) if uninstantiated_name in self.ignore_classes: - self._debug("Ignoring: {} -> {}".format( - cls.name, uninstantiated_name)) continue - def _has_serialization(cls): - for m in cls.methods: - if m.name in self.whitelist: - return True - return False + class_name, class_name_sep = self.get_class_name(cls) + # If a class has instantiations, then declare the typedef for each instance if cls.instantiations: cls_insts = '' - for i, inst in enumerate(cls.instantiations): if i != 0: cls_insts += ', ' cls_insts += self._format_type_name(inst) - typedef_instances += 'typedef {original_class_name} {class_name_sep};\n' \ + typedef_instances.append('typedef {original_class_name} {class_name_sep};' \ .format(original_class_name=cls.to_cpp(), - class_name_sep=cls.name) + class_name_sep=cls.name)) - class_name_sep = cls.name - class_name = self._format_class_name(cls) + # Get the Boost exports for serialization + if cls.original.namespaces() and self._has_serialization(cls): + boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( + class_name_sep, class_name) - if len(cls.original.namespaces()) > 1 and _has_serialization( - cls): - boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( - class_name_sep, class_name) - else: - class_name_sep = cls.to_cpp() - class_name = self._format_class_name(cls) - - if len(cls.original.namespaces()) > 1 and _has_serialization( - cls): - boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( - class_name_sep, class_name) - - typedef_collectors += textwrap.dedent('''\ - typedef std::set*> Collector_{class_name}; - static Collector_{class_name} collector_{class_name}; - ''').format(class_name_sep=class_name_sep, class_name=class_name) - delete_objs += textwrap.indent(textwrap.dedent('''\ - {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin(); - iter != collector_{class_name}.end(); ) {{ - delete *iter; - collector_{class_name}.erase(iter++); - anyDeleted = true; - }} }} - ''').format(class_name=class_name), - prefix=' ') + # Typedef and declare the collector objects. + typedef_collectors += WrapperTemplate.typdef_collectors.format( + class_name_sep=class_name_sep, class_name=class_name) + + # Generate the _deleteAllObjects method + delete_objs += WrapperTemplate.delete_obj.format( + class_name=class_name) if cls.is_virtual: - rtti_reg_mid += ' types.insert(std::make_pair(typeid({}).name(), "{}"));\n' \ + class_name, class_name_sep = self.get_class_name(cls) + rtti_classes += ' types.insert(std::make_pair(typeid({}).name(), "{}"));\n' \ .format(class_name_sep, class_name) + # Generate the typedef instances string + typedef_instances = "\n".join(typedef_instances) + + # Generate the full deleteAllObjects function + delete_all_objs = WrapperTemplate.delete_all_objects.format( + delete_objs=delete_objs) + + # Generate the full RTTIRegister function + rtti_register = WrapperTemplate.rtti_register.format( + module_name=self.module_name, rtti_classes=rtti_classes) + + return typedef_instances, boost_class_export_guid, \ + typedef_collectors, delete_all_objs, rtti_register + + def generate_wrapper(self, namespace): + """Generate the c++ wrapper.""" + assert namespace, "Namespace if empty" + + # Generate the header includes + includes_list = sorted(self.includes, + key=lambda include: include.header) + includes = textwrap.dedent("""\ + {wrapper_file_headers} + {boost_headers} + {includes_list} + """).format(wrapper_file_headers=self.wrapper_file_headers.strip(), + boost_headers=WrapperTemplate.boost_headers, + includes_list='\n'.join(map(str, includes_list))) + + preamble = self.generate_preamble() + typedef_instances, boost_class_export_guid, \ + typedef_collectors, delete_all_objs, \ + rtti_register = preamble + + ptr_ctor_frag = '' set_next_case = False for idx in range(self.wrapper_id): @@ -1784,24 +1466,20 @@ def _has_serialization(cls): ptr_ctor_frag += self.wrap_collector_function_upcast_from_void( id_val[1].name, idx, id_val[1].to_cpp()) - wrapper_file += textwrap.dedent('''\ + wrapper_file = textwrap.dedent('''\ + {includes} {typedef_instances} {boost_class_export_guid} {typedefs_collectors} - {delete_objs} if(anyDeleted) - cout << - "WARNING: Wrap modules with variables in the workspace have been reloaded due to\\n" - "calling destructors, call \'clear all\' again if you plan to now recompile a wrap\\n" - "module, so that your recompiled module is used instead of the old one." << endl; - std::cout.rdbuf(outbuf); - }}\n + {delete_all_objs} {rtti_register} {pointer_constructor_fragment}{mex_function}''') \ - .format(typedef_instances=typedef_instances, + .format(includes=includes, + typedef_instances=typedef_instances, boost_class_export_guid=boost_class_export_guid, typedefs_collectors=typedef_collectors, - delete_objs=delete_objs, - rtti_register=rtti_reg_start + rtti_reg_mid + rtti_reg_end, + delete_all_objs=delete_all_objs, + rtti_register=rtti_register, pointer_constructor_fragment=ptr_ctor_frag, mex_function=self.mex_function()) @@ -1815,23 +1493,10 @@ def wrap_class_serialize_method(self, namespace_name, inst_class): wrapper_id = self._update_wrapper_id( (namespace_name, inst_class, 'string_serialize', 'serialize')) - return textwrap.dedent('''\ - function varargout = string_serialize(this, varargin) - % STRING_SERIALIZE usage: string_serialize() : returns string - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 0 - varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}}); - else - error('Arguments do not match any overload of function {class_name}.string_serialize'); - end - end\n - function sobj = saveobj(obj) - % SAVEOBJ Saves the object to a matlab-readable format - sobj = obj.string_serialize(); - end - ''').format(wrapper=self._wrapper_name(), - wrapper_id=wrapper_id, - class_name=namespace_name + '.' + class_name) + return WrapperTemplate.class_serialize_method.format( + wrapper=self._wrapper_name(), + wrapper_id=wrapper_id, + class_name=namespace_name + '.' + class_name) def wrap_collector_function_serialize(self, class_name, @@ -1840,18 +1505,8 @@ def wrap_collector_function_serialize(self, """ Wrap the serizalize collector function. """ - return textwrap.indent(textwrap.dedent("""\ - typedef boost::shared_ptr<{full_name}> Shared; - checkArguments("string_serialize",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}"); - ostringstream out_archive_stream; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << *obj; - out[0] = wrap< string >(out_archive_stream.str()); - """).format(class_name=class_name, - full_name=full_name, - namespace=namespace), - prefix=' ') + return WrapperTemplate.collector_function_serialize.format( + class_name=class_name, full_name=full_name, namespace=namespace) def wrap_collector_function_deserialize(self, class_name, @@ -1860,87 +1515,85 @@ def wrap_collector_function_deserialize(self, """ Wrap the deserizalize collector function. """ - return textwrap.indent(textwrap.dedent("""\ - typedef boost::shared_ptr<{full_name}> Shared; - checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1); - string serialized = unwrap< string >(in[0]); - istringstream in_archive_stream(serialized); - boost::archive::text_iarchive in_archive(in_archive_stream); - Shared output(new {full_name}()); - in_archive >> *output; - out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false); - """).format(class_name=class_name, - full_name=full_name, - namespace=namespace), - prefix=' ') + return WrapperTemplate.collector_function_deserialize.format( + class_name=class_name, full_name=full_name, namespace=namespace) - def wrap(self, content): - """High level function to wrap the project.""" - # Parse the contents of the interface file - parsed_result = parser.Module.parseString(content) - # Instantiate the module - module = instantiator.instantiate_namespace(parsed_result) - self.wrap_namespace(module) - self.generate_wrapper(module) + def generate_content(self, cc_content, path): + """ + Generate files and folders from matlab wrapper content. - return self.content + Args: + cc_content: The content to generate formatted as + (file_name, file_content) or + (folder_name, [(file_name, file_content)]) + path: The path to the files parent folder within the main folder + """ + for c in cc_content: + if isinstance(c, list): + if len(c) == 0: + continue + path_to_folder = osp.join(path, c[0][0]) + + if not osp.isdir(path_to_folder): + try: + os.makedirs(path_to_folder, exist_ok=True) + except OSError: + pass + + for sub_content in c: + self.generate_content(sub_content[1], path_to_folder) + + elif isinstance(c[1], list): + path_to_folder = osp.join(path, c[0]) + + if not osp.isdir(path_to_folder): + try: + os.makedirs(path_to_folder, exist_ok=True) + except OSError: + pass + for sub_content in c[1]: + path_to_file = osp.join(path_to_folder, sub_content[0]) + with open(path_to_file, 'w') as f: + f.write(sub_content[1]) + else: + path_to_file = osp.join(path, c[0]) -def generate_content(cc_content, path, verbose=False): - """ - Generate files and folders from matlab wrapper content. + if not osp.isdir(path_to_file): + try: + os.mkdir(path) + except OSError: + pass - Args: - cc_content: The content to generate formatted as - (file_name, file_content) or - (folder_name, [(file_name, file_content)]) - path: The path to the files parent folder within the main folder - """ - def _debug(message): - if not verbose: - return - print(message, file=sys.stderr) - - for c in cc_content: - if isinstance(c, list): - if len(c) == 0: - continue - _debug("c object: {}".format(c[0][0])) - path_to_folder = osp.join(path, c[0][0]) - - if not os.path.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - - for sub_content in c: - _debug("sub object: {}".format(sub_content[1][0][0])) - generate_content(sub_content[1], path_to_folder) - - elif isinstance(c[1], list): - path_to_folder = osp.join(path, c[0]) - - _debug("[generate_content_global]: {}".format(path_to_folder)) - if not os.path.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - for sub_content in c[1]: - path_to_file = osp.join(path_to_folder, sub_content[0]) - _debug("[generate_global_method]: {}".format(path_to_file)) with open(path_to_file, 'w') as f: - f.write(sub_content[1]) - else: - path_to_file = osp.join(path, c[0]) + f.write(c[1]) - _debug("[generate_content]: {}".format(path_to_file)) - if not os.path.isdir(path_to_file): - try: - os.mkdir(path) - except OSError: - pass + def wrap(self, files, path): + """High level function to wrap the project.""" + modules = {} + for file in files: + with open(file, 'r') as f: + content = f.read() - with open(path_to_file, 'w') as f: - f.write(c[1]) + # Parse the contents of the interface file + parsed_result = parser.Module.parseString(content) + # print(parsed_result) + + # Instantiate the module + module = instantiator.instantiate_namespace(parsed_result) + + if module.name in modules: + modules[module. + name].content[0].content += module.content[0].content + else: + modules[module.name] = module + + for module in modules.values(): + # Wrap the full namespace + self.wrap_namespace(module) + self.generate_wrapper(module) + + # Generate the corresponding .m and .cpp files + self.generate_content(self.content, path) + + return self.content diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py index 0e1b3c7eaa..40571263aa 100755 --- a/wrap/gtwrap/pybind_wrapper.py +++ b/wrap/gtwrap/pybind_wrapper.py @@ -13,6 +13,7 @@ # pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, line-too-long import re +from pathlib import Path import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator @@ -32,7 +33,7 @@ def __init__(self, self.top_module_namespaces = top_module_namespaces self.use_boost = use_boost self.ignore_classes = ignore_classes - self._serializing_classes = list() + self._serializing_classes = [] self.module_template = module_template self.python_keywords = [ 'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import', @@ -160,7 +161,7 @@ def _wrap_method(self, 'self->print', 'py::scoped_ostream_redirect output; self->print') - # Make __repr__() call print() internally + # Make __repr__() call .print() internally ret += '''{prefix}.def("__repr__", [](const {cpp_class}& self{opt_comma}{args_signature_with_names}){{ gtsam::RedirectCout redirect; @@ -557,8 +558,15 @@ def wrap_namespace(self, namespace): ) return wrapped, includes - def wrap(self, content): - """Wrap the code in the interface file.""" + def wrap_file(self, content, module_name=None, submodules=None): + """ + Wrap the code in the interface file. + + Args: + content: The contents of the interface file. + module_name: The name of the module. + submodules: List of other interface file names that should be linked to. + """ # Parse the contents of the interface file module = parser.Module.parseString(content) # Instantiate all templates @@ -574,23 +582,74 @@ def wrap(self, content): if ',' in cpp_class: new_name = re.sub("[,:<> ]", "", cpp_class) boost_class_export += "typedef {cpp_class} {new_name};\n".format( # noqa - cpp_class=cpp_class, - new_name=new_name, - ) + cpp_class=cpp_class, new_name=new_name) + boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format( new_name=new_name, ) + # Reset the serializing classes list + self._serializing_classes = [] + holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \ "{shared_ptr_type}::shared_ptr);" include_boost = "#include " if self.use_boost else "" + submodules_init = [] + + if submodules is not None: + module_def = "PYBIND11_MODULE({0}, m_)".format(module_name) + + for idx, submodule in enumerate(submodules): + submodules[idx] = "void {0}(py::module_ &);".format(submodule) + submodules_init.append("{0}(m_);".format(submodule)) + + else: + module_def = "void {0}(py::module_ &m_)".format(module_name) + submodules = [] + return self.module_template.format( include_boost=include_boost, - module_name=self.module_name, + module_def=module_def, + module_name=module_name, includes=includes, holder_type=holder_type.format( shared_ptr_type=('boost' if self.use_boost else 'std')) if self.use_boost else "", wrapped_namespace=wrapped_namespace, boost_class_export=boost_class_export, + submodules="\n".join(submodules), + submodules_init="\n".join(submodules_init), ) + + def wrap(self, sources, main_output): + """ + Wrap all the source interface files. + + Args: + sources: List of all interface files. + main_output: The name for the main module. + """ + main_module = sources[0] + submodules = [] + for source in sources[1:]: + filename = Path(source).name + module_name = Path(source).stem + # Read in the complete interface (.i) file + with open(source, "r") as f: + content = f.read() + submodules.append(module_name) + cc_content = self.wrap_file(content, module_name=module_name) + + # Generate the C++ code which Pybind11 will use. + with open(filename.replace(".i", ".cpp"), "w") as f: + f.write(cc_content) + + with open(main_module, "r") as f: + content = f.read() + cc_content = self.wrap_file(content, + module_name=self.module_name, + submodules=submodules) + + # Generate the C++ code which Pybind11 will use. + with open(main_output, "w") as f: + f.write(cc_content) diff --git a/wrap/gtwrap/template_instantiator.py b/wrap/gtwrap/template_instantiator.py index c474246489..87729cfa6d 100644 --- a/wrap/gtwrap/template_instantiator.py +++ b/wrap/gtwrap/template_instantiator.py @@ -4,7 +4,7 @@ import itertools from copy import deepcopy -from typing import List +from typing import Iterable, List import gtwrap.interface_parser as parser @@ -29,12 +29,13 @@ def instantiate_type(ctype: parser.Type, ctype = deepcopy(ctype) # Check if the return type has template parameters - if len(ctype.typename.instantiations) > 0: + if ctype.typename.instantiations: for idx, instantiation in enumerate(ctype.typename.instantiations): if instantiation.name in template_typenames: template_idx = template_typenames.index(instantiation.name) - ctype.typename.instantiations[idx] = instantiations[ - template_idx] + ctype.typename.instantiations[ + idx] = instantiations[ # type: ignore + template_idx] return ctype @@ -212,7 +213,9 @@ class A { void func(X x, Y y); } """ - def __init__(self, original, instantiations: List[parser.Typename] = ''): + def __init__(self, + original, + instantiations: Iterable[parser.Typename] = ()): self.original = original self.instantiations = instantiations self.template = '' @@ -278,7 +281,7 @@ def __init__(self, original: parser.Class, instantiations=(), new_name=''): self.original = original self.instantiations = instantiations - self.template = '' + self.template = None self.is_virtual = original.is_virtual self.parent_class = original.parent_class self.parent = original.parent @@ -318,7 +321,7 @@ def __init__(self, original: parser.Class, instantiations=(), new_name=''): self.methods = [] for method in instantiated_methods: if not method.template: - self.methods.append(InstantiatedMethod(method, '')) + self.methods.append(InstantiatedMethod(method, ())) else: instantiations = [] # Get all combinations of template parameters @@ -342,9 +345,9 @@ def __init__(self, original: parser.Class, instantiations=(), new_name=''): ) def __repr__(self): - return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\ - "{ctors}\n{static_methods}\n{methods}".format( - virtual="virtual" if self.is_virtual else '', + return "{virtual}Class {cpp_class} : {parent_class}\n"\ + "{ctors}\n{static_methods}\n{methods}\n{operators}".format( + virtual="virtual " if self.is_virtual else '', name=self.name, cpp_class=self.to_cpp(), parent_class=self.parent, diff --git a/wrap/matlab.h b/wrap/matlab.h index 4f3bfe96e3..bcdef3c8dd 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -26,6 +26,7 @@ #include #include #include +#include using gtsam::Vector; using gtsam::Matrix; diff --git a/wrap/scripts/matlab_wrap.py b/wrap/scripts/matlab_wrap.py index be6043947f..0f6664a635 100644 --- a/wrap/scripts/matlab_wrap.py +++ b/wrap/scripts/matlab_wrap.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - """ Helper script to wrap C++ to Matlab. This script is installed via CMake to the user's binary directory @@ -7,19 +6,24 @@ """ import argparse -import os import sys -from gtwrap.matlab_wrapper import MatlabWrapper, generate_content +from gtwrap.matlab_wrapper import MatlabWrapper if __name__ == "__main__": arg_parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) - arg_parser.add_argument("--src", type=str, required=True, + arg_parser.add_argument("--src", + type=str, + required=True, help="Input interface .h file.") - arg_parser.add_argument("--module_name", type=str, required=True, + arg_parser.add_argument("--module_name", + type=str, + required=True, help="Name of the C++ class being wrapped.") - arg_parser.add_argument("--out", type=str, required=True, + arg_parser.add_argument("--out", + type=str, + required=True, help="Name of the output folder.") arg_parser.add_argument( "--top_module_namespaces", @@ -33,28 +37,22 @@ "`.Class` of the corresponding C++ `ns1::ns2::ns3::Class`" ", and `from import ns4` gives you access to a Python " "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ") - arg_parser.add_argument("--ignore", - nargs='*', - type=str, - help="A space-separated list of classes to ignore. " - "Class names must include their full namespaces.") + arg_parser.add_argument( + "--ignore", + nargs='*', + type=str, + help="A space-separated list of classes to ignore. " + "Class names must include their full namespaces.") args = arg_parser.parse_args() top_module_namespaces = args.top_module_namespaces.split("::") if top_module_namespaces[0]: top_module_namespaces = [''] + top_module_namespaces - with open(args.src, 'r') as f: - content = f.read() - - if not os.path.exists(args.src): - os.mkdir(args.src) - - print("Ignoring classes: {}".format(args.ignore), file=sys.stderr) + print("[MatlabWrapper] Ignoring classes: {}".format(args.ignore), file=sys.stderr) wrapper = MatlabWrapper(module_name=args.module_name, top_module_namespace=top_module_namespaces, ignore_classes=args.ignore) - cc_content = wrapper.wrap(content) - - generate_content(cc_content, args.out) + sources = args.src.split(';') + cc_content = wrapper.wrap(sources, path=args.out) diff --git a/wrap/scripts/pybind_wrap.py b/wrap/scripts/pybind_wrap.py index 7f2f8d4192..c82a1d24c0 100644 --- a/wrap/scripts/pybind_wrap.py +++ b/wrap/scripts/pybind_wrap.py @@ -67,10 +67,6 @@ def main(): if top_module_namespaces[0]: top_module_namespaces = [''] + top_module_namespaces - # Read in the complete interface (.i) file - with open(args.src, "r") as f: - content = f.read() - with open(args.template, "r") as f: template_content = f.read() @@ -83,11 +79,8 @@ def main(): ) # Wrap the code and get back the cpp/cc code. - cc_content = wrapper.wrap(content) - - # Generate the C++ code which Pybind11 will use. - with open(args.out, "w") as f: - f.write(cc_content) + sources = args.src.split(';') + wrapper.wrap(sources, args.out) if __name__ == "__main__": diff --git a/wrap/templates/pybind_wrapper.tpl.example b/wrap/templates/pybind_wrapper.tpl.example index bf5b334900..485aa8d00c 100644 --- a/wrap/templates/pybind_wrapper.tpl.example +++ b/wrap/templates/pybind_wrapper.tpl.example @@ -7,7 +7,7 @@ #include #include #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. {includes} #include @@ -22,9 +22,13 @@ using namespace std; namespace py = pybind11; -PYBIND11_MODULE({module_name}, m_) {{ +{submodules} + +{module_def} {{ m_.doc() = "pybind11 wrapper of {module_name}"; +{submodules_init} + {wrapped_namespace} #include "python/specializations.h" diff --git a/wrap/tests/expected/matlab/+gtsam/Class1.m b/wrap/tests/expected/matlab/+gtsam/Class1.m new file mode 100644 index 0000000000..00dd5ca746 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/Class1.m @@ -0,0 +1,36 @@ +%class Class1, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%Class1() +% +classdef Class1 < handle + properties + ptr_gtsamClass1 = 0 + end + methods + function obj = Class1(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(0, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(1); + else + error('Arguments do not match any overload of gtsam.Class1 constructor'); + end + obj.ptr_gtsamClass1 = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(2, obj.ptr_gtsamClass1); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/Class2.m b/wrap/tests/expected/matlab/+gtsam/Class2.m new file mode 100644 index 0000000000..93279e1560 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/Class2.m @@ -0,0 +1,36 @@ +%class Class2, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%Class2() +% +classdef Class2 < handle + properties + ptr_gtsamClass2 = 0 + end + methods + function obj = Class2(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(3, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(4); + else + error('Arguments do not match any overload of gtsam.Class2 constructor'); + end + obj.ptr_gtsamClass2 = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(5, obj.ptr_gtsamClass2); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/ClassA.m b/wrap/tests/expected/matlab/+gtsam/ClassA.m new file mode 100644 index 0000000000..3210e93c60 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/ClassA.m @@ -0,0 +1,36 @@ +%class ClassA, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%ClassA() +% +classdef ClassA < handle + properties + ptr_gtsamClassA = 0 + end + methods + function obj = ClassA(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(6, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(7); + else + error('Arguments do not match any overload of gtsam.ClassA constructor'); + end + obj.ptr_gtsamClassA = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(8, obj.ptr_gtsamClassA); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/class_wrapper.cpp b/wrap/tests/expected/matlab/class_wrapper.cpp index e644ac00f2..fab9c14506 100644 --- a/wrap/tests/expected/matlab/class_wrapper.cpp +++ b/wrap/tests/expected/matlab/class_wrapper.cpp @@ -7,7 +7,6 @@ #include - typedef Fun FunDouble; typedef PrimitiveRef PrimitiveRefDouble; typedef MyVector<3> MyVector3; @@ -16,7 +15,6 @@ typedef MultipleTemplates MultipleTemplatesIntDouble; typedef MultipleTemplates MultipleTemplatesIntFloat; typedef MyFactor MyFactorPosePoint2; - typedef std::set*> Collector_FunRange; static Collector_FunRange collector_FunRange; typedef std::set*> Collector_FunDouble; @@ -38,6 +36,7 @@ static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; + void _deleteAllObjects() { mstream mout; @@ -104,6 +103,7 @@ void _deleteAllObjects() collector_MyFactorPosePoint2.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -117,24 +117,29 @@ void _class_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/expected/matlab/functions_wrapper.cpp b/wrap/tests/expected/matlab/functions_wrapper.cpp index ae7f49c410..d0f0f8ca67 100644 --- a/wrap/tests/expected/matlab/functions_wrapper.cpp +++ b/wrap/tests/expected/matlab/functions_wrapper.cpp @@ -5,38 +5,11 @@ #include #include -#include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; + void _deleteAllObjects() { @@ -44,66 +17,7 @@ void _deleteAllObjects() std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -117,24 +31,29 @@ void _functions_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/expected/matlab/geometry_wrapper.cpp b/wrap/tests/expected/matlab/geometry_wrapper.cpp index 4d8a7c7893..81631390c9 100644 --- a/wrap/tests/expected/matlab/geometry_wrapper.cpp +++ b/wrap/tests/expected/matlab/geometry_wrapper.cpp @@ -5,112 +5,25 @@ #include #include -#include #include #include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; static Collector_gtsamPoint2 collector_gtsamPoint2; typedef std::set*> Collector_gtsamPoint3; static Collector_gtsamPoint3 collector_gtsamPoint3; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); iter != collector_gtsamPoint2.end(); ) { delete *iter; @@ -123,6 +36,7 @@ void _deleteAllObjects() collector_gtsamPoint3.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -136,24 +50,29 @@ void _geometry_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/expected/matlab/inheritance_wrapper.cpp b/wrap/tests/expected/matlab/inheritance_wrapper.cpp index 077df48305..8e61ac8c61 100644 --- a/wrap/tests/expected/matlab/inheritance_wrapper.cpp +++ b/wrap/tests/expected/matlab/inheritance_wrapper.cpp @@ -5,47 +5,11 @@ #include #include -#include -#include -#include - -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; + + typedef MyTemplate MyTemplatePoint2; typedef MyTemplate MyTemplateMatrix; -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); - -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; -typedef std::set*> Collector_gtsamPoint2; -static Collector_gtsamPoint2 collector_gtsamPoint2; -typedef std::set*> Collector_gtsamPoint3; -static Collector_gtsamPoint3 collector_gtsamPoint3; typedef std::set*> Collector_MyBase; static Collector_MyBase collector_MyBase; typedef std::set*> Collector_MyTemplatePoint2; @@ -55,84 +19,13 @@ static Collector_MyTemplateMatrix collector_MyTemplateMatrix; typedef std::set*> Collector_ForwardKinematicsFactor; static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); - iter != collector_gtsamPoint2.end(); ) { - delete *iter; - collector_gtsamPoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin(); - iter != collector_gtsamPoint3.end(); ) { - delete *iter; - collector_gtsamPoint3.erase(iter++); - anyDeleted = true; - } } { for(Collector_MyBase::iterator iter = collector_MyBase.begin(); iter != collector_MyBase.end(); ) { delete *iter; @@ -157,6 +50,7 @@ void _deleteAllObjects() collector_ForwardKinematicsFactor.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -169,49 +63,54 @@ void _inheritance_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_inheritance_rttiRegistry_created"); if(!alreadyCreated) { std::map types; + types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix")); types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } -void gtsamPoint2_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); - typedef boost::shared_ptr Shared; + typedef boost::shared_ptr Shared; Shared *self = *reinterpret_cast (mxGetData(in[0])); - collector_gtsamPoint2.insert(self); + collector_MyBase.insert(self); } -void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ +void MyBase_upcastFromVoid_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; - - Shared *self = *reinterpret_cast (mxGetData(in[0])); - collector_MyBase.insert(self); + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; } void MyBase_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) @@ -227,19 +126,6 @@ void MyBase_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArr } } -void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - typedef boost::shared_ptr Shared; - checkArguments("delete_gtsamPoint2",nargout,nargin,1); - Shared *self = *reinterpret_cast(mxGetData(in[0])); - Collector_gtsamPoint2::iterator item; - item = collector_gtsamPoint2.find(self); - if(item != collector_gtsamPoint2.end()) { - delete self; - collector_gtsamPoint2.erase(item); - } -} - void MyTemplatePoint2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -253,6 +139,15 @@ void MyTemplatePoint2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void MyTemplatePoint2_upcastFromVoid_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + void MyTemplatePoint2_constructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -399,20 +294,6 @@ void MyTemplatePoint2_Level_18(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplatePoint2", false); } -void gtsamPoint3_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - mexAtExit(&_deleteAllObjects); - typedef boost::shared_ptr Shared; - - double x = unwrap< double >(in[0]); - double y = unwrap< double >(in[1]); - double z = unwrap< double >(in[2]); - Shared *self = new Shared(new gtsam::Point3(x,y,z)); - collector_gtsamPoint3.insert(self); - out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - *reinterpret_cast (mxGetData(out[0])) = self; -} - void MyTemplateMatrix_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -426,6 +307,15 @@ void MyTemplateMatrix_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void MyTemplateMatrix_upcastFromVoid_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + void MyTemplateMatrix_constructor_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -572,14 +462,6 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateMatrix", false); } -void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("return_vector2",nargout,nargin-1,1); - auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); - Vector value = unwrap< Vector >(in[1]); - out[0] = wrap< Vector >(obj->return_vector2(value)); -} - void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -593,6 +475,15 @@ void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + void ForwardKinematicsFactor_deconstructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; @@ -619,19 +510,19 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) try { switch(id) { case 0: - gtsamPoint2_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + MyBase_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); break; case 1: - MyBase_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + MyBase_upcastFromVoid_1(nargout, out, nargin-1, in+1); break; case 2: MyBase_deconstructor_2(nargout, out, nargin-1, in+1); break; case 3: - gtsamPoint2_deconstructor_3(nargout, out, nargin-1, in+1); + MyTemplatePoint2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); break; case 4: - MyTemplatePoint2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); + MyTemplatePoint2_upcastFromVoid_4(nargout, out, nargin-1, in+1); break; case 5: MyTemplatePoint2_constructor_5(nargout, out, nargin-1, in+1); @@ -676,10 +567,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplatePoint2_Level_18(nargout, out, nargin-1, in+1); break; case 19: - gtsamPoint3_constructor_19(nargout, out, nargin-1, in+1); + MyTemplateMatrix_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1); break; case 20: - MyTemplateMatrix_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1); + MyTemplateMatrix_upcastFromVoid_20(nargout, out, nargin-1, in+1); break; case 21: MyTemplateMatrix_constructor_21(nargout, out, nargin-1, in+1); @@ -724,10 +615,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1); break; case 35: - Test_return_vector2_35(nargout, out, nargin-1, in+1); + ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); break; case 36: - ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); + ForwardKinematicsFactor_upcastFromVoid_36(nargout, out, nargin-1, in+1); break; case 37: ForwardKinematicsFactor_deconstructor_37(nargout, out, nargin-1, in+1); diff --git a/wrap/tests/expected/matlab/multiple_files_wrapper.cpp b/wrap/tests/expected/matlab/multiple_files_wrapper.cpp new file mode 100644 index 0000000000..66ab7ff73d --- /dev/null +++ b/wrap/tests/expected/matlab/multiple_files_wrapper.cpp @@ -0,0 +1,229 @@ +#include +#include + +#include +#include +#include + + + + + +typedef std::set*> Collector_gtsamClass1; +static Collector_gtsamClass1 collector_gtsamClass1; +typedef std::set*> Collector_gtsamClass2; +static Collector_gtsamClass2 collector_gtsamClass2; +typedef std::set*> Collector_gtsamClassA; +static Collector_gtsamClassA collector_gtsamClassA; + + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_gtsamClass1::iterator iter = collector_gtsamClass1.begin(); + iter != collector_gtsamClass1.end(); ) { + delete *iter; + collector_gtsamClass1.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamClass2::iterator iter = collector_gtsamClass2.begin(); + iter != collector_gtsamClass2.end(); ) { + delete *iter; + collector_gtsamClass2.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamClassA::iterator iter = collector_gtsamClassA.begin(); + iter != collector_gtsamClassA.end(); ) { + delete *iter; + collector_gtsamClassA.erase(iter++); + anyDeleted = true; + } } + + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" + "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); +} + +void _multiple_files_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_multiple_files_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + + + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(const StringPair& rtti_matlab: types) { + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + } + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(newAlreadyCreated); + } +} + +void gtsamClass1_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamClass1.insert(self); +} + +void gtsamClass1_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Class1()); + collector_gtsamClass1.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClass1_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClass1",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClass1::iterator item; + item = collector_gtsamClass1.find(self); + if(item != collector_gtsamClass1.end()) { + delete self; + collector_gtsamClass1.erase(item); + } +} + +void gtsamClass2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamClass2.insert(self); +} + +void gtsamClass2_constructor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Class2()); + collector_gtsamClass2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClass2_deconstructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClass2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClass2::iterator item; + item = collector_gtsamClass2.find(self); + if(item != collector_gtsamClass2.end()) { + delete self; + collector_gtsamClass2.erase(item); + } +} + +void gtsamClassA_collectorInsertAndMakeBase_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamClassA.insert(self); +} + +void gtsamClassA_constructor_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::ClassA()); + collector_gtsamClassA.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClassA_deconstructor_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClassA",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClassA::iterator item; + item = collector_gtsamClassA.find(self); + if(item != collector_gtsamClassA.end()) { + delete self; + collector_gtsamClassA.erase(item); + } +} + + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _multiple_files_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + gtsamClass1_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + gtsamClass1_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + gtsamClass1_deconstructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + gtsamClass2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); + break; + case 4: + gtsamClass2_constructor_4(nargout, out, nargin-1, in+1); + break; + case 5: + gtsamClass2_deconstructor_5(nargout, out, nargin-1, in+1); + break; + case 6: + gtsamClassA_collectorInsertAndMakeBase_6(nargout, out, nargin-1, in+1); + break; + case 7: + gtsamClassA_constructor_7(nargout, out, nargin-1, in+1); + break; + case 8: + gtsamClassA_deconstructor_8(nargout, out, nargin-1, in+1); + break; + } + } catch(const std::exception& e) { + mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); + } + + std::cout.rdbuf(outbuf); +} diff --git a/wrap/tests/expected/matlab/namespaces_wrapper.cpp b/wrap/tests/expected/matlab/namespaces_wrapper.cpp index 8f6e415e2d..604ede5da5 100644 --- a/wrap/tests/expected/matlab/namespaces_wrapper.cpp +++ b/wrap/tests/expected/matlab/namespaces_wrapper.cpp @@ -5,9 +5,6 @@ #include #include -#include -#include -#include #include #include #include @@ -15,51 +12,8 @@ #include #include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef MyTemplate MyTemplatePoint2; -typedef MyTemplate MyTemplateMatrix; - -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); - -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; -typedef std::set*> Collector_gtsamPoint2; -static Collector_gtsamPoint2 collector_gtsamPoint2; -typedef std::set*> Collector_gtsamPoint3; -static Collector_gtsamPoint3 collector_gtsamPoint3; -typedef std::set*> Collector_MyBase; -static Collector_MyBase collector_MyBase; -typedef std::set*> Collector_MyTemplatePoint2; -static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; -typedef std::set*> Collector_MyTemplateMatrix; -static Collector_MyTemplateMatrix collector_MyTemplateMatrix; -typedef std::set*> Collector_ForwardKinematicsFactor; -static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; + + typedef std::set*> Collector_ns1ClassA; static Collector_ns1ClassA collector_ns1ClassA; typedef std::set*> Collector_ns1ClassB; @@ -75,108 +29,13 @@ static Collector_ClassD collector_ClassD; typedef std::set*> Collector_gtsamValues; static Collector_gtsamValues collector_gtsamValues; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); - iter != collector_gtsamPoint2.end(); ) { - delete *iter; - collector_gtsamPoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin(); - iter != collector_gtsamPoint3.end(); ) { - delete *iter; - collector_gtsamPoint3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyBase::iterator iter = collector_MyBase.begin(); - iter != collector_MyBase.end(); ) { - delete *iter; - collector_MyBase.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin(); - iter != collector_MyTemplatePoint2.end(); ) { - delete *iter; - collector_MyTemplatePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin(); - iter != collector_MyTemplateMatrix.end(); ) { - delete *iter; - collector_MyTemplateMatrix.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin(); - iter != collector_ForwardKinematicsFactor.end(); ) { - delete *iter; - collector_ForwardKinematicsFactor.erase(iter++); - anyDeleted = true; - } } { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); iter != collector_ns1ClassA.end(); ) { delete *iter; @@ -219,6 +78,7 @@ void _deleteAllObjects() collector_gtsamValues.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -231,10 +91,8 @@ void _namespaces_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_namespaces_rttiRegistry_created"); if(!alreadyCreated) { std::map types; - types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); - types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); - types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix")); - types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) @@ -242,18 +100,21 @@ void _namespaces_RTTIRegister() { typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/expected/matlab/special_cases_wrapper.cpp b/wrap/tests/expected/matlab/special_cases_wrapper.cpp index 056ce80973..69abbf73be 100644 --- a/wrap/tests/expected/matlab/special_cases_wrapper.cpp +++ b/wrap/tests/expected/matlab/special_cases_wrapper.cpp @@ -5,78 +5,11 @@ #include #include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include - -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef MyTemplate MyTemplatePoint2; -typedef MyTemplate MyTemplateMatrix; + typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); - -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; -typedef std::set*> Collector_gtsamPoint2; -static Collector_gtsamPoint2 collector_gtsamPoint2; -typedef std::set*> Collector_gtsamPoint3; -static Collector_gtsamPoint3 collector_gtsamPoint3; -typedef std::set*> Collector_MyBase; -static Collector_MyBase collector_MyBase; -typedef std::set*> Collector_MyTemplatePoint2; -static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; -typedef std::set*> Collector_MyTemplateMatrix; -static Collector_MyTemplateMatrix collector_MyTemplateMatrix; -typedef std::set*> Collector_ForwardKinematicsFactor; -static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; -typedef std::set*> Collector_ns1ClassA; -static Collector_ns1ClassA collector_ns1ClassA; -typedef std::set*> Collector_ns1ClassB; -static Collector_ns1ClassB collector_ns1ClassB; -typedef std::set*> Collector_ns2ClassA; -static Collector_ns2ClassA collector_ns2ClassA; -typedef std::set*> Collector_ns2ns3ClassB; -static Collector_ns2ns3ClassB collector_ns2ns3ClassB; -typedef std::set*> Collector_ns2ClassC; -static Collector_ns2ClassC collector_ns2ClassC; -typedef std::set*> Collector_ClassD; -static Collector_ClassD collector_ClassD; -typedef std::set*> Collector_gtsamValues; -static Collector_gtsamValues collector_gtsamValues; typedef std::set*> Collector_gtsamNonlinearFactorGraph; static Collector_gtsamNonlinearFactorGraph collector_gtsamNonlinearFactorGraph; typedef std::set*> Collector_gtsamSfmTrack; @@ -86,150 +19,13 @@ static Collector_gtsamPinholeCameraCal3Bundler collector_gtsamPinholeCameraCal3B typedef std::set*> Collector_gtsamGeneralSFMFactorCal3Bundler; static Collector_gtsamGeneralSFMFactorCal3Bundler collector_gtsamGeneralSFMFactorCal3Bundler; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); - iter != collector_gtsamPoint2.end(); ) { - delete *iter; - collector_gtsamPoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin(); - iter != collector_gtsamPoint3.end(); ) { - delete *iter; - collector_gtsamPoint3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyBase::iterator iter = collector_MyBase.begin(); - iter != collector_MyBase.end(); ) { - delete *iter; - collector_MyBase.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin(); - iter != collector_MyTemplatePoint2.end(); ) { - delete *iter; - collector_MyTemplatePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin(); - iter != collector_MyTemplateMatrix.end(); ) { - delete *iter; - collector_MyTemplateMatrix.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin(); - iter != collector_ForwardKinematicsFactor.end(); ) { - delete *iter; - collector_ForwardKinematicsFactor.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); - iter != collector_ns1ClassA.end(); ) { - delete *iter; - collector_ns1ClassA.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin(); - iter != collector_ns1ClassB.end(); ) { - delete *iter; - collector_ns1ClassB.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin(); - iter != collector_ns2ClassA.end(); ) { - delete *iter; - collector_ns2ClassA.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin(); - iter != collector_ns2ns3ClassB.end(); ) { - delete *iter; - collector_ns2ns3ClassB.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin(); - iter != collector_ns2ClassC.end(); ) { - delete *iter; - collector_ns2ClassC.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ClassD::iterator iter = collector_ClassD.begin(); - iter != collector_ClassD.end(); ) { - delete *iter; - collector_ClassD.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin(); - iter != collector_gtsamValues.end(); ) { - delete *iter; - collector_gtsamValues.erase(iter++); - anyDeleted = true; - } } { for(Collector_gtsamNonlinearFactorGraph::iterator iter = collector_gtsamNonlinearFactorGraph.begin(); iter != collector_gtsamNonlinearFactorGraph.end(); ) { delete *iter; @@ -254,6 +50,7 @@ void _deleteAllObjects() collector_gtsamGeneralSFMFactorCal3Bundler.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -266,10 +63,8 @@ void _special_cases_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_special_cases_rttiRegistry_created"); if(!alreadyCreated) { std::map types; - types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); - types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); - types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix")); - types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) @@ -277,18 +72,21 @@ void _special_cases_RTTIRegister() { typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/fixtures/part1.i b/wrap/tests/fixtures/part1.i new file mode 100644 index 0000000000..b69850baff --- /dev/null +++ b/wrap/tests/fixtures/part1.i @@ -0,0 +1,11 @@ +// First file to test for multi-file support. + +namespace gtsam { +class Class1 { + Class1(); +}; + +class Class2 { + Class2(); +}; +} // namespace gtsam \ No newline at end of file diff --git a/wrap/tests/fixtures/part2.i b/wrap/tests/fixtures/part2.i new file mode 100644 index 0000000000..29ad86a7f8 --- /dev/null +++ b/wrap/tests/fixtures/part2.i @@ -0,0 +1,7 @@ +// Second file to test for multi-file support. + +namespace gtsam { +class ClassA { + ClassA(); +}; +} // namespace gtsam \ No newline at end of file diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py index b321c4e151..fad4de16a6 100644 --- a/wrap/tests/test_matlab_wrapper.py +++ b/wrap/tests/test_matlab_wrapper.py @@ -22,73 +22,31 @@ class TestWrap(unittest.TestCase): """ Test the Matlab wrapper """ - TEST_DIR = osp.dirname(osp.realpath(__file__)) - INTERFACE_DIR = osp.join(TEST_DIR, "fixtures") - MATLAB_TEST_DIR = osp.join(TEST_DIR, "expected", "matlab") - MATLAB_ACTUAL_DIR = osp.join(TEST_DIR, "actual", "matlab") - - # Create the `actual/matlab` directory - os.makedirs(MATLAB_ACTUAL_DIR, exist_ok=True) - - # set the log level to INFO by default - logger.remove() # remove the default sink - logger.add(sys.stderr, format="{time} {level} {message}", level="INFO") - - def generate_content(self, cc_content, path=MATLAB_ACTUAL_DIR): - """Generate files and folders from matlab wrapper content. - - Keyword arguments: - cc_content -- the content to generate formatted as - (file_name, file_content) or - (folder_name, [(file_name, file_content)]) - path -- the path to the files parent folder within the main folder - """ - for c in cc_content: - if isinstance(c, list): - if len(c) == 0: - continue - logger.debug("c object: {}".format(c[0][0])) - path_to_folder = osp.join(path, c[0][0]) - - if not osp.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - - for sub_content in c: - logger.debug("sub object: {}".format(sub_content[1][0][0])) - self.generate_content(sub_content[1], path_to_folder) - - elif isinstance(c[1], list): - path_to_folder = osp.join(path, c[0]) - - logger.debug( - "[generate_content_global]: {}".format(path_to_folder)) - if not osp.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - for sub_content in c[1]: - path_to_file = osp.join(path_to_folder, sub_content[0]) - logger.debug( - "[generate_global_method]: {}".format(path_to_file)) - with open(path_to_file, 'w') as f: - f.write(sub_content[1]) - - else: - path_to_file = osp.join(path, c[0]) - - logger.debug("[generate_content]: {}".format(path_to_file)) - if not osp.isdir(path_to_file): - try: - os.mkdir(path) - except OSError: - pass - - with open(path_to_file, 'w') as f: - f.write(c[1]) + def setUp(self) -> None: + super().setUp() + + # Set up all the directories + self.TEST_DIR = osp.dirname(osp.realpath(__file__)) + self.INTERFACE_DIR = osp.join(self.TEST_DIR, "fixtures") + self.MATLAB_TEST_DIR = osp.join(self.TEST_DIR, "expected", "matlab") + self.MATLAB_ACTUAL_DIR = osp.join(self.TEST_DIR, "actual", "matlab") + + if not osp.exists(self.MATLAB_ACTUAL_DIR): + os.mkdir(self.MATLAB_ACTUAL_DIR) + + # Generate the matlab.h file if it does not exist + template_file = osp.join(self.TEST_DIR, "..", "gtwrap", + "matlab_wrapper", "matlab_wrapper.tpl") + if not osp.exists(template_file): + with open(template_file, 'w') as tpl: + tpl.write("#include \n#include \n") + + # Create the `actual/matlab` directory + os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True) + + # set the log level to INFO by default + logger.remove() # remove the default sink + logger.add(sys.stderr, format="{time} {level} {message}", level="INFO") def compare_and_diff(self, file): """ @@ -109,11 +67,7 @@ def test_geometry(self): python3 wrap/matlab_wrapper.py --src wrap/tests/geometry.h --module_name geometry --out wrap/tests/actual-matlab """ - with open(osp.join(self.INTERFACE_DIR, 'geometry.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'geometry.i') # Create MATLAB wrapper instance wrapper = MatlabWrapper( @@ -122,24 +76,18 @@ def test_geometry(self): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) - self.generate_content(cc_content) + files = ['+gtsam/Point2.m', '+gtsam/Point3.m', 'geometry_wrapper.cpp'] self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam'))) - files = ['+gtsam/Point2.m', '+gtsam/Point3.m', 'geometry_wrapper.cpp'] - for file in files: self.compare_and_diff(file) def test_functions(self): """Test interface file with function info.""" - with open(osp.join(self.INTERFACE_DIR, 'functions.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'functions.i') wrapper = MatlabWrapper( module_name='functions', @@ -147,9 +95,7 @@ def test_functions(self): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'functions_wrapper.cpp', 'aGlobalFunction.m', 'load2D.m', @@ -163,11 +109,7 @@ def test_functions(self): def test_class(self): """Test interface file with only class info.""" - with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'class.i') wrapper = MatlabWrapper( module_name='class', @@ -175,9 +117,7 @@ def test_class(self): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'class_wrapper.cpp', 'FunDouble.m', 'FunRange.m', @@ -191,21 +131,14 @@ def test_class(self): def test_inheritance(self): """Test interface file with class inheritance definitions.""" - with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'inheritance.i') wrapper = MatlabWrapper( module_name='inheritance', top_module_namespace=['gtsam'], ignore_classes=[''], ) - - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'inheritance_wrapper.cpp', 'MyBase.m', 'MyTemplateMatrix.m', @@ -219,11 +152,7 @@ def test_namespaces(self): """ Test interface file with full namespace definition. """ - with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'namespaces.i') wrapper = MatlabWrapper( module_name='namespaces', @@ -231,9 +160,7 @@ def test_namespaces(self): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'namespaces_wrapper.cpp', '+ns1/aGlobalFunction.m', @@ -249,21 +176,14 @@ def test_special_cases(self): """ Tests for some unique, non-trivial features. """ - with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'special_cases.i') wrapper = MatlabWrapper( module_name='special_cases', top_module_namespace=['gtsam'], ignore_classes=[''], ) - - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'special_cases_wrapper.cpp', @@ -274,6 +194,31 @@ def test_special_cases(self): for file in files: self.compare_and_diff(file) + def test_multiple_files(self): + """ + Test for when multiple interface files are specified. + """ + file1 = osp.join(self.INTERFACE_DIR, 'part1.i') + file2 = osp.join(self.INTERFACE_DIR, 'part2.i') + + wrapper = MatlabWrapper( + module_name='multiple_files', + top_module_namespace=['gtsam'], + ignore_classes=[''], + ) + + wrapper.wrap([file1, file2], path=self.MATLAB_ACTUAL_DIR) + + files = [ + 'multiple_files_wrapper.cpp', + '+gtsam/Class1.m', + '+gtsam/Class2.m', + '+gtsam/ClassA.m', + ] + + for file in files: + self.compare_and_diff(file) + if __name__ == '__main__': unittest.main() diff --git a/wrap/tests/test_pybind_wrapper.py b/wrap/tests/test_pybind_wrapper.py index 77c884b622..67c637d146 100644 --- a/wrap/tests/test_pybind_wrapper.py +++ b/wrap/tests/test_pybind_wrapper.py @@ -31,9 +31,9 @@ class TestWrap(unittest.TestCase): # Create the `actual/python` directory os.makedirs(PYTHON_ACTUAL_DIR, exist_ok=True) - def wrap_content(self, content, module_name, output_dir): + def wrap_content(self, sources, module_name, output_dir): """ - Common function to wrap content. + Common function to wrap content in `sources`. """ with open(osp.join(self.TEST_DIR, "pybind_wrapper.tpl")) as template_file: @@ -46,15 +46,12 @@ def wrap_content(self, content, module_name, output_dir): ignore_classes=[''], module_template=module_template) - cc_content = wrapper.wrap(content) - output = osp.join(self.TEST_DIR, output_dir, module_name + ".cpp") if not osp.exists(osp.join(self.TEST_DIR, output_dir)): os.mkdir(osp.join(self.TEST_DIR, output_dir)) - with open(output, 'w') as f: - f.write(cc_content) + wrapper.wrap(sources, output) return output @@ -76,39 +73,32 @@ def test_geometry(self): python3 ../pybind_wrapper.py --src geometry.h --module_name geometry_py --out output/geometry_py.cc """ - with open(osp.join(self.INTERFACE_DIR, 'geometry.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'geometry_py', + source = osp.join(self.INTERFACE_DIR, 'geometry.i') + output = self.wrap_content([source], 'geometry_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('geometry_pybind.cpp', output) def test_functions(self): """Test interface file with function info.""" - with open(osp.join(self.INTERFACE_DIR, 'functions.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'functions_py', + source = osp.join(self.INTERFACE_DIR, 'functions.i') + output = self.wrap_content([source], 'functions_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('functions_pybind.cpp', output) def test_class(self): """Test interface file with only class info.""" - with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'class_py', self.PYTHON_ACTUAL_DIR) + source = osp.join(self.INTERFACE_DIR, 'class.i') + output = self.wrap_content([source], 'class_py', + self.PYTHON_ACTUAL_DIR) self.compare_and_diff('class_pybind.cpp', output) def test_inheritance(self): """Test interface file with class inheritance definitions.""" - with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'inheritance_py', + source = osp.join(self.INTERFACE_DIR, 'inheritance.i') + output = self.wrap_content([source], 'inheritance_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('inheritance_pybind.cpp', output) @@ -119,10 +109,8 @@ def test_namespaces(self): python3 ../pybind_wrapper.py --src namespaces.i --module_name namespaces_py --out output/namespaces_py.cpp """ - with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'namespaces_py', + source = osp.join(self.INTERFACE_DIR, 'namespaces.i') + output = self.wrap_content([source], 'namespaces_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('namespaces_pybind.cpp', output) @@ -131,10 +119,8 @@ def test_operator_overload(self): """ Tests for operator overloading. """ - with open(osp.join(self.INTERFACE_DIR, 'operator.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'operator_py', + source = osp.join(self.INTERFACE_DIR, 'operator.i') + output = self.wrap_content([source], 'operator_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('operator_pybind.cpp', output) @@ -143,10 +129,8 @@ def test_special_cases(self): """ Tests for some unique, non-trivial features. """ - with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'special_cases_py', + source = osp.join(self.INTERFACE_DIR, 'special_cases.i') + output = self.wrap_content([source], 'special_cases_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('special_cases_pybind.cpp', output) @@ -155,10 +139,8 @@ def test_enum(self): """ Test if enum generation is correct. """ - with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR) + source = osp.join(self.INTERFACE_DIR, 'enum.i') + output = self.wrap_content([source], 'enum_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('enum_pybind.cpp', output)