From 9bafebb5218f40ec40dfbaa2b0f7576fe27ec683 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:09:59 -0700 Subject: [PATCH 01/12] break interface file into multiple files --- gtsam/base/base.i | 113 ++ gtsam/geometry/geometry.i | 942 +++++++++ gtsam/gtsam.i | 3520 +-------------------------------- gtsam/linear/linear.i | 653 ++++++ gtsam/navigation/navigation.i | 355 ++++ gtsam/nonlinear/nonlinear.i | 769 +++++++ gtsam/sam/sam.i | 96 + gtsam/sfm/sfm.i | 209 ++ gtsam/slam/slam.i | 325 +++ gtsam/symbolic/symbolic.i | 201 ++ 10 files changed, 3709 insertions(+), 3474 deletions(-) create mode 100644 gtsam/base/base.i create mode 100644 gtsam/geometry/geometry.i create mode 100644 gtsam/linear/linear.i create mode 100644 gtsam/navigation/navigation.i create mode 100644 gtsam/nonlinear/nonlinear.i create mode 100644 gtsam/sam/sam.i create mode 100644 gtsam/sfm/sfm.i create mode 100644 gtsam/slam/slam.i create mode 100644 gtsam/symbolic/symbolic.i diff --git a/gtsam/base/base.i b/gtsam/base/base.i new file mode 100644 index 0000000000..09278ff5b7 --- /dev/null +++ b/gtsam/base/base.i @@ -0,0 +1,113 @@ +//************************************************************************* +// base +//************************************************************************* + +namespace gtsam { + +#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/geometry/geometry.i b/gtsam/geometry/geometry.i new file mode 100644 index 0000000000..a39c227973 --- /dev/null +++ b/gtsam/geometry/geometry.i @@ -0,0 +1,942 @@ +//************************************************************************* +// 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); + 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; + + // 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; + +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); + +#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/gtsam.i b/gtsam/gtsam.i index 049a0110c6..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,3483 +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; - - // 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; - -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); - -//************************************************************************* -// 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 , - 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::EssentialMatrix& essential_matrix); - void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_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::EssentialMatrix& essential_matrix); - void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_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::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 , - 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 }> -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; - - -#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, gtsam::Point3> GeneralSFMFactorCal3Bundler; - -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); - - // 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..e22ac5954b --- /dev/null +++ b/gtsam/nonlinear/nonlinear.i @@ -0,0 +1,769 @@ +//************************************************************************* +// nonlinear +//************************************************************************* + +namespace gtsam { + +#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::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::EssentialMatrix& essential_matrix); + void insert(size_t j, + const gtsam::PinholeCamera& simple_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::EssentialMatrix& essential_matrix); + void update(size_t j, + const gtsam::PinholeCamera& simple_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::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, 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::imuBias::ConstantBias, + gtsam::PinholeCamera}> +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::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/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..c86de8d88e --- /dev/null +++ b/gtsam/sfm/sfm.i @@ -0,0 +1,209 @@ +//************************************************************************* +// 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); + + // 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..457e907b92 --- /dev/null +++ b/gtsam/slam/slam.i @@ -0,0 +1,325 @@ +//************************************************************************* +// 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; + +#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; + +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 From f33e6a801f5cb9a316dc29b0c96e6ac3f605d16c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:05 -0700 Subject: [PATCH 02/12] break up preamble and specializations so there are no duplicate includes --- python/gtsam/preamble.h | 30 ------------------- python/gtsam/preamble/base.h | 16 +++++++++++ python/gtsam/preamble/geometry.h | 21 ++++++++++++++ python/gtsam/preamble/gtsam.h | 17 +++++++++++ python/gtsam/preamble/linear.h | 12 ++++++++ python/gtsam/preamble/navigation.h | 18 ++++++++++++ python/gtsam/preamble/nonlinear.h | 12 ++++++++ python/gtsam/preamble/sam.h | 12 ++++++++ python/gtsam/preamble/sfm.h | 12 ++++++++ python/gtsam/preamble/slam.h | 17 +++++++++++ python/gtsam/preamble/symbolic.h | 12 ++++++++ python/gtsam/specializations.h | 35 ----------------------- python/gtsam/specializations/base.h | 17 +++++++++++ python/gtsam/specializations/geometry.h | 23 +++++++++++++++ python/gtsam/specializations/gtsam.h | 20 +++++++++++++ python/gtsam/specializations/linear.h | 12 ++++++++ python/gtsam/specializations/navigation.h | 12 ++++++++ python/gtsam/specializations/nonlinear.h | 12 ++++++++ python/gtsam/specializations/sam.h | 12 ++++++++ python/gtsam/specializations/sfm.h | 16 +++++++++++ python/gtsam/specializations/slam.h | 19 ++++++++++++ python/gtsam/specializations/symbolic.h | 12 ++++++++ 22 files changed, 304 insertions(+), 65 deletions(-) delete mode 100644 python/gtsam/preamble.h create mode 100644 python/gtsam/preamble/base.h create mode 100644 python/gtsam/preamble/geometry.h create mode 100644 python/gtsam/preamble/gtsam.h create mode 100644 python/gtsam/preamble/linear.h create mode 100644 python/gtsam/preamble/navigation.h create mode 100644 python/gtsam/preamble/nonlinear.h create mode 100644 python/gtsam/preamble/sam.h create mode 100644 python/gtsam/preamble/sfm.h create mode 100644 python/gtsam/preamble/slam.h create mode 100644 python/gtsam/preamble/symbolic.h delete mode 100644 python/gtsam/specializations.h create mode 100644 python/gtsam/specializations/base.h create mode 100644 python/gtsam/specializations/geometry.h create mode 100644 python/gtsam/specializations/gtsam.h create mode 100644 python/gtsam/specializations/linear.h create mode 100644 python/gtsam/specializations/navigation.h create mode 100644 python/gtsam/specializations/nonlinear.h create mode 100644 python/gtsam/specializations/sam.h create mode 100644 python/gtsam/specializations/sfm.h create mode 100644 python/gtsam/specializations/slam.h create mode 100644 python/gtsam/specializations/symbolic.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 be8eb8a6cf..0000000000 --- a/python/gtsam/specializations.h +++ /dev/null @@ -1,35 +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_, "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..3d22581d98 --- /dev/null +++ b/python/gtsam/specializations/geometry.h @@ -0,0 +1,23 @@ +/* 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"); 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 From 54063934fab2d041cd54b60c2c0ba7ac624290b6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:06 -0700 Subject: [PATCH 03/12] update template for wrapper --- python/gtsam/gtsam.tpl | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) 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" }} From 86c47d52d571d427323288197a5b8a07209cbb7a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:08 -0700 Subject: [PATCH 04/12] move RedirectCout to base/utilities.h --- gtsam/base/utilities.h | 29 +++++++++++++++++++++++++++++ gtsam/nonlinear/utilities.h | 25 ------------------------- 2 files changed, 29 insertions(+), 25 deletions(-) create mode 100644 gtsam/base/utilities.h 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/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_; -}; - } From e8e3094556f2c28e55b03d3438c30fa36c37b8c4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:09 -0700 Subject: [PATCH 05/12] update CMake --- python/CMakeLists.txt | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 5f51368e6a..676479bd55 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 From fe95b8b9709db56d67790a294270be8a0f27e001 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:09 -0700 Subject: [PATCH 06/12] wrapper updates --- wrap/cmake/PybindWrap.cmake | 157 +++++++++++----------- wrap/gtwrap/pybind_wrapper.py | 75 +++++++++-- wrap/scripts/pybind_wrap.py | 11 +- wrap/templates/pybind_wrapper.tpl.example | 8 +- 4 files changed, 157 insertions(+), 94 deletions(-) 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/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/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" From 0989aed0cfa2bfbec198dfa25d7bc7abcb6d9f7f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:10 -0700 Subject: [PATCH 07/12] enable CI builds --- .github/workflows/build-python.yml | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) 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 From 17842dcea7ed5ae377a5b9f57fdfb45a3a2b0291 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:11 -0700 Subject: [PATCH 08/12] fixes --- gtsam/geometry/geometry.i | 1 + python/gtsam_unstable/gtsam_unstable.tpl | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index a39c227973..6217d9e80a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -459,6 +459,7 @@ class Pose3 { // 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; 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} From 4c410fcd0ebe1869400f6ea601e21792da74f019 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:35 -0700 Subject: [PATCH 09/12] Squashed 'wrap/' changes from 07330d100..d9ae5ce03 d9ae5ce03 Merge pull request #118 from borglab/feature/matlab-multi-files 9adddf7dd update the main script for matlab wrapping 0b0398d46 remove debug statements since they aren't needed for now df064a364 support for parsing mutiple interface files for Matlab wrapping 1929e197c add test for parsing multiple interface files bac442056 Merge pull request #117 from borglab/fix/matlab-refactor 331f4a8ce update tests to remove redundant code 5426e3af4 generate all content from within the wrap function f78612bf9 make directory check common b7acd7a1f fixed import and test setup 88007b153 Merge pull request #116 from borglab/feature/matlab-refactor a074896e6 utils -> mixins 414557e00 structure 187100439 update gitignore adbc55aea don't use class attributes in matlab wrapper f45ba5b2d broke down some large functions into smaller ones 7756f0548 add mixin for checks and call method to wrap global functions a318e2a67 Merge pull request #115 from borglab/feature/multiple-modules b02b74c3d convert matlab_wrapper to a submodule be8641e83 improved function naming in tests 02ddbfbb0 update tests and docs dfbded2c7 small fixes e9ec5af07 update docs d124e2cfb wrap multiple files 7c7342f86 update cmake to take in new changes for multiple modules 54850f724 Merge pull request #114 from borglab/fix/remove-py35 71ee98321 add mypy annotations ccaea6294 remove support for python 3.5 git-subtree-dir: wrap git-subtree-split: d9ae5ce036c4315db3c28b12db9c73eae246f314 --- .github/workflows/linux-ci.yml | 2 +- .github/workflows/macos-ci.yml | 2 +- .gitignore | 2 +- CMakeLists.txt | 2 +- DOCS.md | 8 +- README.md | 4 +- cmake/PybindWrap.cmake | 157 ++-- gtwrap/interface_parser/__init__.py | 2 +- gtwrap/interface_parser/classes.py | 12 +- gtwrap/interface_parser/declaration.py | 2 +- gtwrap/interface_parser/enum.py | 2 +- gtwrap/interface_parser/function.py | 12 +- gtwrap/interface_parser/module.py | 3 +- gtwrap/interface_parser/namespace.py | 6 +- gtwrap/interface_parser/template.py | 4 +- gtwrap/interface_parser/tokens.py | 6 +- gtwrap/interface_parser/type.py | 5 +- gtwrap/interface_parser/variable.py | 6 +- gtwrap/matlab_wrapper/__init__.py | 3 + gtwrap/matlab_wrapper/mixins.py | 222 +++++ gtwrap/matlab_wrapper/templates.py | 166 ++++ .../wrapper.py} | 843 ++++++------------ gtwrap/pybind_wrapper.py | 75 +- gtwrap/template_instantiator.py | 23 +- scripts/matlab_wrap.py | 40 +- scripts/pybind_wrap.py | 11 +- templates/pybind_wrapper.tpl.example | 8 +- tests/expected/matlab/+gtsam/Class1.m | 36 + tests/expected/matlab/+gtsam/Class2.m | 36 + tests/expected/matlab/+gtsam/ClassA.m | 36 + tests/expected/matlab/class_wrapper.cpp | 17 +- tests/expected/matlab/functions_wrapper.cpp | 103 +-- tests/expected/matlab/geometry_wrapper.cpp | 103 +-- tests/expected/matlab/inheritance_wrapper.cpp | 221 ++--- .../matlab/multiple_files_wrapper.cpp | 229 +++++ tests/expected/matlab/namespaces_wrapper.cpp | 165 +--- .../expected/matlab/special_cases_wrapper.cpp | 226 +---- tests/fixtures/part1.i | 11 + tests/fixtures/part2.i | 7 + tests/test_matlab_wrapper.py | 181 ++-- tests/test_pybind_wrapper.py | 58 +- 41 files changed, 1424 insertions(+), 1633 deletions(-) create mode 100644 gtwrap/matlab_wrapper/__init__.py create mode 100644 gtwrap/matlab_wrapper/mixins.py create mode 100644 gtwrap/matlab_wrapper/templates.py rename gtwrap/{matlab_wrapper.py => matlab_wrapper/wrapper.py} (68%) create mode 100644 tests/expected/matlab/+gtsam/Class1.m create mode 100644 tests/expected/matlab/+gtsam/Class2.m create mode 100644 tests/expected/matlab/+gtsam/ClassA.m create mode 100644 tests/expected/matlab/multiple_files_wrapper.cpp create mode 100644 tests/fixtures/part1.i create mode 100644 tests/fixtures/part2.i diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index 0ca9ba8f5b..34623385ed 100644 --- a/.github/workflows/linux-ci.yml +++ b/.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/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml index b0ccb3fbe9..3910d28d8a 100644 --- a/.github/workflows/macos-ci.yml +++ b/.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/.gitignore b/.gitignore index 8e2bafa7a0..9f79deafab 100644 --- a/.gitignore +++ b/.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/CMakeLists.txt b/CMakeLists.txt index 9e03da0607..2a11a760d4 100644 --- a/CMakeLists.txt +++ b/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/DOCS.md b/DOCS.md index 8537ddd276..c8285baeff 100644 --- a/DOCS.md +++ b/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/README.md b/README.md index 442fc2f934..a04a2ef2d0 100644 --- a/README.md +++ b/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/cmake/PybindWrap.cmake b/cmake/PybindWrap.cmake index 331dfff8c4..f341c2f989 100644 --- a/cmake/PybindWrap.cmake +++ b/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/gtwrap/interface_parser/__init__.py b/gtwrap/interface_parser/__init__.py index 0f87eaaa9d..3be52d7d9f 100644 --- a/gtwrap/interface_parser/__init__.py +++ b/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/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index ea7a3b3c38..3e6a0fc3c7 100644 --- a/gtwrap/interface_parser/classes.py +++ b/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/gtwrap/interface_parser/declaration.py b/gtwrap/interface_parser/declaration.py index 292d6aeaa6..f47ee6e057 100644 --- a/gtwrap/interface_parser/declaration.py +++ b/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/gtwrap/interface_parser/enum.py b/gtwrap/interface_parser/enum.py index fca7080ef2..265e1ad612 100644 --- a/gtwrap/interface_parser/enum.py +++ b/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/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py index 3b9a5d4ada..995aba10e1 100644 --- a/gtwrap/interface_parser/function.py +++ b/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/gtwrap/interface_parser/module.py b/gtwrap/interface_parser/module.py index 6412098b8a..7912c40d5b 100644 --- a/gtwrap/interface_parser/module.py +++ b/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/gtwrap/interface_parser/namespace.py b/gtwrap/interface_parser/namespace.py index 575d982371..9c135ffe8c 100644 --- a/gtwrap/interface_parser/namespace.py +++ b/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/gtwrap/interface_parser/template.py b/gtwrap/interface_parser/template.py index dc9d0ce44f..fd9de830ae 100644 --- a/gtwrap/interface_parser/template.py +++ b/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/gtwrap/interface_parser/tokens.py b/gtwrap/interface_parser/tokens.py index 4eba95900a..0f8d38d868 100644 --- a/gtwrap/interface_parser/tokens.py +++ b/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/gtwrap/interface_parser/type.py b/gtwrap/interface_parser/type.py index b9f2bd8f74..0b9be65017 100644 --- a/gtwrap/interface_parser/type.py +++ b/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/gtwrap/interface_parser/variable.py b/gtwrap/interface_parser/variable.py index fcb02666f7..3779cf74fa 100644 --- a/gtwrap/interface_parser/variable.py +++ b/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/gtwrap/matlab_wrapper/__init__.py b/gtwrap/matlab_wrapper/__init__.py new file mode 100644 index 0000000000..f10338c1c7 --- /dev/null +++ b/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/gtwrap/matlab_wrapper/mixins.py b/gtwrap/matlab_wrapper/mixins.py new file mode 100644 index 0000000000..061cea2833 --- /dev/null +++ b/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/gtwrap/matlab_wrapper/templates.py b/gtwrap/matlab_wrapper/templates.py new file mode 100644 index 0000000000..7aaf8f487b --- /dev/null +++ b/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/gtwrap/matlab_wrapper.py b/gtwrap/matlab_wrapper/wrapper.py similarity index 68% rename from gtwrap/matlab_wrapper.py rename to gtwrap/matlab_wrapper/wrapper.py index de6221bbcf..b040d27311 100755 --- a/gtwrap/matlab_wrapper.py +++ b/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/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index 0e1b3c7eaa..40571263aa 100755 --- a/gtwrap/pybind_wrapper.py +++ b/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/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index c474246489..87729cfa6d 100644 --- a/gtwrap/template_instantiator.py +++ b/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/scripts/matlab_wrap.py b/scripts/matlab_wrap.py index be6043947f..0f6664a635 100644 --- a/scripts/matlab_wrap.py +++ b/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/scripts/pybind_wrap.py b/scripts/pybind_wrap.py index 7f2f8d4192..c82a1d24c0 100644 --- a/scripts/pybind_wrap.py +++ b/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/templates/pybind_wrapper.tpl.example b/templates/pybind_wrapper.tpl.example index bf5b334900..485aa8d00c 100644 --- a/templates/pybind_wrapper.tpl.example +++ b/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/tests/expected/matlab/+gtsam/Class1.m b/tests/expected/matlab/+gtsam/Class1.m new file mode 100644 index 0000000000..00dd5ca746 --- /dev/null +++ b/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/tests/expected/matlab/+gtsam/Class2.m b/tests/expected/matlab/+gtsam/Class2.m new file mode 100644 index 0000000000..93279e1560 --- /dev/null +++ b/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/tests/expected/matlab/+gtsam/ClassA.m b/tests/expected/matlab/+gtsam/ClassA.m new file mode 100644 index 0000000000..3210e93c60 --- /dev/null +++ b/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/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp index e644ac00f2..fab9c14506 100644 --- a/tests/expected/matlab/class_wrapper.cpp +++ b/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/tests/expected/matlab/functions_wrapper.cpp b/tests/expected/matlab/functions_wrapper.cpp index ae7f49c410..d0f0f8ca67 100644 --- a/tests/expected/matlab/functions_wrapper.cpp +++ b/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/tests/expected/matlab/geometry_wrapper.cpp b/tests/expected/matlab/geometry_wrapper.cpp index 4d8a7c7893..81631390c9 100644 --- a/tests/expected/matlab/geometry_wrapper.cpp +++ b/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/tests/expected/matlab/inheritance_wrapper.cpp b/tests/expected/matlab/inheritance_wrapper.cpp index 077df48305..8e61ac8c61 100644 --- a/tests/expected/matlab/inheritance_wrapper.cpp +++ b/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/tests/expected/matlab/multiple_files_wrapper.cpp b/tests/expected/matlab/multiple_files_wrapper.cpp new file mode 100644 index 0000000000..66ab7ff73d --- /dev/null +++ b/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/tests/expected/matlab/namespaces_wrapper.cpp b/tests/expected/matlab/namespaces_wrapper.cpp index 8f6e415e2d..604ede5da5 100644 --- a/tests/expected/matlab/namespaces_wrapper.cpp +++ b/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/tests/expected/matlab/special_cases_wrapper.cpp b/tests/expected/matlab/special_cases_wrapper.cpp index 056ce80973..69abbf73be 100644 --- a/tests/expected/matlab/special_cases_wrapper.cpp +++ b/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/tests/fixtures/part1.i b/tests/fixtures/part1.i new file mode 100644 index 0000000000..b69850baff --- /dev/null +++ b/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/tests/fixtures/part2.i b/tests/fixtures/part2.i new file mode 100644 index 0000000000..29ad86a7f8 --- /dev/null +++ b/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/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index b321c4e151..fad4de16a6 100644 --- a/tests/test_matlab_wrapper.py +++ b/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/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py index 77c884b622..67c637d146 100644 --- a/tests/test_pybind_wrapper.py +++ b/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) From 36c2aa1e56a11b4a99acf855258413d0230b08aa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 12 Jul 2021 11:46:31 -0400 Subject: [PATCH 10/12] matlab wrapper header update --- wrap/matlab.h | 1 + 1 file changed, 1 insertion(+) 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; From 6919ad927729385ba6583b7987099a628e708db8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 14 Jul 2021 21:53:20 -0400 Subject: [PATCH 11/12] update interface files with latest develop --- gtsam/base/base.i | 6 +- gtsam/geometry/SimpleCamera.h | 2 + gtsam/geometry/geometry.i | 58 ++++++++++++++++++ gtsam/geometry/triangulation.h | 10 ++- gtsam/linear/Coreset.h | 81 +++++++++++++++++++++++++ gtsam/nonlinear/nonlinear.i | 57 +++++++++++++---- gtsam/sfm/sfm.i | 2 + gtsam/slam/slam.i | 15 ++++- python/CMakeLists.txt | 2 + python/gtsam/specializations/geometry.h | 4 ++ 10 files changed, 220 insertions(+), 17 deletions(-) create mode 100644 gtsam/linear/Coreset.h diff --git a/gtsam/base/base.i b/gtsam/base/base.i index 09278ff5b7..c24b04088a 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -6,6 +6,8 @@ namespace gtsam { #include #include +#include +#include #include #include #include @@ -104,8 +106,8 @@ virtual class Value { template + gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix, + gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}> virtual class GenericValue : gtsam::Value { void serializable() const; }; diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c77..27b8bb549d 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,7 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 6217d9e80a..4e56347d37 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -684,6 +684,57 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { 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; @@ -860,6 +911,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; template class CameraSet { @@ -924,6 +976,12 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, 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 diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22e..6f6ade6f70 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,14 +18,16 @@ #pragma once -#include #include +#include +#include +#include #include #include #include -#include -#include #include +#include +#include namespace gtsam { @@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3Fisheye = CameraSet>; +using CameraSetCal3Unified = CameraSet>; } // \namespace gtsam diff --git a/gtsam/linear/Coreset.h b/gtsam/linear/Coreset.h new file mode 100644 index 0000000000..f622d9c81c --- /dev/null +++ b/gtsam/linear/Coreset.h @@ -0,0 +1,81 @@ +#include + +#include + +namespace gtsam { + +Vector FastCaratheodory(Matrix& P, Vector& weights, size_t coreset_size) { + size_t n = P.rows(), d = P.cols(); + size_t m = 2 * d + 2; + + if (n < d + 1) { + return weights; + } + + Vector weights = weights / weights.sum(); + + size_t chunk_size = ceil(n / m); + size_t current_m = ceil(n / chunk_size); + + size_t add_z = chunk_size - size_t(n % chunk_size); + Matrix u(weights.size(), 1); + u.col(0) = weights; + + if (add_z != chunk_size) { + Matrix zeros = Matrix::Zero(add_z, d); + Matrix P_new = Matrix(P.rows() + zeros.rows(), P.cols() + zeros.cols()); + P_new << P, zeros; + zeros = Matrix::Zero(add_z, u.cols()); + Matrix u_new(u.rows() + zeros.rows(), u.cols() + zeros.cols()); + u_new << u, zeros; + } + + Vector idxarray = Vector::LinSpaced(n, 0, n - 1); + Eigen::Tensor p_groups; + + // p_groups = P.reshape(current_m, chunk_size, P.shape[1]) + // u_groups = u.reshape(current_m, chunk_size) + // idx_group = idxarray.reshape(current_m, chunk_size) + // u_nonzero = np.count_nonzero(u) + + // if not coreset_size: + // coreset_size = d+1 + // while u_nonzero > coreset_size: + + // groups_means = np.einsum('ijk,ij->ik',p_groups, u_groups) + // group_weigts = np.ones(groups_means.shape[0], dtype = + // dtype)*1/current_m + + // Cara_u_idx = Caratheodory(groups_means , group_weigts,dtype = dtype ) + + // IDX = np.nonzero(Cara_u_idx) + + // new_P = p_groups[IDX].reshape(-1,d) + + // subset_u = (current_m * u_groups[IDX] * Cara_u_idx[IDX][:, + // np.newaxis]).reshape(-1, 1) new_idx_array = + // idx_group[IDX].reshape(-1,1) + // ##############################################################################3 + // u_nonzero = np.count_nonzero(subset_u) + // chunk_size = math.ceil(new_P.shape[0]/ m) + // current_m = math.ceil(new_P.shape[0]/ chunk_size) + + // add_z = chunk_size - int(new_P.shape[0] % chunk_size) + // if add_z != chunk_size: + // new_P = np.concatenate((new_P, np.zeros((add_z, new_P.shape[1]), + // dtype = dtype))) subset_u = np.concatenate((subset_u, + // np.zeros((add_z, subset_u.shape[1]),dtype = dtype))) + // new_idx_array = np.concatenate((new_idx_array, np.zeros((add_z, + // new_idx_array.shape[1]),dtype = dtype))) + // p_groups = new_P.reshape(current_m, chunk_size, new_P.shape[1]) + // u_groups = subset_u.reshape(current_m, chunk_size) + // idx_group = new_idx_array.reshape(current_m , chunk_size) + // ########################################################### + + // new_u = np.zeros(n) + // subset_u = subset_u[(new_idx_array < n)] + // new_idx_array = new_idx_array[(new_idx_array < + // n)].reshape(-1).astype(int) new_u[new_idx_array] = subset_u return u_sum + // * new_u +} +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index e22ac5954b..8071e8bc79 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -6,6 +6,8 @@ namespace gtsam { #include #include +#include +#include #include #include #include @@ -151,11 +153,25 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template , + template , gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -291,10 +307,13 @@ class Values { 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& simple_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::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); @@ -312,10 +331,13 @@ class Values { 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& simple_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::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); @@ -335,9 +357,13 @@ class Values { gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, + gtsam::Cal3Fisheye, + gtsam::Cal3Unified, gtsam::EssentialMatrix, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, @@ -681,7 +707,9 @@ class ISAM2 { gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCamera, - gtsam::PinholeCamera, Vector, Matrix}> + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -734,10 +762,14 @@ template , - gtsam::imuBias::ConstantBias, - gtsam::PinholeCamera}> + 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); @@ -755,6 +787,9 @@ template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias}> virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index c86de8d88e..705892e60f 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -87,6 +87,8 @@ 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; diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 457e907b92..1c04fd14c0 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -62,6 +62,12 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Fisheye; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Unified; #include template @@ -80,8 +86,15 @@ typedef gtsam::GeneralSFMFactor, typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Fisheye; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Unified; -template +template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 676479bd55..7b8347da5c 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -116,6 +116,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler + gtsam::CameraSetCal3Unified + gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_unstable_py # target diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index 3d22581d98..e11d3cc4fe 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -21,3 +21,7 @@ py::bind_vector>>( m_, "CameraSetCal3_S2"); py::bind_vector>>( m_, "CameraSetCal3Bundler"); +py::bind_vector>>( + m_, "CameraSetCal3Unified"); +py::bind_vector>>( + m_, "CameraSetCal3Fisheye"); From 6db646d80057555925845d7618a0f9284e77033f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Jul 2021 00:25:40 -0400 Subject: [PATCH 12/12] remove extraneous file --- gtsam/linear/Coreset.h | 81 ------------------------------------------ 1 file changed, 81 deletions(-) delete mode 100644 gtsam/linear/Coreset.h diff --git a/gtsam/linear/Coreset.h b/gtsam/linear/Coreset.h deleted file mode 100644 index f622d9c81c..0000000000 --- a/gtsam/linear/Coreset.h +++ /dev/null @@ -1,81 +0,0 @@ -#include - -#include - -namespace gtsam { - -Vector FastCaratheodory(Matrix& P, Vector& weights, size_t coreset_size) { - size_t n = P.rows(), d = P.cols(); - size_t m = 2 * d + 2; - - if (n < d + 1) { - return weights; - } - - Vector weights = weights / weights.sum(); - - size_t chunk_size = ceil(n / m); - size_t current_m = ceil(n / chunk_size); - - size_t add_z = chunk_size - size_t(n % chunk_size); - Matrix u(weights.size(), 1); - u.col(0) = weights; - - if (add_z != chunk_size) { - Matrix zeros = Matrix::Zero(add_z, d); - Matrix P_new = Matrix(P.rows() + zeros.rows(), P.cols() + zeros.cols()); - P_new << P, zeros; - zeros = Matrix::Zero(add_z, u.cols()); - Matrix u_new(u.rows() + zeros.rows(), u.cols() + zeros.cols()); - u_new << u, zeros; - } - - Vector idxarray = Vector::LinSpaced(n, 0, n - 1); - Eigen::Tensor p_groups; - - // p_groups = P.reshape(current_m, chunk_size, P.shape[1]) - // u_groups = u.reshape(current_m, chunk_size) - // idx_group = idxarray.reshape(current_m, chunk_size) - // u_nonzero = np.count_nonzero(u) - - // if not coreset_size: - // coreset_size = d+1 - // while u_nonzero > coreset_size: - - // groups_means = np.einsum('ijk,ij->ik',p_groups, u_groups) - // group_weigts = np.ones(groups_means.shape[0], dtype = - // dtype)*1/current_m - - // Cara_u_idx = Caratheodory(groups_means , group_weigts,dtype = dtype ) - - // IDX = np.nonzero(Cara_u_idx) - - // new_P = p_groups[IDX].reshape(-1,d) - - // subset_u = (current_m * u_groups[IDX] * Cara_u_idx[IDX][:, - // np.newaxis]).reshape(-1, 1) new_idx_array = - // idx_group[IDX].reshape(-1,1) - // ##############################################################################3 - // u_nonzero = np.count_nonzero(subset_u) - // chunk_size = math.ceil(new_P.shape[0]/ m) - // current_m = math.ceil(new_P.shape[0]/ chunk_size) - - // add_z = chunk_size - int(new_P.shape[0] % chunk_size) - // if add_z != chunk_size: - // new_P = np.concatenate((new_P, np.zeros((add_z, new_P.shape[1]), - // dtype = dtype))) subset_u = np.concatenate((subset_u, - // np.zeros((add_z, subset_u.shape[1]),dtype = dtype))) - // new_idx_array = np.concatenate((new_idx_array, np.zeros((add_z, - // new_idx_array.shape[1]),dtype = dtype))) - // p_groups = new_P.reshape(current_m, chunk_size, new_P.shape[1]) - // u_groups = subset_u.reshape(current_m, chunk_size) - // idx_group = new_idx_array.reshape(current_m , chunk_size) - // ########################################################### - - // new_u = np.zeros(n) - // subset_u = subset_u[(new_idx_array < n)] - // new_idx_array = new_idx_array[(new_idx_array < - // n)].reshape(-1).astype(int) new_u[new_idx_array] = subset_u return u_sum - // * new_u -} -} // namespace gtsam \ No newline at end of file